drivers/staging/rtl8188eu/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 <osdep_service.h>
  23 #include <drv_types.h>
  24 #include <wifi.h>
  25 #include <rtw_mlme_ext.h>
  26 #include <wlan_bssdef.h>
  27 #include <mlme_osdep.h>
  28 #include <recv_osdep.h>
  29
  30 static struct mlme_handler mlme_sta_tbl[] = {
  31         {WIFI_ASSOCREQ,         "OnAssocReq",   &OnAssocReq},
  32         {WIFI_ASSOCRSP,         "OnAssocRsp",   &OnAssocRsp},
  33         {WIFI_REASSOCREQ,       "OnReAssocReq", &OnAssocReq},
  34         {WIFI_REASSOCRSP,       "OnReAssocRsp", &OnAssocRsp},
  35         {WIFI_PROBEREQ,         "OnProbeReq",   &OnProbeReq},
  36         {WIFI_PROBERSP,         "OnProbeRsp",           &OnProbeRsp},
  37
  38         /*----------------------------------------------------------
  39                                         below 2 are reserved
  40         -----------------------------------------------------------*/
  41         {0,                                     "DoReserved",           &DoReserved},
  42         {0,                                     "DoReserved",           &DoReserved},
  43         {WIFI_BEACON,           "OnBeacon",             &OnBeacon},
  44         {WIFI_ATIM,                     "OnATIM",               &OnAtim},
  45         {WIFI_DISASSOC,         "OnDisassoc",           &OnDisassoc},
  46         {WIFI_AUTH,                     "OnAuth",               &OnAuthClient},
  47         {WIFI_DEAUTH,           "OnDeAuth",             &OnDeAuth},
  48         {WIFI_ACTION,           "OnAction",             &OnAction},
  49 };
  50
  51 static struct action_handler OnAction_tbl[] = {
  52         {RTW_WLAN_CATEGORY_SPECTRUM_MGMT,        "ACTION_SPECTRUM_MGMT", on_action_spct},
  53         {RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos},
  54         {RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls},
  55         {RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
  56         {RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
  57         {RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
  58         {RTW_WLAN_CATEGORY_FT, "ACTION_FT",     &DoReserved},
  59         {RTW_WLAN_CATEGORY_HT,  "ACTION_HT",    &OnAction_ht},
  60         {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved},
  61         {RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm},
  62         {RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p},
  63 };
  64
  65
  66 static u8 null_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
  67
  68 /**************************************************
  69 OUI definitions for the vendor specific IE
  70 ***************************************************/
  71 unsigned char   RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
  72 unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
  73 unsigned char   WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
  74 unsigned char   P2P_OUI[] = {0x50, 0x6F, 0x9A, 0x09};
  75 unsigned char   WFD_OUI[] = {0x50, 0x6F, 0x9A, 0x0A};
  76
  77 unsigned char   WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
  78 unsigned char   WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
  79
  80 unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
  81 unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
  82
  83 extern unsigned char REALTEK_96B_IE[];
  84
  85 /********************************************************
  86 MCS rate definitions
  87 *********************************************************/
  88 unsigned char   MCS_rate_2R[16] = {0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
  89 unsigned char   MCS_rate_1R[16] = {0xff, 0x00, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
  90
  91 /********************************************************
  92 ChannelPlan definitions
  93 *********************************************************/
  94 static struct rt_channel_plan_2g RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
  95         {{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 */
  96         {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},              /*  0x01, RT_CHANNEL_DOMAIN_2G_ETSI1 */
  97         {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11},                      /*  0x02, RT_CHANNEL_DOMAIN_2G_FCC1 */
  98         {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14},  /*  0x03, RT_CHANNEL_DOMAIN_2G_MIKK1 */
  99         {{10, 11, 12, 13}, 4},                                  /*  0x04, RT_CHANNEL_DOMAIN_2G_ETSI2 */
 100         {{}, 0},                                                                        /*  0x05, RT_CHANNEL_DOMAIN_2G_NULL */
 101 };
 102
 103 static struct rt_channel_plan_map       RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
 104         /*  0x00 ~ 0x1F , Old Define ===== */
 105         {0x02}, /* 0x00, RT_CHANNEL_DOMAIN_FCC */
 106         {0x02}, /* 0x01, RT_CHANNEL_DOMAIN_IC */
 107         {0x01}, /* 0x02, RT_CHANNEL_DOMAIN_ETSI */
 108         {0x01}, /* 0x03, RT_CHANNEL_DOMAIN_SPAIN */
 109         {0x01}, /* 0x04, RT_CHANNEL_DOMAIN_FRANCE */
 110         {0x03}, /* 0x05, RT_CHANNEL_DOMAIN_MKK */
 111         {0x03}, /* 0x06, RT_CHANNEL_DOMAIN_MKK1 */
 112         {0x01}, /* 0x07, RT_CHANNEL_DOMAIN_ISRAEL */
 113         {0x03}, /* 0x08, RT_CHANNEL_DOMAIN_TELEC */
 114         {0x03}, /* 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN */
 115         {0x00}, /* 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13 */
 116         {0x02}, /* 0x0B, RT_CHANNEL_DOMAIN_TAIWAN */
 117         {0x01}, /* 0x0C, RT_CHANNEL_DOMAIN_CHINA */
 118         {0x02}, /* 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO */
 119         {0x02}, /* 0x0E, RT_CHANNEL_DOMAIN_KOREA */
 120         {0x02}, /* 0x0F, RT_CHANNEL_DOMAIN_TURKEY */
 121         {0x01}, /* 0x10, RT_CHANNEL_DOMAIN_JAPAN */
 122         {0x02}, /* 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS */
 123         {0x01}, /* 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
 124         {0x00}, /* 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G */
 125         {0x02}, /* 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS */
 126         {0x00}, /* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */
 127         {0x00}, /* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */
 128         {0x03}, /* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
 129         {0x05}, /* 0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS */
 130         {0x02}, /* 0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS */
 131         {0x00}, /* 0x1A, */
 132         {0x00}, /* 0x1B, */
 133         {0x00}, /* 0x1C, */
 134         {0x00}, /* 0x1D, */
 135         {0x00}, /* 0x1E, */
 136         {0x05}, /* 0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G */
 137         /*  0x20 ~ 0x7F , New Define ===== */
 138         {0x00}, /* 0x20, RT_CHANNEL_DOMAIN_WORLD_NULL */
 139         {0x01}, /* 0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL */
 140         {0x02}, /* 0x22, RT_CHANNEL_DOMAIN_FCC1_NULL */
 141         {0x03}, /* 0x23, RT_CHANNEL_DOMAIN_MKK1_NULL */
 142         {0x04}, /* 0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL */
 143         {0x02}, /* 0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1 */
 144         {0x00}, /* 0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1 */
 145         {0x03}, /* 0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1 */
 146         {0x00}, /* 0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1 */
 147         {0x00}, /* 0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2 */
 148         {0x00}, /* 0x2A, */
 149         {0x00}, /* 0x2B, */
 150         {0x00}, /* 0x2C, */
 151         {0x00}, /* 0x2D, */
 152         {0x00}, /* 0x2E, */
 153         {0x00}, /* 0x2F, */
 154         {0x00}, /* 0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3 */
 155         {0x00}, /* 0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4 */
 156         {0x00}, /* 0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5 */
 157         {0x00}, /* 0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6 */
 158         {0x02}, /* 0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7 */
 159         {0x00}, /* 0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2 */
 160         {0x00}, /* 0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3 */
 161         {0x03}, /* 0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2 */
 162         {0x03}, /* 0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3 */
 163         {0x02}, /* 0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1 */
 164         {0x00}, /* 0x3A, */
 165         {0x00}, /* 0x3B, */
 166         {0x00}, /* 0x3C, */
 167         {0x00}, /* 0x3D, */
 168         {0x00}, /* 0x3E, */
 169         {0x00}, /* 0x3F, */
 170         {0x02}, /* 0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2 */
 171         {0x03}, /* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G */
 172 };
 173
 174 static struct rt_channel_plan_map RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03}; /* use the combination for max channel numbers */
 175
 176 /*
 177  * Search the @param channel_num in given @param channel_set
 178  * @ch_set: the given channel set
 179  * @ch: the given channel number
 180  *
 181  * return the index of channel_num in channel_set, -1 if not found
 182  */
 183 int rtw_ch_set_search_ch(struct rt_channel_info *ch_set, const u32 ch)
 184 {
 185         int i;
 186         for (i = 0; ch_set[i].ChannelNum != 0; i++) {
 187                 if (ch == ch_set[i].ChannelNum)
 188                         break;
 189         }
 190
 191         if (i >= ch_set[i].ChannelNum)
 192                 return -1;
 193         return i;
 194 }
 195
 196 /****************************************************************************
 197
 198 Following are the initialization functions for WiFi MLME
 199
 200 *****************************************************************************/
 201
 202 int init_hw_mlme_ext(struct adapter *padapter)
 203 {
 204         struct  mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 205
 206         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 207         return _SUCCESS;
 208 }
 209
 210 static void init_mlme_ext_priv_value(struct adapter *padapter)
 211 {
 212         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 213         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 214         unsigned char   mixed_datarate[NumRates] = {
 215                 _1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
 216                 _9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_,
 217                  _48M_RATE_, _54M_RATE_, 0xff
 218         };
 219         unsigned char   mixed_basicrate[NumRates] = {
 220                 _1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
 221                 _12M_RATE_, _24M_RATE_, 0xff,
 222         };
 223
 224         atomic_set(&pmlmeext->event_seq, 0);
 225         pmlmeext->mgnt_seq = 0;/* reset to zero when disconnect at client mode */
 226
 227         pmlmeext->cur_channel = padapter->registrypriv.channel;
 228         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
 229         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 230         pmlmeext->oper_channel = pmlmeext->cur_channel ;
 231         pmlmeext->oper_bwmode = pmlmeext->cur_bwmode;
 232         pmlmeext->oper_ch_offset = pmlmeext->cur_ch_offset;
 233         pmlmeext->retry = 0;
 234
 235         pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
 236
 237         memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
 238         memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
 239
 240         pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
 241
 242         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
 243         pmlmeext->sitesurvey_res.channel_idx = 0;
 244         pmlmeext->sitesurvey_res.bss_cnt = 0;
 245         pmlmeext->scan_abort = false;
 246
 247         pmlmeinfo->state = WIFI_FW_NULL_STATE;
 248         pmlmeinfo->reauth_count = 0;
 249         pmlmeinfo->reassoc_count = 0;
 250         pmlmeinfo->link_count = 0;
 251         pmlmeinfo->auth_seq = 0;
 252         pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
 253         pmlmeinfo->key_index = 0;
 254         pmlmeinfo->iv = 0;
 255
 256         pmlmeinfo->enc_algo = _NO_PRIVACY_;
 257         pmlmeinfo->authModeToggle = 0;
 258
 259         _rtw_memset(pmlmeinfo->chg_txt, 0, 128);
 260
 261         pmlmeinfo->slotTime = SHORT_SLOT_TIME;
 262         pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
 263
 264         pmlmeinfo->dialogToken = 0;
 265
 266         pmlmeext->action_public_rxseq = 0xffff;
 267         pmlmeext->action_public_dialog_token = 0xff;
 268 }
 269
 270 static int has_channel(struct rt_channel_info *channel_set,
 271                                            u8 chanset_size,
 272                                            u8 chan) {
 273         int i;
 274
 275         for (i = 0; i < chanset_size; i++) {
 276                 if (channel_set[i].ChannelNum == chan)
 277                         return 1;
 278         }
 279         return 0;
 280 }
 281
 282 static void init_channel_list(struct adapter *padapter, struct rt_channel_info *channel_set,
 283                                                           u8 chanset_size,
 284                                                           struct p2p_channels *channel_list) {
 285         struct p2p_oper_class_map op_class[] = {
 286                 { IEEE80211G,  81,   1,  13,  1, BW20 },
 287                 { IEEE80211G,  82,  14,  14,  1, BW20 },
 288                 { -1, 0, 0, 0, 0, BW20 }
 289         };
 290
 291         int cla, op;
 292
 293         cla = 0;
 294
 295         for (op = 0; op_class[op].op_class; op++) {
 296                 u8 ch;
 297                 struct p2p_oper_class_map *o = &op_class[op];
 298                 struct p2p_reg_class *reg = NULL;
 299
 300                 for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
 301                         if (!has_channel(channel_set, chanset_size, ch)) {
 302                                 continue;
 303                         }
 304
 305                         if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
 306                                 continue;
 307
 308                         if ((0 == (padapter->registrypriv.cbw40_enable & BIT(1))) &&
 309                             ((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
 310                                 continue;
 311
 312                         if (reg == NULL) {
 313                                 reg = &channel_list->reg_class[cla];
 314                                 cla++;
 315                                 reg->reg_class = o->op_class;
 316                                 reg->channels = 0;
 317                         }
 318                         reg->channel[reg->channels] = ch;
 319                         reg->channels++;
 320                 }
 321         }
 322         channel_list->reg_classes = cla;
 323 }
 324
 325 static u8 init_channel_set(struct adapter *padapter, u8 ChannelPlan, struct rt_channel_info *channel_set)
 326 {
 327         u8 index, chanset_size = 0;
 328         u8 b2_4GBand = false;
 329         u8 Index2G = 0;
 330
 331         _rtw_memset(channel_set, 0, sizeof(struct rt_channel_info) * MAX_CHANNEL_NUM);
 332
 333         if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
 334                 DBG_88E("ChannelPlan ID %x error !!!!!\n", ChannelPlan);
 335                 return chanset_size;
 336         }
 337
 338         if (padapter->registrypriv.wireless_mode & WIRELESS_11G) {
 339                 b2_4GBand = true;
 340                 if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
 341                         Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
 342                 else
 343                         Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
 344         }
 345
 346         if (b2_4GBand) {
 347                 for (index = 0; index < RTW_ChannelPlan2G[Index2G].Len; index++) {
 348                         channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
 349
 350                         if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||/* Channel 1~11 is active, and 12~14 is passive */
 351                             (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == ChannelPlan)) {
 352                                 if (channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
 353                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 354                                 else if ((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
 355                                         channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
 356                         } else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
 357                                    RT_CHANNEL_DOMAIN_2G_WORLD == Index2G) {/*  channel 12~13, passive scan */
 358                                 if (channel_set[chanset_size].ChannelNum <= 11)
 359                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 360                                 else
 361                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 362                         } else {
 363                                 channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 364                         }
 365
 366                         chanset_size++;
 367                 }
 368         }
 369         return chanset_size;
 370 }
 371
 372 int     init_mlme_ext_priv(struct adapter *padapter)
 373 {
 374         int     res = _SUCCESS;
 375         struct registry_priv *pregistrypriv = &padapter->registrypriv;
 376         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 377         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
 378         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 379
 380         pmlmeext->padapter = padapter;
 381
 382         init_mlme_ext_priv_value(padapter);
 383         pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
 384
 385         init_mlme_ext_timer(padapter);
 386
 387 #ifdef CONFIG_88EU_AP_MODE
 388         init_mlme_ap_info(padapter);
 389 #endif
 390
 391         pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan, pmlmeext->channel_set);
 392         init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
 393
 394         pmlmeext->chan_scan_time = SURVEY_TO;
 395         pmlmeext->mlmeext_init = true;
 396
 397
 398         pmlmeext->active_keep_alive_check = true;
 399
 400         return res;
 401 }
 402
 403 void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
 404 {
 405         struct adapter *padapter = pmlmeext->padapter;
 406
 407         if (!padapter)
 408                 return;
 409
 410         if (padapter->bDriverStopped) {
 411                 _cancel_timer_ex(&pmlmeext->survey_timer);
 412                 _cancel_timer_ex(&pmlmeext->link_timer);
 413                 /* _cancel_timer_ex(&pmlmeext->ADDBA_timer); */
 414         }
 415 }
 416
 417 static void _mgt_dispatcher(struct adapter *padapter, struct mlme_handler *ptable, struct recv_frame *precv_frame)
 418 {
 419         u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 420         u8 *pframe = precv_frame->rx_data;
 421
 422           if (ptable->func) {
 423          /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
 424                 if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
 425                     memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
 426                         return;
 427                 ptable->func(padapter, precv_frame);
 428         }
 429 }
 430
 431 void mgt_dispatcher(struct adapter *padapter, struct recv_frame *precv_frame)
 432 {
 433         int index;
 434         struct mlme_handler *ptable;
 435 #ifdef CONFIG_88EU_AP_MODE
 436         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 437 #endif /* CONFIG_88EU_AP_MODE */
 438         u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 439         u8 *pframe = precv_frame->rx_data;
 440         struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));
 441
 442         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 443                  ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
 444                   GetFrameType(pframe), GetFrameSubType(pframe)));
 445
 446         if (GetFrameType(pframe) != WIFI_MGT_TYPE) {
 447                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
 448                 return;
 449         }
 450
 451         /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
 452         if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
 453             memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
 454                 return;
 455
 456         ptable = mlme_sta_tbl;
 457
 458         index = GetFrameSubType(pframe) >> 4;
 459
 460         if (index > 13) {
 461                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Currently we do not support reserved sub-fr-type=%d\n", index));
 462                 return;
 463         }
 464         ptable += index;
 465
 466         if (psta != NULL) {
 467                 if (GetRetry(pframe)) {
 468                         if (precv_frame->attrib.seq_num ==
 469                             psta->RxMgmtFrameSeqNum) {
 470                                 /* drop the duplicate management frame */
 471                                 DBG_88E("Drop duplicate management frame with seq_num=%d.\n",
 472                                         precv_frame->attrib.seq_num);
 473                                 return;
 474                         }
 475                 }
 476                 psta->RxMgmtFrameSeqNum = precv_frame->attrib.seq_num;
 477         }
 478
 479 #ifdef CONFIG_88EU_AP_MODE
 480         switch (GetFrameSubType(pframe)) {
 481         case WIFI_AUTH:
 482                 if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 483                         ptable->func = &OnAuth;
 484                 else
 485                         ptable->func = &OnAuthClient;
 486                 /* fall through */
 487         case WIFI_ASSOCREQ:
 488         case WIFI_REASSOCREQ:
 489                 _mgt_dispatcher(padapter, ptable, precv_frame);
 490                 break;
 491         case WIFI_PROBEREQ:
 492                 if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 493                         _mgt_dispatcher(padapter, ptable, precv_frame);
 494                 else
 495                         _mgt_dispatcher(padapter, ptable, precv_frame);
 496                 break;
 497         case WIFI_BEACON:
 498                 _mgt_dispatcher(padapter, ptable, precv_frame);
 499                 break;
 500         case WIFI_ACTION:
 501                 _mgt_dispatcher(padapter, ptable, precv_frame);
 502                 break;
 503         default:
 504                 _mgt_dispatcher(padapter, ptable, precv_frame);
 505                 if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 506                         rtw_hostapd_mlme_rx(padapter, precv_frame);
 507                 break;
 508         }
 509 #else
 510         _mgt_dispatcher(padapter, ptable, precv_frame);
 511 #endif
 512 }
 513
 514 #ifdef CONFIG_88EU_P2P
 515 static u32 p2p_listen_state_process(struct adapter *padapter, unsigned char *da)
 516 {
 517         bool response = true;
 518
 519         /*      do nothing if the device name is empty */
 520         if (!padapter->wdinfo.device_name_len)
 521                 response = false;
 522
 523         if (response)
 524                 issue_probersp_p2p(padapter, da);
 525
 526         return _SUCCESS;
 527 }
 528 #endif /* CONFIG_88EU_P2P */
 529
 530
 531 /****************************************************************************
 532
 533 Following are the callback functions for each subtype of the management frames
 534
 535 *****************************************************************************/
 536
 537 unsigned int OnProbeReq(struct adapter *padapter, struct recv_frame *precv_frame)
 538 {
 539         unsigned int    ielen;
 540         unsigned char   *p;
 541         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 542         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 543         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 544         struct wlan_bssid_ex *cur = &(pmlmeinfo->network);
 545         u8 *pframe = precv_frame->rx_data;
 546         uint len = precv_frame->len;
 547         u8 is_valid_p2p_probereq = false;
 548
 549 #ifdef CONFIG_88EU_P2P
 550         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
 551         u8 wifi_test_chk_rate = 1;
 552
 553         if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
 554             !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) &&
 555             !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
 556             !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
 557             !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)) {
 558                 /*      mcs_rate = 0 -> CCK 1M rate */
 559                 /*      mcs_rate = 1 -> CCK 2M rate */
 560                 /*      mcs_rate = 2 -> CCK 5.5M rate */
 561                 /*      mcs_rate = 3 -> CCK 11M rate */
 562                 /*      In the P2P mode, the driver should not support the CCK rate */
 563
 564                 /*      Commented by Kurt 2012/10/16 */
 565                 /*      IOT issue: Google Nexus7 use 1M rate to send p2p_probe_req after GO nego completed and Nexus7 is client */
 566                 if (wifi_test_chk_rate == 1) {
 567                         is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len);
 568                         if (is_valid_p2p_probereq) {
 569                                 if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
 570                                         /*  FIXME */
 571                                         report_survey_event(padapter, precv_frame);
 572                                         p2p_listen_state_process(padapter,  get_sa(pframe));
 573
 574                                         return _SUCCESS;
 575                                 }
 576
 577                                 if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
 578                                         goto _continue;
 579                         }
 580                 }
 581         }
 582
 583 _continue:
 584 #endif /* CONFIG_88EU_P2P */
 585
 586         if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 587                 return _SUCCESS;
 588
 589         if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
 590             !check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE))
 591                 return _SUCCESS;
 592
 593         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
 594                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
 595
 596         /* check (wildcard) SSID */
 597         if (p != NULL) {
 598                 if (is_valid_p2p_probereq)
 599                         goto _issue_probersp;
 600
 601                 if ((ielen != 0 && memcmp((void *)(p+2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength)) ||
 602                     (ielen == 0 && pmlmeinfo->hidden_ssid_mode))
 603                         return _SUCCESS;
 604
 605 _issue_probersp:
 606
 607                 if (check_fwstate(pmlmepriv, _FW_LINKED) &&
 608                     pmlmepriv->cur_network.join_res)
 609                         issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
 610         }
 611         return _SUCCESS;
 612 }
 613
 614 unsigned int OnProbeRsp(struct adapter *padapter, struct recv_frame *precv_frame)
 615 {
 616         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 617 #ifdef CONFIG_88EU_P2P
 618         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
 619         u8 *pframe = precv_frame->rx_data;
 620 #endif
 621
 622 #ifdef CONFIG_88EU_P2P
 623         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
 624                 if (pwdinfo->tx_prov_disc_info.benable) {
 625                         if (!memcmp(pwdinfo->tx_prov_disc_info.peerIFAddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
 626                                 if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
 627                                         pwdinfo->tx_prov_disc_info.benable = false;
 628                                         issue_p2p_provision_request(padapter,
 629                                                                     pwdinfo->tx_prov_disc_info.ssid.Ssid,
 630                                                                     pwdinfo->tx_prov_disc_info.ssid.SsidLength,
 631                                                                     pwdinfo->tx_prov_disc_info.peerDevAddr);
 632                                 } else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
 633                                         pwdinfo->tx_prov_disc_info.benable = false;
 634                                         issue_p2p_provision_request(padapter, NULL, 0,
 635                                                                     pwdinfo->tx_prov_disc_info.peerDevAddr);
 636                                 }
 637                         }
 638                 }
 639                 return _SUCCESS;
 640         } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
 641                 if (pwdinfo->nego_req_info.benable) {
 642                         DBG_88E("[%s] P2P State is GONEGO ING!\n", __func__);
 643                         if (!memcmp(pwdinfo->nego_req_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
 644                                 pwdinfo->nego_req_info.benable = false;
 645                                 issue_p2p_GO_request(padapter, pwdinfo->nego_req_info.peerDevAddr);
 646                         }
 647                 }
 648         } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) {
 649                 if (pwdinfo->invitereq_info.benable) {
 650                         DBG_88E("[%s] P2P_STATE_TX_INVITE_REQ!\n", __func__);
 651                         if (!memcmp(pwdinfo->invitereq_info.peer_macaddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
 652                                 pwdinfo->invitereq_info.benable = false;
 653                                 issue_p2p_invitation_request(padapter, pwdinfo->invitereq_info.peer_macaddr);
 654                         }
 655                 }
 656         }
 657 #endif
 658
 659
 660         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
 661                 report_survey_event(padapter, precv_frame);
 662                 return _SUCCESS;
 663         }
 664
 665         return _SUCCESS;
 666 }
 667
 668 unsigned int OnBeacon(struct adapter *padapter, struct recv_frame *precv_frame)
 669 {
 670         int cam_idx;
 671         struct sta_info *psta;
 672         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 673         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 674         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 675         struct sta_priv *pstapriv = &padapter->stapriv;
 676         u8 *pframe = precv_frame->rx_data;
 677         uint len = precv_frame->len;
 678         struct wlan_bssid_ex *pbss;
 679         int ret = _SUCCESS;
 680
 681         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
 682                 report_survey_event(padapter, precv_frame);
 683                 return _SUCCESS;
 684         }
 685
 686         if (!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)) {
 687                 if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
 688                         /* we should update current network before auth, or some IE is wrong */
 689                         pbss = (struct wlan_bssid_ex *)rtw_malloc(sizeof(struct wlan_bssid_ex));
 690                         if (pbss) {
 691                                 if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
 692                                         update_network(&(pmlmepriv->cur_network.network), pbss, padapter, true);
 693                                         rtw_get_bcn_info(&(pmlmepriv->cur_network));
 694                                 }
 695                                 kfree(pbss);
 696                         }
 697
 698                         /* check the vendor of the assoc AP */
 699                         pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct rtw_ieee80211_hdr_3addr), len-sizeof(struct rtw_ieee80211_hdr_3addr));
 700
 701                         /* update TSF Value */
 702                         update_TSF(pmlmeext, pframe, len);
 703
 704                         /* start auth */
 705                         start_clnt_auth(padapter);
 706
 707                         return _SUCCESS;
 708                 }
 709
 710                 if (((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
 711                         psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
 712                         if (psta != NULL) {
 713                                 ret = rtw_check_bcn_info(padapter, pframe, len);
 714                                 if (!ret) {
 715                                                 DBG_88E_LEVEL(_drv_info_, "ap has changed, disconnect now\n ");
 716                                                 receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 65535);
 717                                                 return _SUCCESS;
 718                                 }
 719                                 /* update WMM, ERP in the beacon */
 720                                 /* todo: the timer is used instead of the number of the beacon received */
 721                                 if ((sta_rx_pkts(psta) & 0xf) == 0)
 722                                         update_beacon_info(padapter, pframe, len, psta);
 723                                 process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
 724                         }
 725                 } else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
 726                         psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
 727                         if (psta != NULL) {
 728                                 /* update WMM, ERP in the beacon */
 729                                 /* todo: the timer is used instead of the number of the beacon received */
 730                                 if ((sta_rx_pkts(psta) & 0xf) == 0)
 731                                         update_beacon_info(padapter, pframe, len, psta);
 732                         } else {
 733                                 /* allocate a new CAM entry for IBSS station */
 734                                 cam_idx = allocate_fw_sta_entry(padapter);
 735                                 if (cam_idx == NUM_STA)
 736                                         goto _END_ONBEACON_;
 737
 738                                 /* get supported rate */
 739                                 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) {
 740                                         pmlmeinfo->FW_sta_info[cam_idx].status = 0;
 741                                         goto _END_ONBEACON_;
 742                                 }
 743
 744                                 /* update TSF Value */
 745                                 update_TSF(pmlmeext, pframe, len);
 746
 747                                 /* report sta add event */
 748                                 report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
 749                         }
 750                 }
 751         }
 752
 753 _END_ONBEACON_:
 754
 755         return _SUCCESS;
 756 }
 757
 758 unsigned int OnAuth(struct adapter *padapter, struct recv_frame *precv_frame)
 759 {
 760 #ifdef CONFIG_88EU_AP_MODE
 761         unsigned int    auth_mode, ie_len;
 762         u16 seq;
 763         unsigned char   *sa, *p;
 764         u16 algorithm;
 765         int     status;
 766         static struct sta_info stat;
 767         struct  sta_info        *pstat = NULL;
 768         struct  sta_priv *pstapriv = &padapter->stapriv;
 769         struct security_priv *psecuritypriv = &padapter->securitypriv;
 770         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 771         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 772         u8 *pframe = precv_frame->rx_data;
 773         uint len = precv_frame->len;
 774
 775         if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
 776                 return _FAIL;
 777
 778         DBG_88E("+OnAuth\n");
 779
 780         sa = GetAddr2Ptr(pframe);
 781
 782         auth_mode = psecuritypriv->dot11AuthAlgrthm;
 783         seq = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + 2));
 784         algorithm = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN));
 785
 786         DBG_88E("auth alg=%x, seq=%X\n", algorithm, seq);
 787
 788         if (auth_mode == 2 && psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
 789             psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
 790                 auth_mode = 0;
 791
 792         if ((algorithm > 0 && auth_mode == 0) ||        /*  rx a shared-key auth but shared not enabled */
 793             (algorithm == 0 && auth_mode == 1)) {       /*  rx a open-system auth but shared-key is enabled */
 794                 DBG_88E("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n",
 795                         algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
 796
 797                 status = _STATS_NO_SUPP_ALG_;
 798
 799                 goto auth_fail;
 800         }
 801
 802         if (!rtw_access_ctrl(padapter, sa)) {
 803                 status = _STATS_UNABLE_HANDLE_STA_;
 804                 goto auth_fail;
 805         }
 806
 807         pstat = rtw_get_stainfo(pstapriv, sa);
 808         if (pstat == NULL) {
 809                 /*  allocate a new one */
 810                 DBG_88E("going to alloc stainfo for sa=%pM\n", sa);
 811                 pstat = rtw_alloc_stainfo(pstapriv, sa);
 812                 if (pstat == NULL) {
 813                         DBG_88E(" Exceed the upper limit of supported clients...\n");
 814                         status = _STATS_UNABLE_HANDLE_STA_;
 815                         goto auth_fail;
 816                 }
 817
 818                 pstat->state = WIFI_FW_AUTH_NULL;
 819                 pstat->auth_seq = 0;
 820         } else {
 821                 spin_lock_bh(&pstapriv->asoc_list_lock);
 822                 if (!rtw_is_list_empty(&pstat->asoc_list)) {
 823                         rtw_list_delete(&pstat->asoc_list);
 824                         pstapriv->asoc_list_cnt--;
 825                 }
 826                 spin_unlock_bh(&pstapriv->asoc_list_lock);
 827
 828                 if (seq == 1) {
 829                         /* TODO: STA re_auth and auth timeout */
 830                 }
 831         }
 832
 833         spin_lock_bh(&pstapriv->auth_list_lock);
 834         if (rtw_is_list_empty(&pstat->auth_list)) {
 835                 rtw_list_insert_tail(&pstat->auth_list, &pstapriv->auth_list);
 836                 pstapriv->auth_list_cnt++;
 837         }
 838         spin_unlock_bh(&pstapriv->auth_list_lock);
 839
 840         if (pstat->auth_seq == 0)
 841                 pstat->expire_to = pstapriv->auth_to;
 842
 843         if ((pstat->auth_seq + 1) != seq) {
 844                 DBG_88E("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
 845                         seq, pstat->auth_seq+1);
 846                 status = _STATS_OUT_OF_AUTH_SEQ_;
 847                 goto auth_fail;
 848         }
 849
 850         if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2)) {
 851                 if (seq == 1) {
 852                         pstat->state &= ~WIFI_FW_AUTH_NULL;
 853                         pstat->state |= WIFI_FW_AUTH_SUCCESS;
 854                         pstat->expire_to = pstapriv->assoc_to;
 855                         pstat->authalg = algorithm;
 856                 } else {
 857                         DBG_88E("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
 858                                 seq, pstat->auth_seq+1);
 859                         status = _STATS_OUT_OF_AUTH_SEQ_;
 860                         goto auth_fail;
 861                 }
 862         } else { /*  shared system or auto authentication */
 863                 if (seq == 1) {
 864                         /* prepare for the challenging txt... */
 865
 866                         pstat->state &= ~WIFI_FW_AUTH_NULL;
 867                         pstat->state |= WIFI_FW_AUTH_STATE;
 868                         pstat->authalg = algorithm;
 869                         pstat->auth_seq = 2;
 870                 } else if (seq == 3) {
 871                         /* checking for challenging txt... */
 872                         DBG_88E("checking for challenging txt...\n");
 873
 874                         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len,
 875                                         len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
 876
 877                         if ((p == NULL) || (ie_len <= 0)) {
 878                                 DBG_88E("auth rejected because challenge failure!(1)\n");
 879                                 status = _STATS_CHALLENGE_FAIL_;
 880                                 goto auth_fail;
 881                         }
 882
 883                         if (!memcmp((void *)(p + 2), pstat->chg_txt, 128)) {
 884                                 pstat->state &= (~WIFI_FW_AUTH_STATE);
 885                                 pstat->state |= WIFI_FW_AUTH_SUCCESS;
 886                                 /*  challenging txt is correct... */
 887                                 pstat->expire_to =  pstapriv->assoc_to;
 888                         } else {
 889                                 DBG_88E("auth rejected because challenge failure!\n");
 890                                 status = _STATS_CHALLENGE_FAIL_;
 891                                 goto auth_fail;
 892                         }
 893                 } else {
 894                         DBG_88E("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
 895                                 seq, pstat->auth_seq+1);
 896                         status = _STATS_OUT_OF_AUTH_SEQ_;
 897                         goto auth_fail;
 898                 }
 899         }
 900
 901         /*  Now, we are going to issue_auth... */
 902         pstat->auth_seq = seq + 1;
 903
 904 #ifdef CONFIG_88EU_AP_MODE
 905         issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
 906 #endif
 907
 908         if (pstat->state & WIFI_FW_AUTH_SUCCESS)
 909                 pstat->auth_seq = 0;
 910
 911         return _SUCCESS;
 912
 913 auth_fail:
 914
 915         if (pstat)
 916                 rtw_free_stainfo(padapter , pstat);
 917
 918         pstat = &stat;
 919         _rtw_memset((char *)pstat, '\0', sizeof(stat));
 920         pstat->auth_seq = 2;
 921         memcpy(pstat->hwaddr, sa, 6);
 922
 923 #ifdef CONFIG_88EU_AP_MODE
 924         issue_auth(padapter, pstat, (unsigned short)status);
 925 #endif
 926
 927 #endif
 928         return _FAIL;
 929 }
 930
 931 unsigned int OnAuthClient(struct adapter *padapter, struct recv_frame *precv_frame)
 932 {
 933         unsigned int    seq, len, status, offset;
 934         unsigned char   *p;
 935         unsigned int    go2asoc = 0;
 936         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 937         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 938         u8 *pframe = precv_frame->rx_data;
 939         uint pkt_len = precv_frame->len;
 940
 941         DBG_88E("%s\n", __func__);
 942
 943         /* check A1 matches or not */
 944         if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
 945                 return _SUCCESS;
 946
 947         if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
 948                 return _SUCCESS;
 949
 950         offset = (GetPrivacy(pframe)) ? 4 : 0;
 951
 952         seq     = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 2));
 953         status  = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 4));
 954
 955         if (status != 0) {
 956                 DBG_88E("clnt auth fail, status: %d\n", status);
 957                 if (status == 13) { /*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
 958                         if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
 959                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
 960                         else
 961                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
 962                 }
 963
 964                 set_link_timer(pmlmeext, 1);
 965                 goto authclnt_fail;
 966         }
 967
 968         if (seq == 2) {
 969                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
 970                          /*  legendary shared system */
 971                         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
 972                                 pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
 973
 974                         if (p == NULL)
 975                                 goto authclnt_fail;
 976
 977                         memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
 978                         pmlmeinfo->auth_seq = 3;
 979                         issue_auth(padapter, NULL, 0);
 980                         set_link_timer(pmlmeext, REAUTH_TO);
 981
 982                         return _SUCCESS;
 983                 } else {
 984                         /*  open system */
 985                         go2asoc = 1;
 986                 }
 987         } else if (seq == 4) {
 988                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
 989                         go2asoc = 1;
 990                 else
 991                         goto authclnt_fail;
 992         } else {
 993                 /*  this is also illegal */
 994                 goto authclnt_fail;
 995         }
 996
 997         if (go2asoc) {
 998                 DBG_88E_LEVEL(_drv_info_, "auth success, start assoc\n");
 999                 start_clnt_assoc(padapter);
1000                 return _SUCCESS;
1001         }
1002 authclnt_fail:
1003         return _FAIL;
1004 }
1005
1006 unsigned int OnAssocReq(struct adapter *padapter, struct recv_frame *precv_frame)
1007 {
1008 #ifdef CONFIG_88EU_AP_MODE
1009         u16 capab_info;
1010         struct rtw_ieee802_11_elems elems;
1011         struct sta_info *pstat;
1012         unsigned char           reassoc, *p, *pos, *wpa_ie;
1013         unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
1014         int             i, ie_len, wpa_ie_len, left;
1015         unsigned char           supportRate[16];
1016         int                                     supportRateNum;
1017         unsigned short          status = _STATS_SUCCESSFUL_;
1018         unsigned short          frame_type, ie_offset = 0;
1019         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1020         struct security_priv *psecuritypriv = &padapter->securitypriv;
1021         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
1022         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1023         struct wlan_bssid_ex *cur = &(pmlmeinfo->network);
1024         struct sta_priv *pstapriv = &padapter->stapriv;
1025         u8 *pframe = precv_frame->rx_data;
1026         uint pkt_len = precv_frame->len;
1027 #ifdef CONFIG_88EU_P2P
1028         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
1029         u8 p2p_status_code = P2P_STATUS_SUCCESS;
1030         u8 *p2pie;
1031         u32 p2pielen = 0;
1032 #endif /* CONFIG_88EU_P2P */
1033
1034         if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
1035                 return _FAIL;
1036
1037         frame_type = GetFrameSubType(pframe);
1038         if (frame_type == WIFI_ASSOCREQ) {
1039                 reassoc = 0;
1040                 ie_offset = _ASOCREQ_IE_OFFSET_;
1041         } else { /*  WIFI_REASSOCREQ */
1042                 reassoc = 1;
1043                 ie_offset = _REASOCREQ_IE_OFFSET_;
1044         }
1045
1046
1047         if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
1048                 DBG_88E("handle_assoc(reassoc=%d) - too short payload (len=%lu)"
1049                        "\n", reassoc, (unsigned long)pkt_len);
1050                 return _FAIL;
1051         }
1052
1053         pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
1054         if (pstat == (struct sta_info *)NULL) {
1055                 status = _RSON_CLS2_;
1056                 goto asoc_class2_error;
1057         }
1058
1059         capab_info = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN);
1060
1061         left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
1062         pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
1063
1064
1065         DBG_88E("%s\n", __func__);
1066
1067         /*  check if this stat has been successfully authenticated/assocated */
1068         if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) {
1069                 if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) {
1070                         status = _RSON_CLS2_;
1071                         goto asoc_class2_error;
1072                 } else {
1073                         pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
1074                         pstat->state |= WIFI_FW_ASSOC_STATE;
1075                 }
1076         } else {
1077                 pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
1078                 pstat->state |= WIFI_FW_ASSOC_STATE;
1079         }
1080         pstat->capability = capab_info;
1081         /* now parse all ieee802_11 ie to point to elems */
1082         if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
1083             !elems.ssid) {
1084                 DBG_88E("STA %pM sent invalid association request\n",
1085                         pstat->hwaddr);
1086                 status = _STATS_FAILURE_;
1087                 goto OnAssocReqFail;
1088         }
1089
1090
1091         /*  now we should check all the fields... */
1092         /*  checking SSID */
1093         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
1094                 pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1095         if (p == NULL)
1096                 status = _STATS_FAILURE_;
1097
1098         if (ie_len == 0) { /*  broadcast ssid, however it is not allowed in assocreq */
1099                 status = _STATS_FAILURE_;
1100         } else {
1101                 /*  check if ssid match */
1102                 if (memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
1103                         status = _STATS_FAILURE_;
1104
1105                 if (ie_len != cur->Ssid.SsidLength)
1106                         status = _STATS_FAILURE_;
1107         }
1108
1109         if (_STATS_SUCCESSFUL_ != status)
1110                 goto OnAssocReqFail;
1111
1112         /*  check if the supported rate is ok */
1113         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1114         if (p == NULL) {
1115                 DBG_88E("Rx a sta assoc-req which supported rate is empty!\n");
1116                 /*  use our own rate set as statoin used */
1117                 /* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
1118                 /* supportRateNum = AP_BSSRATE_LEN; */
1119
1120                 status = _STATS_FAILURE_;
1121                 goto OnAssocReqFail;
1122         } else {
1123                 memcpy(supportRate, p+2, ie_len);
1124                 supportRateNum = ie_len;
1125
1126                 p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_ , &ie_len,
1127                                 pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1128                 if (p !=  NULL) {
1129                         if (supportRateNum <= sizeof(supportRate)) {
1130                                 memcpy(supportRate+supportRateNum, p+2, ie_len);
1131                                 supportRateNum += ie_len;
1132                         }
1133                 }
1134         }
1135
1136         /* todo: mask supportRate between AP & STA -> move to update raid */
1137         /* get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0); */
1138
1139         /* update station supportRate */
1140         pstat->bssratelen = supportRateNum;
1141         memcpy(pstat->bssrateset, supportRate, supportRateNum);
1142         UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
1143
1144         /* check RSN/WPA/WPS */
1145         pstat->dot8021xalg = 0;
1146         pstat->wpa_psk = 0;
1147         pstat->wpa_group_cipher = 0;
1148         pstat->wpa2_group_cipher = 0;
1149         pstat->wpa_pairwise_cipher = 0;
1150         pstat->wpa2_pairwise_cipher = 0;
1151         _rtw_memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
1152         if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
1153                 int group_cipher = 0, pairwise_cipher = 0;
1154
1155                 wpa_ie = elems.rsn_ie;
1156                 wpa_ie_len = elems.rsn_ie_len;
1157
1158                 if (rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
1159                         pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
1160                         pstat->wpa_psk |= BIT(1);
1161
1162                         pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
1163                         pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
1164
1165                         if (!pstat->wpa2_group_cipher)
1166                                 status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
1167
1168                         if (!pstat->wpa2_pairwise_cipher)
1169                                 status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
1170                 } else {
1171                         status = WLAN_STATUS_INVALID_IE;
1172                 }
1173         } else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
1174                 int group_cipher = 0, pairwise_cipher = 0;
1175
1176                 wpa_ie = elems.wpa_ie;
1177                 wpa_ie_len = elems.wpa_ie_len;
1178
1179                 if (rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
1180                         pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
1181                         pstat->wpa_psk |= BIT(0);
1182
1183                         pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
1184                         pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
1185
1186                         if (!pstat->wpa_group_cipher)
1187                                 status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
1188
1189                         if (!pstat->wpa_pairwise_cipher)
1190                                 status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
1191                 } else {
1192                         status = WLAN_STATUS_INVALID_IE;
1193                 }
1194         } else {
1195                 wpa_ie = NULL;
1196                 wpa_ie_len = 0;
1197         }
1198
1199         if (_STATS_SUCCESSFUL_ != status)
1200                 goto OnAssocReqFail;
1201
1202         pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
1203         if (wpa_ie == NULL) {
1204                 if (elems.wps_ie) {
1205                         DBG_88E("STA included WPS IE in "
1206                                    "(Re)Association Request - assume WPS is "
1207                                    "used\n");
1208                         pstat->flags |= WLAN_STA_WPS;
1209                         /* wpabuf_free(sta->wps_ie); */
1210                         /* sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4, */
1211                         /*                              elems.wps_ie_len - 4); */
1212                 } else {
1213                         DBG_88E("STA did not include WPA/RSN IE "
1214                                    "in (Re)Association Request - possible WPS "
1215                                    "use\n");
1216                         pstat->flags |= WLAN_STA_MAYBE_WPS;
1217                 }
1218
1219
1220                 /*  AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
1221                 /*  that the selected registrar of AP is _FLASE */
1222                 if ((psecuritypriv->wpa_psk > 0) && (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS))) {
1223                         if (pmlmepriv->wps_beacon_ie) {
1224                                 u8 selected_registrar = 0;
1225
1226                                 rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR , &selected_registrar, NULL);
1227
1228                                 if (!selected_registrar) {
1229                                         DBG_88E("selected_registrar is false , or AP is not ready to do WPS\n");
1230
1231                                         status = _STATS_UNABLE_HANDLE_STA_;
1232
1233                                         goto OnAssocReqFail;
1234                                 }
1235                         }
1236                 }
1237         } else {
1238                 int copy_len;
1239
1240                 if (psecuritypriv->wpa_psk == 0) {
1241                         DBG_88E("STA %pM: WPA/RSN IE in association "
1242                         "request, but AP don't support WPA/RSN\n", pstat->hwaddr);
1243
1244                         status = WLAN_STATUS_INVALID_IE;
1245
1246                         goto OnAssocReqFail;
1247                 }
1248
1249                 if (elems.wps_ie) {
1250                         DBG_88E("STA included WPS IE in "
1251                                    "(Re)Association Request - WPS is "
1252                                    "used\n");
1253                         pstat->flags |= WLAN_STA_WPS;
1254                         copy_len = 0;
1255                 } else {
1256                         copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)) : (wpa_ie_len+2);
1257                 }
1258                 if (copy_len > 0)
1259                         memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
1260         }
1261         /*  check if there is WMM IE & support WWM-PS */
1262         pstat->flags &= ~WLAN_STA_WME;
1263         pstat->qos_option = 0;
1264         pstat->qos_info = 0;
1265         pstat->has_legacy_ac = true;
1266         pstat->uapsd_vo = 0;
1267         pstat->uapsd_vi = 0;
1268         pstat->uapsd_be = 0;
1269         pstat->uapsd_bk = 0;
1270         if (pmlmepriv->qospriv.qos_option) {
1271                 p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
1272                 for (;;) {
1273                         p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1274                         if (p != NULL) {
1275                                 if (!memcmp(p+2, WMM_IE, 6)) {
1276                                         pstat->flags |= WLAN_STA_WME;
1277
1278                                         pstat->qos_option = 1;
1279                                         pstat->qos_info = *(p+8);
1280
1281                                         pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
1282
1283                                         if ((pstat->qos_info&0xf) != 0xf)
1284                                                 pstat->has_legacy_ac = true;
1285                                         else
1286                                                 pstat->has_legacy_ac = false;
1287
1288                                         if (pstat->qos_info&0xf) {
1289                                                 if (pstat->qos_info&BIT(0))
1290                                                         pstat->uapsd_vo = BIT(0)|BIT(1);
1291                                                 else
1292                                                         pstat->uapsd_vo = 0;
1293
1294                                                 if (pstat->qos_info&BIT(1))
1295                                                         pstat->uapsd_vi = BIT(0)|BIT(1);
1296                                                 else
1297                                                         pstat->uapsd_vi = 0;
1298
1299                                                 if (pstat->qos_info&BIT(2))
1300                                                         pstat->uapsd_bk = BIT(0)|BIT(1);
1301                                                 else
1302                                                         pstat->uapsd_bk = 0;
1303
1304                                                 if (pstat->qos_info&BIT(3))
1305                                                         pstat->uapsd_be = BIT(0)|BIT(1);
1306                                                 else
1307                                                         pstat->uapsd_be = 0;
1308                                         }
1309                                         break;
1310                                 }
1311                         } else {
1312                                 break;
1313                         }
1314                         p = p + ie_len + 2;
1315                 }
1316         }
1317
1318         /* save HT capabilities in the sta object */
1319         _rtw_memset(&pstat->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));
1320         if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap)) {
1321                 pstat->flags |= WLAN_STA_HT;
1322
1323                 pstat->flags |= WLAN_STA_WME;
1324
1325                 memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap));
1326         } else {
1327                 pstat->flags &= ~WLAN_STA_HT;
1328         }
1329         if ((!pmlmepriv->htpriv.ht_option) && (pstat->flags&WLAN_STA_HT)) {
1330                 status = _STATS_FAILURE_;
1331                 goto OnAssocReqFail;
1332         }
1333
1334         if ((pstat->flags & WLAN_STA_HT) &&
1335             ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
1336             (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP))) {
1337                 DBG_88E("HT: %pM tried to "
1338                         "use TKIP with HT association\n", pstat->hwaddr);
1339
1340                 /* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
1341                 /* goto OnAssocReqFail; */
1342         }
1343
1344         pstat->flags |= WLAN_STA_NONERP;
1345         for (i = 0; i < pstat->bssratelen; i++) {
1346                 if ((pstat->bssrateset[i] & 0x7f) > 22) {
1347                         pstat->flags &= ~WLAN_STA_NONERP;
1348                         break;
1349                 }
1350         }
1351
1352         if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
1353                 pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
1354         else
1355                 pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
1356
1357
1358
1359         if (status != _STATS_SUCCESSFUL_)
1360                 goto OnAssocReqFail;
1361
1362 #ifdef CONFIG_88EU_P2P
1363         pstat->is_p2p_device = false;
1364         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
1365                 p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , NULL, &p2pielen);
1366                 if (p2pie) {
1367                         pstat->is_p2p_device = true;
1368                         p2p_status_code = (u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat);
1369                         if (p2p_status_code > 0) {
1370                                 pstat->p2p_status_code = p2p_status_code;
1371                                 status = _STATS_CAP_FAIL_;
1372                                 goto OnAssocReqFail;
1373                         }
1374                 }
1375         }
1376         pstat->p2p_status_code = p2p_status_code;
1377 #endif /* CONFIG_88EU_P2P */
1378
1379         /* TODO: identify_proprietary_vendor_ie(); */
1380         /*  Realtek proprietary IE */
1381         /*  identify if this is Broadcom sta */
1382         /*  identify if this is ralink sta */
1383         /*  Customer proprietary IE */
1384
1385         /* get a unique AID */
1386         if (pstat->aid > 0) {
1387                 DBG_88E("  old AID %d\n", pstat->aid);
1388         } else {
1389                 for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
1390                         if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
1391                                 break;
1392
1393                 /* if (pstat->aid > NUM_STA) { */
1394                 if (pstat->aid > pstapriv->max_num_sta) {
1395                         pstat->aid = 0;
1396
1397                         DBG_88E("  no room for more AIDs\n");
1398
1399                         status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
1400
1401                         goto OnAssocReqFail;
1402                 } else {
1403                         pstapriv->sta_aid[pstat->aid - 1] = pstat;
1404                         DBG_88E("allocate new AID=(%d)\n", pstat->aid);
1405                 }
1406         }
1407
1408         pstat->state &= (~WIFI_FW_ASSOC_STATE);
1409         pstat->state |= WIFI_FW_ASSOC_SUCCESS;
1410
1411         spin_lock_bh(&pstapriv->auth_list_lock);
1412         if (!rtw_is_list_empty(&pstat->auth_list)) {
1413                 rtw_list_delete(&pstat->auth_list);
1414                 pstapriv->auth_list_cnt--;
1415         }
1416         spin_unlock_bh(&pstapriv->auth_list_lock);
1417
1418         spin_lock_bh(&pstapriv->asoc_list_lock);
1419         if (rtw_is_list_empty(&pstat->asoc_list)) {
1420                 pstat->expire_to = pstapriv->expire_to;
1421                 rtw_list_insert_tail(&pstat->asoc_list, &pstapriv->asoc_list);
1422                 pstapriv->asoc_list_cnt++;
1423         }
1424         spin_unlock_bh(&pstapriv->asoc_list_lock);
1425
1426         /*  now the station is qualified to join our BSS... */
1427         if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_ == status)) {
1428 #ifdef CONFIG_88EU_AP_MODE
1429                 /* 1 bss_cap_update & sta_info_update */
1430                 bss_cap_update_on_sta_join(padapter, pstat);
1431                 sta_info_update(padapter, pstat);
1432
1433                 /* issue assoc rsp before notify station join event. */
1434                 if (frame_type == WIFI_ASSOCREQ)
1435                         issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
1436                 else
1437                         issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
1438
1439                 /* 2 - report to upper layer */
1440                 DBG_88E("indicate_sta_join_event to upper layer - hostapd\n");
1441                 rtw_indicate_sta_assoc_event(padapter, pstat);
1442
1443                 /* 3-(1) report sta add event */
1444                 report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
1445 #endif
1446         }
1447
1448         return _SUCCESS;
1449
1450 asoc_class2_error:
1451
1452 #ifdef CONFIG_88EU_AP_MODE
1453         issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
1454 #endif
1455
1456         return _FAIL;
1457
1458 OnAssocReqFail:
1459
1460
1461 #ifdef CONFIG_88EU_AP_MODE
1462         pstat->aid = 0;
1463         if (frame_type == WIFI_ASSOCREQ)
1464                 issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
1465         else
1466                 issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
1467 #endif
1468
1469
1470 #endif /* CONFIG_88EU_AP_MODE */
1471
1472         return _FAIL;
1473 }
1474
1475 unsigned int OnAssocRsp(struct adapter *padapter, struct recv_frame *precv_frame)
1476 {
1477         uint i;
1478         int res;
1479         unsigned short  status;
1480         struct ndis_802_11_var_ie *pIE;
1481         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1482         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1483         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1484         /* struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); */
1485         u8 *pframe = precv_frame->rx_data;
1486         uint pkt_len = precv_frame->len;
1487
1488         DBG_88E("%s\n", __func__);
1489
1490         /* check A1 matches or not */
1491         if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
1492                 return _SUCCESS;
1493
1494         if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
1495                 return _SUCCESS;
1496
1497         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
1498                 return _SUCCESS;
1499
1500         _cancel_timer_ex(&pmlmeext->link_timer);
1501
1502         /* status */
1503         status = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 2));
1504         if (status > 0) {
1505                 DBG_88E("assoc reject, status code: %d\n", status);
1506                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
1507                 res = -4;
1508                 goto report_assoc_result;
1509         }
1510
1511         /* get capabilities */
1512         pmlmeinfo->capability = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
1513
1514         /* set slot time */
1515         pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10)) ? 9 : 20;
1516
1517         /* AID */
1518         pmlmeinfo->aid = (int)(le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
1519         res = pmlmeinfo->aid;
1520
1521         /* following are moved to join event callback function */
1522         /* to handle HT, WMM, rate adaptive, update MAC reg */
1523         /* for not to handle the synchronous IO in the tasklet */
1524         for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;) {
1525                 pIE = (struct ndis_802_11_var_ie *)(pframe + i);
1526
1527                 switch (pIE->ElementID) {
1528                 case _VENDOR_SPECIFIC_IE_:
1529                         if (!memcmp(pIE->data, WMM_PARA_OUI, 6)) /* WMM */
1530                                 WMM_param_handler(padapter, pIE);
1531                         break;
1532                 case _HT_CAPABILITY_IE_:        /* HT caps */
1533                         HT_caps_handler(padapter, pIE);
1534                         break;
1535                 case _HT_EXTRA_INFO_IE_:        /* HT info */
1536                         HT_info_handler(padapter, pIE);
1537                         break;
1538                 case _ERPINFO_IE_:
1539                         ERP_IE_handler(padapter, pIE);
1540                 default:
1541                         break;
1542                 }
1543
1544                 i += (pIE->Length + 2);
1545         }
1546
1547         pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
1548         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
1549
1550         /* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
1551         UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);
1552
1553 report_assoc_result:
1554         if (res > 0) {
1555                 rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
1556         } else {
1557                 rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
1558         }
1559
1560         report_join_res(padapter, res);
1561
1562         return _SUCCESS;
1563 }
1564
1565 unsigned int OnDeAuth(struct adapter *padapter, struct recv_frame *precv_frame)
1566 {
1567         unsigned short  reason;
1568         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1569         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1570         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1571         u8 *pframe = precv_frame->rx_data;
1572 #ifdef CONFIG_88EU_P2P
1573         struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
1574 #endif /* CONFIG_88EU_P2P */
1575
1576         /* check A3 */
1577         if (memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network),
1578                    ETH_ALEN))
1579                 return _SUCCESS;
1580
1581 #ifdef CONFIG_88EU_P2P
1582         if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
1583                 _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
1584                 _set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
1585         }
1586 #endif /* CONFIG_88EU_P2P */
1587
1588         reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
1589
1590         DBG_88E("%s Reason code(%d)\n", __func__, reason);
1591
1592 #ifdef CONFIG_88EU_AP_MODE
1593         if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
1594                 struct sta_info *psta;
1595                 struct sta_priv *pstapriv = &padapter->stapriv;
1596
1597                 DBG_88E_LEVEL(_drv_always_, "ap recv deauth reason code(%d) sta:%pM\n",
1598                               reason, GetAddr2Ptr(pframe));
1599
1600                 psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
1601                 if (psta) {
1602                         u8 updated = 0;
1603
1604                         spin_lock_bh(&pstapriv->asoc_list_lock);
1605                         if (!rtw_is_list_empty(&psta->asoc_list)) {
1606                                 rtw_list_delete(&psta->asoc_list);
1607                                 pstapriv->asoc_list_cnt--;
1608                                 updated = ap_free_sta(padapter, psta, false, reason);
1609                         }
1610                         spin_unlock_bh(&pstapriv->asoc_list_lock);
1611
1612                         associated_clients_update(padapter, updated);
1613                 }
1614
1615
1616                 return _SUCCESS;
1617         } else
1618 #endif
1619         {
1620                 DBG_88E_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM\n",
1621                               reason, GetAddr3Ptr(pframe));
1622
1623                 receive_disconnect(padapter, GetAddr3Ptr(pframe) , reason);
1624         }
1625         pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
1626         return _SUCCESS;
1627 }
1628
1629 unsigned int OnDisassoc(struct adapter *padapter, struct recv_frame *precv_frame)
1630 {
1631         u16 reason;
1632         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1633         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1634         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1635         u8 *pframe = precv_frame->rx_data;
1636 #ifdef CONFIG_88EU_P2P
1637         struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
1638 #endif /* CONFIG_88EU_P2P */
1639
1640         /* check A3 */
1641         if (memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network),
1642                    ETH_ALEN))
1643                 return _SUCCESS;
1644
1645 #ifdef CONFIG_88EU_P2P
1646         if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
1647                 _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
1648                 _set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
1649         }
1650 #endif /* CONFIG_88EU_P2P */
1651
1652         reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
1653
1654         DBG_88E("%s Reason code(%d)\n", __func__, reason);
1655
1656 #ifdef CONFIG_88EU_AP_MODE
1657         if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
1658                 struct sta_info *psta;
1659                 struct sta_priv *pstapriv = &padapter->stapriv;
1660
1661                 DBG_88E_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
1662                               reason, GetAddr2Ptr(pframe));
1663
1664                 psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
1665                 if (psta) {
1666                         u8 updated = 0;
1667
1668                         spin_lock_bh(&pstapriv->asoc_list_lock);
1669                         if (!rtw_is_list_empty(&psta->asoc_list)) {
1670                                 rtw_list_delete(&psta->asoc_list);
1671                                 pstapriv->asoc_list_cnt--;
1672                                 updated = ap_free_sta(padapter, psta, false, reason);
1673                         }
1674                         spin_unlock_bh(&pstapriv->asoc_list_lock);
1675
1676                         associated_clients_update(padapter, updated);
1677                 }
1678
1679                 return _SUCCESS;
1680         } else
1681 #endif
1682         {
1683                 DBG_88E_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
1684                               reason, GetAddr3Ptr(pframe));
1685
1686                 receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
1687         }
1688         pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
1689         return _SUCCESS;
1690 }
1691
1692 unsigned int OnAtim(struct adapter *padapter, struct recv_frame *precv_frame)
1693 {
1694         DBG_88E("%s\n", __func__);
1695         return _SUCCESS;
1696 }
1697
1698 unsigned int on_action_spct(struct adapter *padapter, struct recv_frame *precv_frame)
1699 {
1700         unsigned int ret = _FAIL;
1701         struct sta_info *psta = NULL;
1702         struct sta_priv *pstapriv = &padapter->stapriv;
1703         u8 *pframe = precv_frame->rx_data;
1704         u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
1705         u8 category;
1706         u8 action;
1707
1708         DBG_88E(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
1709
1710         psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
1711
1712         if (!psta)
1713                 goto exit;
1714
1715         category = frame_body[0];
1716         if (category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
1717                 goto exit;
1718
1719         action = frame_body[1];
1720         switch (action) {
1721         case RTW_WLAN_ACTION_SPCT_MSR_REQ:
1722         case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
1723         case RTW_WLAN_ACTION_SPCT_TPC_REQ:
1724         case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
1725                 break;
1726         case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
1727                 break;
1728         default:
1729                 break;
1730         }
1731
1732 exit:
1733         return ret;
1734 }
1735
1736 unsigned int OnAction_qos(struct adapter *padapter, struct recv_frame *precv_frame)
1737 {
1738         return _SUCCESS;
1739 }
1740
1741 unsigned int OnAction_dls(struct adapter *padapter, struct recv_frame *precv_frame)
1742 {
1743         return _SUCCESS;
1744 }
1745
1746 unsigned int OnAction_back(struct adapter *padapter, struct recv_frame *precv_frame)
1747 {
1748         u8 *addr;
1749         struct sta_info *psta = NULL;
1750         struct recv_reorder_ctrl *preorder_ctrl;
1751         unsigned char           *frame_body;
1752         unsigned char           category, action;
1753         unsigned short  tid, status, reason_code = 0;
1754         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1755         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1756         u8 *pframe = precv_frame->rx_data;
1757         struct sta_priv *pstapriv = &padapter->stapriv;
1758         /* check RA matches or not */
1759         if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe),
1760                    ETH_ALEN))/* for if1, sta/ap mode */
1761                 return _SUCCESS;
1762
1763         DBG_88E("%s\n", __func__);
1764
1765         if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
1766                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
1767                         return _SUCCESS;
1768
1769         addr = GetAddr2Ptr(pframe);
1770         psta = rtw_get_stainfo(pstapriv, addr);
1771
1772         if (psta == NULL)
1773                 return _SUCCESS;
1774
1775         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
1776
1777         category = frame_body[0];
1778         if (category == RTW_WLAN_CATEGORY_BACK) { /*  representing Block Ack */
1779                 if (!pmlmeinfo->HT_enable)
1780                         return _SUCCESS;
1781                 action = frame_body[1];
1782                 DBG_88E("%s, action=%d\n", __func__, action);
1783                 switch (action) {
1784                 case RTW_WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
1785                         memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
1786                         process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), addr);
1787
1788                         if (pmlmeinfo->bAcceptAddbaReq)
1789                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0);
1790                         else
1791                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);/* reject ADDBA Req */
1792                         break;
1793                 case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
1794                         status = RTW_GET_LE16(&frame_body[3]);
1795                         tid = ((frame_body[5] >> 2) & 0x7);
1796                         if (status == 0) {      /* successful */
1797                                 DBG_88E("agg_enable for TID=%d\n", tid);
1798                                 psta->htpriv.agg_enable_bitmap |= 1 << tid;
1799                                 psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
1800                         } else {
1801                                 psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
1802                         }
1803                         break;
1804                 case RTW_WLAN_ACTION_DELBA: /* DELBA */
1805                         if ((frame_body[3] & BIT(3)) == 0) {
1806                                 psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
1807                                 psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
1808                                 reason_code = RTW_GET_LE16(&frame_body[4]);
1809                         } else if ((frame_body[3] & BIT(3)) == BIT(3)) {
1810                                 tid = (frame_body[3] >> 4) & 0x0F;
1811                                 preorder_ctrl =  &psta->recvreorder_ctrl[tid];
1812                                 preorder_ctrl->enable = false;
1813                                 preorder_ctrl->indicate_seq = 0xffff;
1814                         }
1815                         DBG_88E("%s(): DELBA: %x(%x)\n", __func__, pmlmeinfo->agg_enable_bitmap, reason_code);
1816                         /* todo: how to notify the host while receiving DELETE BA */
1817                         break;
1818                 default:
1819                         break;
1820                 }
1821         }
1822         return _SUCCESS;
1823 }
1824
1825 #ifdef CONFIG_88EU_P2P
1826
1827 static int get_reg_classes_full_count(struct p2p_channels *channel_list)
1828 {
1829         int cnt = 0;
1830         int i;
1831
1832         for (i = 0; i < channel_list->reg_classes; i++) {
1833                 cnt += channel_list->reg_class[i].channels;
1834         }
1835
1836         return cnt;
1837 }
1838
1839 void issue_p2p_GO_request(struct adapter *padapter, u8 *raddr)
1840 {
1841         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
1842         u8 action = P2P_PUB_ACTION_ACTION;
1843         __be32 p2poui = cpu_to_be32(P2POUI);
1844         u8 oui_subtype = P2P_GO_NEGO_REQ;
1845         u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
1846         u8 wpsielen = 0, p2pielen = 0;
1847         u16 len_channellist_attr = 0;
1848         struct xmit_frame *pmgntframe;
1849         struct pkt_attrib *pattrib;
1850         unsigned char *pframe;
1851         struct rtw_ieee80211_hdr        *pwlanhdr;
1852         __le16 *fctrl;
1853         struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
1854         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
1855         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
1856
1857         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
1858         if (pmgntframe == NULL)
1859                 return;
1860
1861         DBG_88E("[%s] In\n", __func__);
1862         /* update attribute */
1863         pattrib = &pmgntframe->attrib;
1864         update_mgntframe_attrib(padapter, pattrib);
1865
1866         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
1867
1868         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
1869         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
1870
1871         fctrl = &(pwlanhdr->frame_ctl);
1872         *(fctrl) = 0;
1873
1874         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
1875         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
1876         memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
1877
1878         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
1879         pmlmeext->mgnt_seq++;
1880         SetFrameSubType(pframe, WIFI_ACTION);
1881
1882         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
1883         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
1884
1885         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
1886         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
1887         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
1888         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
1889         pwdinfo->negotiation_dialog_token = 1;  /*      Initialize the dialog value */
1890         pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &(pattrib->pktlen));
1891
1892
1893
1894         /*      WPS Section */
1895         wpsielen = 0;
1896         /*      WPS OUI */
1897         *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
1898         wpsielen += 4;
1899
1900         /*      WPS version */
1901         /*      Type: */
1902         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
1903         wpsielen += 2;
1904
1905         /*      Length: */
1906         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
1907         wpsielen += 2;
1908
1909         /*      Value: */
1910         wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */
1911
1912         /*      Device Password ID */
1913         /*      Type: */
1914         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
1915         wpsielen += 2;
1916
1917         /*      Length: */
1918         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
1919         wpsielen += 2;
1920
1921         /*      Value: */
1922
1923         if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN)
1924                 *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
1925         else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN)
1926                 *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
1927         else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC)
1928                 *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
1929
1930         wpsielen += 2;
1931
1932         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
1933
1934
1935         /*      P2P IE Section. */
1936
1937         /*      P2P OUI */
1938         p2pielen = 0;
1939         p2pie[p2pielen++] = 0x50;
1940         p2pie[p2pielen++] = 0x6F;
1941         p2pie[p2pielen++] = 0x9A;
1942         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
1943
1944         /*      Commented by Albert 20110306 */
1945         /*      According to the P2P Specification, the group negotiation request frame should contain 9 P2P attributes */
1946         /*      1. P2P Capability */
1947         /*      2. Group Owner Intent */
1948         /*      3. Configuration Timeout */
1949         /*      4. Listen Channel */
1950         /*      5. Extended Listen Timing */
1951         /*      6. Intended P2P Interface Address */
1952         /*      7. Channel List */
1953         /*      8. P2P Device Info */
1954         /*      9. Operating Channel */
1955
1956
1957         /*      P2P Capability */
1958         /*      Type: */
1959         p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
1960
1961         /*      Length: */
1962         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
1963         p2pielen += 2;
1964
1965         /*      Value: */
1966         /*      Device Capability Bitmap, 1 byte */
1967         p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
1968
1969         /*      Group Capability Bitmap, 1 byte */
1970         if (pwdinfo->persistent_supported)
1971                 p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
1972         else
1973                 p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
1974
1975         /*      Group Owner Intent */
1976         /*      Type: */
1977         p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
1978
1979         /*      Length: */
1980         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
1981         p2pielen += 2;
1982
1983         /*      Value: */
1984         /*      Todo the tie breaker bit. */
1985         p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
1986
1987         /*      Configuration Timeout */
1988         /*      Type: */
1989         p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
1990
1991         /*      Length: */
1992         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
1993         p2pielen += 2;
1994
1995         /*      Value: */
1996         p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P GO */
1997         p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P Client */
1998
1999
2000         /*      Listen Channel */
2001         /*      Type: */
2002         p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
2003
2004         /*      Length: */
2005         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
2006         p2pielen += 2;
2007
2008         /*      Value: */
2009         /*      Country String */
2010         p2pie[p2pielen++] = 'X';
2011         p2pie[p2pielen++] = 'X';
2012
2013         /*      The third byte should be set to 0x04. */
2014         /*      Described in the "Operating Channel Attribute" section. */
2015         p2pie[p2pielen++] = 0x04;
2016
2017         /*      Operating Class */
2018         p2pie[p2pielen++] = 0x51;       /*      Copy from SD7 */
2019
2020         /*      Channel Number */
2021         p2pie[p2pielen++] = pwdinfo->listen_channel;    /*      listening channel number */
2022
2023
2024         /*      Extended Listen Timing ATTR */
2025         /*      Type: */
2026         p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
2027
2028         /*      Length: */
2029         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
2030         p2pielen += 2;
2031
2032         /*      Value: */
2033         /*      Availability Period */
2034         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
2035         p2pielen += 2;
2036
2037         /*      Availability Interval */
2038         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
2039         p2pielen += 2;
2040
2041
2042         /*      Intended P2P Interface Address */
2043         /*      Type: */
2044         p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
2045
2046         /*      Length: */
2047         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
2048         p2pielen += 2;
2049
2050         /*      Value: */
2051         memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
2052         p2pielen += ETH_ALEN;
2053
2054
2055         /*      Channel List */
2056         /*      Type: */
2057         p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
2058
2059         /*  Length: */
2060         /*  Country String(3) */
2061         /*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
2062         /*  + number of channels in all classes */
2063         len_channellist_attr = 3
2064            + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
2065            + get_reg_classes_full_count(&pmlmeext->channel_list);
2066
2067         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
2068         p2pielen += 2;
2069
2070         /*      Value: */
2071         /*      Country String */
2072         p2pie[p2pielen++] = 'X';
2073         p2pie[p2pielen++] = 'X';
2074
2075         /*      The third byte should be set to 0x04. */
2076         /*      Described in the "Operating Channel Attribute" section. */
2077         p2pie[p2pielen++] = 0x04;
2078
2079         /*      Channel Entry List */
2080
2081         {
2082                 int i, j;
2083                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
2084                         /*      Operating Class */
2085                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
2086
2087                         /*      Number of Channels */
2088                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
2089
2090                         /*      Channel List */
2091                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
2092                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
2093                         }
2094                 }
2095         }
2096
2097         /*      Device Info */
2098         /*      Type: */
2099         p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
2100
2101         /*      Length: */
2102         /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
2103         /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
2104         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
2105         p2pielen += 2;
2106
2107         /*      Value: */
2108         /*      P2P Device Address */
2109         memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
2110         p2pielen += ETH_ALEN;
2111
2112         /*      Config Method */
2113         /*      This field should be big endian. Noted by P2P specification. */
2114
2115         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
2116
2117         p2pielen += 2;
2118
2119         /*      Primary Device Type */
2120         /*      Category ID */
2121         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
2122         p2pielen += 2;
2123
2124         /*      OUI */
2125         *(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
2126         p2pielen += 4;
2127
2128         /*      Sub Category ID */
2129         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
2130         p2pielen += 2;
2131
2132         /*      Number of Secondary Device Types */
2133         p2pie[p2pielen++] = 0x00;       /*      No Secondary Device Type List */
2134
2135         /*      Device Name */
2136         /*      Type: */
2137         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
2138         p2pielen += 2;
2139
2140         /*      Length: */
2141         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
2142         p2pielen += 2;
2143
2144         /*      Value: */
2145         memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len);
2146         p2pielen += pwdinfo->device_name_len;
2147
2148
2149         /*      Operating Channel */
2150         /*      Type: */
2151         p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
2152
2153         /*      Length: */
2154         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
2155         p2pielen += 2;
2156
2157         /*      Value: */
2158         /*      Country String */
2159         p2pie[p2pielen++] = 'X';
2160         p2pie[p2pielen++] = 'X';
2161
2162         /*      The third byte should be set to 0x04. */
2163         /*      Described in the "Operating Channel Attribute" section. */
2164         p2pie[p2pielen++] = 0x04;
2165
2166         /*      Operating Class */
2167         p2pie[p2pielen++] = 0x51;
2168
2169         /*      Channel Number */
2170         p2pie[p2pielen++] = pwdinfo->operating_channel; /*      operating channel number */
2171
2172         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
2173
2174         pattrib->last_txcmdsz = pattrib->pktlen;
2175
2176         dump_mgntframe(padapter, pmgntframe);
2177
2178         return;
2179 }
2180
2181 static void issue_p2p_GO_response(struct adapter *padapter, u8 *raddr, u8 *frame_body, uint len, u8 result)
2182 {
2183         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
2184         u8 action = P2P_PUB_ACTION_ACTION;
2185         __be32                  p2poui = cpu_to_be32(P2POUI);
2186         u8 oui_subtype = P2P_GO_NEGO_RESP;
2187         u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
2188         u8 p2pielen = 0;
2189         uint                    wpsielen = 0;
2190         u16 wps_devicepassword_id = 0x0000;
2191         __be16                  be_tmp;
2192         uint                    wps_devicepassword_id_len = 0;
2193         u16 len_channellist_attr = 0;
2194
2195         struct xmit_frame                       *pmgntframe;
2196         struct pkt_attrib                       *pattrib;
2197         unsigned char                                   *pframe;
2198         struct rtw_ieee80211_hdr        *pwlanhdr;
2199         __le16 *fctrl;
2200         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
2201         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
2202         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
2203
2204         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
2205         if (pmgntframe == NULL)
2206                 return;
2207
2208         DBG_88E("[%s] In, result=%d\n", __func__,  result);
2209         /* update attribute */
2210         pattrib = &pmgntframe->attrib;
2211         update_mgntframe_attrib(padapter, pattrib);
2212
2213         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2214
2215         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2216         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2217
2218         fctrl = &(pwlanhdr->frame_ctl);
2219         *(fctrl) = 0;
2220
2221         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
2222         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
2223         memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
2224
2225         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
2226         pmlmeext->mgnt_seq++;
2227         SetFrameSubType(pframe, WIFI_ACTION);
2228
2229         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
2230         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
2231
2232         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
2233         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
2234         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
2235         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
2236         pwdinfo->negotiation_dialog_token = frame_body[7];      /*      The Dialog Token of provisioning discovery request frame. */
2237         pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
2238
2239         /*      Commented by Albert 20110328 */
2240         /*      Try to get the device password ID from the WPS IE of group negotiation request frame */
2241         /*      WiFi Direct test plan 5.1.15 */
2242         rtw_get_wps_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
2243         rtw_get_wps_attr_content(wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, (u8 *)&be_tmp, &wps_devicepassword_id_len);
2244         wps_devicepassword_id = be16_to_cpu(be_tmp);
2245
2246         _rtw_memset(wpsie, 0x00, 255);
2247         wpsielen = 0;
2248
2249         /*      WPS Section */
2250         wpsielen = 0;
2251         /*      WPS OUI */
2252         *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
2253         wpsielen += 4;
2254
2255         /*      WPS version */
2256         /*      Type: */
2257         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
2258         wpsielen += 2;
2259
2260         /*      Length: */
2261         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
2262         wpsielen += 2;
2263
2264         /*      Value: */
2265         wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */
2266
2267         /*      Device Password ID */
2268         /*      Type: */
2269         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
2270         wpsielen += 2;
2271
2272         /*      Length: */
2273         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
2274         wpsielen += 2;
2275
2276         /*      Value: */
2277         if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
2278                 *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
2279         else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
2280                 *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
2281         else
2282                 *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
2283         wpsielen += 2;
2284
2285         /*      Commented by Kurt 20120113 */
2286         /*      If some device wants to do p2p handshake without sending prov_disc_req */
2287         /*      We have to get peer_req_cm from here. */
2288         if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) {
2289                 if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
2290                         memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
2291                 else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
2292                         memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
2293                 else
2294                         memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
2295         }
2296
2297         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
2298
2299
2300         /*      P2P IE Section. */
2301
2302         /*      P2P OUI */
2303         p2pielen = 0;
2304         p2pie[p2pielen++] = 0x50;
2305         p2pie[p2pielen++] = 0x6F;
2306         p2pie[p2pielen++] = 0x9A;
2307         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
2308
2309         /*      Commented by Albert 20100908 */
2310         /*      According to the P2P Specification, the group negotiation response frame should contain 9 P2P attributes */
2311         /*      1. Status */
2312         /*      2. P2P Capability */
2313         /*      3. Group Owner Intent */
2314         /*      4. Configuration Timeout */
2315         /*      5. Operating Channel */
2316         /*      6. Intended P2P Interface Address */
2317         /*      7. Channel List */
2318         /*      8. Device Info */
2319         /*      9. Group ID     (Only GO) */
2320
2321
2322         /*      ToDo: */
2323
2324         /*      P2P Status */
2325         /*      Type: */
2326         p2pie[p2pielen++] = P2P_ATTR_STATUS;
2327
2328         /*      Length: */
2329         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
2330         p2pielen += 2;
2331
2332         /*      Value: */
2333         p2pie[p2pielen++] = result;
2334
2335         /*      P2P Capability */
2336         /*      Type: */
2337         p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
2338
2339         /*      Length: */
2340         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
2341         p2pielen += 2;
2342
2343         /*      Value: */
2344         /*      Device Capability Bitmap, 1 byte */
2345
2346         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
2347                 /*      Commented by Albert 2011/03/08 */
2348                 /*      According to the P2P specification */
2349                 /*      if the sending device will be client, the P2P Capability should be reserved of group negotiation response frame */
2350                 p2pie[p2pielen++] = 0;
2351         } else {
2352                 /*      Be group owner or meet the error case */
2353                 p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
2354         }
2355
2356         /*      Group Capability Bitmap, 1 byte */
2357         if (pwdinfo->persistent_supported) {
2358                 p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
2359         } else {
2360                 p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
2361         }
2362
2363         /*      Group Owner Intent */
2364         /*      Type: */
2365         p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
2366
2367         /*      Length: */
2368         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
2369         p2pielen += 2;
2370
2371         /*      Value: */
2372         if (pwdinfo->peer_intent & 0x01) {
2373                 /*      Peer's tie breaker bit is 1, our tie breaker bit should be 0 */
2374                 p2pie[p2pielen++] = (pwdinfo->intent << 1);
2375         } else {
2376                 /*      Peer's tie breaker bit is 0, our tie breaker bit should be 1 */
2377                 p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
2378         }
2379
2380
2381         /*      Configuration Timeout */
2382         /*      Type: */
2383         p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
2384
2385         /*      Length: */
2386         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
2387         p2pielen += 2;
2388
2389         /*      Value: */
2390         p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P GO */
2391         p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P Client */
2392
2393         /*      Operating Channel */
2394         /*      Type: */
2395         p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
2396
2397         /*      Length: */
2398         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
2399         p2pielen += 2;
2400
2401         /*      Value: */
2402         /*      Country String */
2403         p2pie[p2pielen++] = 'X';
2404         p2pie[p2pielen++] = 'X';
2405
2406         /*      The third byte should be set to 0x04. */
2407         /*      Described in the "Operating Channel Attribute" section. */
2408         p2pie[p2pielen++] = 0x04;
2409
2410         /*      Operating Class */
2411         p2pie[p2pielen++] = 0x51;
2412
2413         /*      Channel Number */
2414         p2pie[p2pielen++] = pwdinfo->operating_channel; /*      operating channel number */
2415
2416         /*      Intended P2P Interface Address */
2417         /*      Type: */
2418         p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
2419
2420         /*      Length: */
2421         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
2422         p2pielen += 2;
2423
2424         /*      Value: */
2425         memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
2426         p2pielen += ETH_ALEN;
2427
2428         /*      Channel List */
2429         /*      Type: */
2430         p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
2431
2432         /*  Country String(3) */
2433         /*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
2434         /*  + number of channels in all classes */
2435         len_channellist_attr = 3
2436            + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
2437            + get_reg_classes_full_count(&pmlmeext->channel_list);
2438
2439         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
2440
2441         p2pielen += 2;
2442
2443         /*      Value: */
2444         /*      Country String */
2445         p2pie[p2pielen++] = 'X';
2446         p2pie[p2pielen++] = 'X';
2447
2448         /*      The third byte should be set to 0x04. */
2449         /*      Described in the "Operating Channel Attribute" section. */
2450         p2pie[p2pielen++] = 0x04;
2451
2452         /*      Channel Entry List */
2453
2454         {
2455                 int i, j;
2456                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
2457                         /*      Operating Class */
2458                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
2459
2460                         /*      Number of Channels */
2461                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
2462
2463                         /*      Channel List */
2464                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
2465                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
2466                         }
2467                 }
2468         }
2469
2470         /*      Device Info */
2471         /*      Type: */
2472         p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
2473
2474         /*      Length: */
2475         /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
2476         /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
2477         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
2478         p2pielen += 2;
2479
2480         /*      Value: */
2481         /*      P2P Device Address */
2482         memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
2483         p2pielen += ETH_ALEN;
2484
2485         /*      Config Method */
2486         /*      This field should be big endian. Noted by P2P specification. */
2487
2488         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
2489
2490         p2pielen += 2;
2491
2492         /*      Primary Device Type */
2493         /*      Category ID */
2494         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
2495         p2pielen += 2;
2496
2497         /*      OUI */
2498         *(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
2499         p2pielen += 4;
2500
2501         /*      Sub Category ID */
2502         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
2503         p2pielen += 2;
2504
2505         /*      Number of Secondary Device Types */
2506         p2pie[p2pielen++] = 0x00;       /*      No Secondary Device Type List */
2507
2508         /*      Device Name */
2509         /*      Type: */
2510         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
2511         p2pielen += 2;
2512
2513         /*      Length: */
2514         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
2515         p2pielen += 2;
2516
2517         /*      Value: */
2518         memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len);
2519         p2pielen += pwdinfo->device_name_len;
2520
2521         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
2522                 /*      Group ID Attribute */
2523                 /*      Type: */
2524                 p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
2525
2526                 /*      Length: */
2527                 *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
2528                 p2pielen += 2;
2529
2530                 /*      Value: */
2531                 /*      p2P Device Address */
2532                 memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN);
2533                 p2pielen += ETH_ALEN;
2534
2535                 /*      SSID */
2536                 memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
2537                 p2pielen += pwdinfo->nego_ssidlen;
2538         }
2539
2540         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
2541
2542         pattrib->last_txcmdsz = pattrib->pktlen;
2543
2544         dump_mgntframe(padapter, pmgntframe);
2545         return;
2546 }
2547
2548 static void issue_p2p_GO_confirm(struct adapter *padapter, u8 *raddr, u8 result)
2549 {
2550         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
2551         u8 action = P2P_PUB_ACTION_ACTION;
2552         __be32                  p2poui = cpu_to_be32(P2POUI);
2553         u8 oui_subtype = P2P_GO_NEGO_CONF;
2554         u8 p2pie[255] = { 0x00 };
2555         u8 p2pielen = 0;
2556
2557         struct xmit_frame                       *pmgntframe;
2558         struct pkt_attrib                       *pattrib;
2559         unsigned char                                   *pframe;
2560         struct rtw_ieee80211_hdr        *pwlanhdr;
2561         __le16 *fctrl;
2562         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
2563         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
2564         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
2565
2566         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
2567         if (pmgntframe == NULL)
2568                 return;
2569
2570         DBG_88E("[%s] In\n", __func__);
2571         /* update attribute */
2572         pattrib = &pmgntframe->attrib;
2573         update_mgntframe_attrib(padapter, pattrib);
2574
2575         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2576
2577         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2578         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2579
2580         fctrl = &(pwlanhdr->frame_ctl);
2581         *(fctrl) = 0;
2582
2583         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
2584         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
2585         memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
2586
2587         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
2588         pmlmeext->mgnt_seq++;
2589         SetFrameSubType(pframe, WIFI_ACTION);
2590
2591         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
2592         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
2593
2594         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
2595         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
2596         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
2597         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
2598         pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
2599
2600
2601
2602         /*      P2P IE Section. */
2603
2604         /*      P2P OUI */
2605         p2pielen = 0;
2606         p2pie[p2pielen++] = 0x50;
2607         p2pie[p2pielen++] = 0x6F;
2608         p2pie[p2pielen++] = 0x9A;
2609         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
2610
2611         /*      Commented by Albert 20110306 */
2612         /*      According to the P2P Specification, the group negotiation request frame should contain 5 P2P attributes */
2613         /*      1. Status */
2614         /*      2. P2P Capability */
2615         /*      3. Operating Channel */
2616         /*      4. Channel List */
2617         /*      5. Group ID     (if this WiFi is GO) */
2618
2619         /*      P2P Status */
2620         /*      Type: */
2621         p2pie[p2pielen++] = P2P_ATTR_STATUS;
2622
2623         /*      Length: */
2624         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
2625         p2pielen += 2;
2626
2627         /*      Value: */
2628         p2pie[p2pielen++] = result;
2629
2630         /*      P2P Capability */
2631         /*      Type: */
2632         p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
2633
2634         /*      Length: */
2635         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
2636         p2pielen += 2;
2637
2638         /*      Value: */
2639         /*      Device Capability Bitmap, 1 byte */
2640         p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
2641
2642         /*      Group Capability Bitmap, 1 byte */
2643         if (pwdinfo->persistent_supported)
2644                 p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
2645         else
2646                 p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
2647
2648         /*      Operating Channel */
2649         /*      Type: */
2650         p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
2651
2652         /*      Length: */
2653         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
2654         p2pielen += 2;
2655
2656         /*      Value: */
2657         /*      Country String */
2658         p2pie[p2pielen++] = 'X';
2659         p2pie[p2pielen++] = 'X';
2660
2661         /*      The third byte should be set to 0x04. */
2662         /*      Described in the "Operating Channel Attribute" section. */
2663         p2pie[p2pielen++] = 0x04;
2664
2665
2666         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
2667                 /*      Operating Class */
2668                 p2pie[p2pielen++] = 0x51;
2669                 p2pie[p2pielen++] = pwdinfo->peer_operating_ch;
2670         } else {
2671                 /*      Operating Class */
2672                 p2pie[p2pielen++] = 0x51;
2673
2674                 /*      Channel Number */
2675                 p2pie[p2pielen++] = pwdinfo->operating_channel;         /*      Use the listen channel as the operating channel */
2676         }
2677
2678
2679         /*      Channel List */
2680         /*      Type: */
2681         p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
2682
2683         /*      Length: */
2684         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(pwdinfo->channel_list_attr_len);
2685         p2pielen += 2;
2686
2687         /*      Value: */
2688         memcpy(p2pie + p2pielen, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len);
2689         p2pielen += pwdinfo->channel_list_attr_len;
2690
2691         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
2692                 /*      Group ID Attribute */
2693                 /*      Type: */
2694                 p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
2695
2696                 /*      Length: */
2697                 *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
2698                 p2pielen += 2;
2699
2700                 /*      Value: */
2701                 /*      p2P Device Address */
2702                 memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN);
2703                 p2pielen += ETH_ALEN;
2704
2705                 /*      SSID */
2706                 memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
2707                 p2pielen += pwdinfo->nego_ssidlen;
2708         }
2709         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
2710         pattrib->last_txcmdsz = pattrib->pktlen;
2711         dump_mgntframe(padapter, pmgntframe);
2712         return;
2713 }
2714
2715 void issue_p2p_invitation_request(struct adapter *padapter, u8 *raddr)
2716 {
2717         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
2718         u8 action = P2P_PUB_ACTION_ACTION;
2719         __be32                  p2poui = cpu_to_be32(P2POUI);
2720         u8 oui_subtype = P2P_INVIT_REQ;
2721         u8 p2pie[255] = { 0x00 };
2722         u8 p2pielen = 0;
2723         u8 dialogToken = 3;
2724         u16 len_channellist_attr = 0;
2725         struct xmit_frame                       *pmgntframe;
2726         struct pkt_attrib                       *pattrib;
2727         unsigned char                                   *pframe;
2728         struct rtw_ieee80211_hdr        *pwlanhdr;
2729         __le16 *fctrl;
2730         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
2731         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
2732         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
2733
2734         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
2735         if (pmgntframe == NULL)
2736                 return;
2737
2738         /* update attribute */
2739         pattrib = &pmgntframe->attrib;
2740         update_mgntframe_attrib(padapter, pattrib);
2741
2742         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2743
2744         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2745         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2746
2747         fctrl = &(pwlanhdr->frame_ctl);
2748         *(fctrl) = 0;
2749
2750         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
2751         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
2752         memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
2753
2754         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
2755         pmlmeext->mgnt_seq++;
2756         SetFrameSubType(pframe, WIFI_ACTION);
2757
2758         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
2759         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
2760
2761         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
2762         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
2763         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
2764         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
2765         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
2766
2767         /*      P2P IE Section. */
2768
2769         /*      P2P OUI */
2770         p2pielen = 0;
2771         p2pie[p2pielen++] = 0x50;
2772         p2pie[p2pielen++] = 0x6F;
2773         p2pie[p2pielen++] = 0x9A;
2774         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
2775
2776         /*      Commented by Albert 20101011 */
2777         /*      According to the P2P Specification, the P2P Invitation request frame should contain 7 P2P attributes */
2778         /*      1. Configuration Timeout */
2779         /*      2. Invitation Flags */
2780         /*      3. Operating Channel    (Only GO) */
2781         /*      4. P2P Group BSSID      (Should be included if I am the GO) */
2782         /*      5. Channel List */
2783         /*      6. P2P Group ID */
2784         /*      7. P2P Device Info */
2785
2786         /*      Configuration Timeout */
2787         /*      Type: */
2788         p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
2789
2790         /*      Length: */
2791         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
2792         p2pielen += 2;
2793
2794         /*      Value: */
2795         p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P GO */
2796         p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P Client */
2797
2798         /*      Invitation Flags */
2799         /*      Type: */
2800         p2pie[p2pielen++] = P2P_ATTR_INVITATION_FLAGS;
2801
2802         /*      Length: */
2803         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
2804         p2pielen += 2;
2805
2806         /*      Value: */
2807         p2pie[p2pielen++] = P2P_INVITATION_FLAGS_PERSISTENT;
2808
2809
2810         /*      Operating Channel */
2811         /*      Type: */
2812         p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
2813
2814         /*      Length: */
2815         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
2816         p2pielen += 2;
2817
2818         /*      Value: */
2819         /*      Country String */
2820         p2pie[p2pielen++] = 'X';
2821         p2pie[p2pielen++] = 'X';
2822
2823         /*      The third byte should be set to 0x04. */
2824         /*      Described in the "Operating Channel Attribute" section. */
2825         p2pie[p2pielen++] = 0x04;
2826
2827         /*      Operating Class */
2828         p2pie[p2pielen++] = 0x51;
2829
2830         /*      Channel Number */
2831         p2pie[p2pielen++] = pwdinfo->invitereq_info.operating_ch;       /*      operating channel number */
2832
2833         if (!memcmp(myid(&padapter->eeprompriv),
2834                     pwdinfo->invitereq_info.go_bssid, ETH_ALEN)) {
2835                 /*      P2P Group BSSID */
2836                 /*      Type: */
2837                 p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
2838
2839                 /*      Length: */
2840                 *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
2841                 p2pielen += 2;
2842
2843                 /*      Value: */
2844                 /*      P2P Device Address for GO */
2845                 memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
2846                 p2pielen += ETH_ALEN;
2847         }
2848
2849         /*      Channel List */
2850         /*      Type: */
2851         p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
2852
2853
2854         /*      Length: */
2855         /*  Country String(3) */
2856         /*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
2857         /*  + number of channels in all classes */
2858         len_channellist_attr = 3
2859            + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
2860            + get_reg_classes_full_count(&pmlmeext->channel_list);
2861
2862         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
2863
2864         p2pielen += 2;
2865
2866         /*      Value: */
2867         /*      Country String */
2868         p2pie[p2pielen++] = 'X';
2869         p2pie[p2pielen++] = 'X';
2870
2871         /*      The third byte should be set to 0x04. */
2872         /*      Described in the "Operating Channel Attribute" section. */
2873         p2pie[p2pielen++] = 0x04;
2874
2875         /*      Channel Entry List */
2876         {
2877                 int i, j;
2878                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
2879                         /*      Operating Class */
2880                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
2881
2882                         /*      Number of Channels */
2883                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
2884
2885                         /*      Channel List */
2886                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
2887                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
2888                         }
2889                 }
2890         }
2891
2892
2893         /*      P2P Group ID */
2894         /*      Type: */
2895         p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
2896
2897         /*      Length: */
2898         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(6 + pwdinfo->invitereq_info.ssidlen);
2899         p2pielen += 2;
2900
2901         /*      Value: */
2902         /*      P2P Device Address for GO */
2903         memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
2904         p2pielen += ETH_ALEN;
2905
2906         /*      SSID */
2907         memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid, pwdinfo->invitereq_info.ssidlen);
2908         p2pielen += pwdinfo->invitereq_info.ssidlen;
2909
2910
2911         /*      Device Info */
2912         /*      Type: */
2913         p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
2914
2915         /*      Length: */
2916         /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
2917         /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
2918         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
2919         p2pielen += 2;
2920
2921         /*      Value: */
2922         /*      P2P Device Address */
2923         memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
2924         p2pielen += ETH_ALEN;
2925
2926         /*      Config Method */
2927         /*      This field should be big endian. Noted by P2P specification. */
2928         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
2929         p2pielen += 2;
2930
2931         /*      Primary Device Type */
2932         /*      Category ID */
2933         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
2934         p2pielen += 2;
2935
2936         /*      OUI */
2937         *(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
2938         p2pielen  += 4;
2939
2940         /*      Sub Category ID */
2941         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
2942         p2pielen += 2;
2943
2944         /*      Number of Secondary Device Types */
2945         p2pie[p2pielen++] = 0x00;       /*      No Secondary Device Type List */
2946
2947         /*      Device Name */
2948         /*      Type: */
2949         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
2950         p2pielen += 2;
2951
2952         /*      Length: */
2953         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
2954         p2pielen += 2;
2955
2956         /*      Value: */
2957         memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
2958         p2pielen += pwdinfo->device_name_len;
2959
2960         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
2961
2962         pattrib->last_txcmdsz = pattrib->pktlen;
2963
2964         dump_mgntframe(padapter, pmgntframe);
2965
2966         return;
2967 }
2968
2969 void issue_p2p_invitation_response(struct adapter *padapter, u8 *raddr, u8 dialogToken, u8 status_code)
2970 {
2971         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
2972         u8 action = P2P_PUB_ACTION_ACTION;
2973         __be32                  p2poui = cpu_to_be32(P2POUI);
2974         u8 oui_subtype = P2P_INVIT_RESP;
2975         u8 p2pie[255] = { 0x00 };
2976         u8 p2pielen = 0;
2977         u16 len_channellist_attr = 0;
2978         struct xmit_frame                       *pmgntframe;
2979         struct pkt_attrib                       *pattrib;
2980         unsigned char                                   *pframe;
2981         struct rtw_ieee80211_hdr        *pwlanhdr;
2982         __le16 *fctrl;
2983         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
2984         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
2985         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
2986
2987         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
2988         if (pmgntframe == NULL)
2989                 return;
2990
2991         /* update attribute */
2992         pattrib = &pmgntframe->attrib;
2993         update_mgntframe_attrib(padapter, pattrib);
2994
2995         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2996
2997         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2998         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2999
3000         fctrl = &(pwlanhdr->frame_ctl);
3001         *(fctrl) = 0;
3002
3003         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
3004         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
3005         memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
3006
3007         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3008         pmlmeext->mgnt_seq++;
3009         SetFrameSubType(pframe, WIFI_ACTION);
3010
3011         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
3012         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3013
3014         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
3015         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
3016         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
3017         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3018         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
3019
3020         /*      P2P IE Section. */
3021
3022         /*      P2P OUI */
3023         p2pielen = 0;
3024         p2pie[p2pielen++] = 0x50;
3025         p2pie[p2pielen++] = 0x6F;
3026         p2pie[p2pielen++] = 0x9A;
3027         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
3028
3029         /*      Commented by Albert 20101005 */
3030         /*      According to the P2P Specification, the P2P Invitation response frame should contain 5 P2P attributes */
3031         /*      1. Status */
3032         /*      2. Configuration Timeout */
3033         /*      3. Operating Channel    (Only GO) */
3034         /*      4. P2P Group BSSID      (Only GO) */
3035         /*      5. Channel List */
3036
3037         /*      P2P Status */
3038         /*      Type: */
3039         p2pie[p2pielen++] = P2P_ATTR_STATUS;
3040
3041         /*      Length: */
3042         *(__le16  *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
3043         p2pielen += 2;
3044
3045         /*      Value: */
3046         /*      When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE. */
3047         /*      Sent the event receiving the P2P Invitation Req frame to DMP UI. */
3048         /*      DMP had to compare the MAC address to find out the profile. */
3049         /*      So, the WiFi driver will send the P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB. */
3050         /*      If the UI found the corresponding profile, the WiFi driver sends the P2P Invitation Req */
3051         /*      to NB to rebuild the persistent group. */
3052         p2pie[p2pielen++] = status_code;
3053
3054         /*      Configuration Timeout */
3055         /*      Type: */
3056         p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
3057
3058         /*      Length: */
3059         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
3060         p2pielen += 2;
3061
3062         /*      Value: */
3063         p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P GO */
3064         p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P Client */
3065
3066         if (status_code == P2P_STATUS_SUCCESS) {
3067                 if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
3068                         /*      The P2P Invitation request frame asks this Wi-Fi device to be the P2P GO */
3069                         /*      In this case, the P2P Invitation response frame should carry the two more P2P attributes. */
3070                         /*      First one is operating channel attribute. */
3071                         /*      Second one is P2P Group BSSID attribute. */
3072
3073                         /*      Operating Channel */
3074                         /*      Type: */
3075                         p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
3076
3077                         /*      Length: */
3078                         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
3079                         p2pielen += 2;
3080
3081                         /*      Value: */
3082                         /*      Country String */
3083                         p2pie[p2pielen++] = 'X';
3084                         p2pie[p2pielen++] = 'X';
3085
3086                         /*      The third byte should be set to 0x04. */
3087                         /*      Described in the "Operating Channel Attribute" section. */
3088                         p2pie[p2pielen++] = 0x04;
3089
3090                         /*      Operating Class */
3091                         p2pie[p2pielen++] = 0x51;       /*      Copy from SD7 */
3092
3093                         /*      Channel Number */
3094                         p2pie[p2pielen++] = pwdinfo->operating_channel; /*      operating channel number */
3095
3096
3097                         /*      P2P Group BSSID */
3098                         /*      Type: */
3099                         p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
3100
3101                         /*      Length: */
3102                         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
3103                         p2pielen += 2;
3104
3105                         /*      Value: */
3106                         /*      P2P Device Address for GO */
3107                         memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
3108                         p2pielen += ETH_ALEN;
3109                 }
3110
3111                 /*      Channel List */
3112                 /*      Type: */
3113                 p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
3114
3115                 /*      Length: */
3116                 /*  Country String(3) */
3117                 /*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
3118                 /*  + number of channels in all classes */
3119                 len_channellist_attr = 3
3120                         + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
3121                         + get_reg_classes_full_count(&pmlmeext->channel_list);
3122
3123                 *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
3124                 p2pielen += 2;
3125
3126                 /*      Value: */
3127                 /*      Country String */
3128                 p2pie[p2pielen++] = 'X';
3129                 p2pie[p2pielen++] = 'X';
3130
3131                 /*      The third byte should be set to 0x04. */
3132                 /*      Described in the "Operating Channel Attribute" section. */
3133                 p2pie[p2pielen++] = 0x04;
3134
3135                 /*      Channel Entry List */
3136                 {
3137                         int i, j;
3138                         for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3139                                 /*      Operating Class */
3140                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3141
3142                                 /*      Number of Channels */
3143                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3144
3145                                 /*      Channel List */
3146                                 for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3147                                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3148                                 }
3149                         }
3150                 }
3151         }
3152
3153         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
3154
3155         pattrib->last_txcmdsz = pattrib->pktlen;
3156
3157         dump_mgntframe(padapter, pmgntframe);
3158
3159         return;
3160 }
3161
3162 void issue_p2p_provision_request(struct adapter *padapter, u8 *pssid, u8 ussidlen, u8 *pdev_raddr)
3163 {
3164         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3165         u8 action = P2P_PUB_ACTION_ACTION;
3166         u8 dialogToken = 1;
3167         u8 oui_subtype = P2P_PROVISION_DISC_REQ;
3168         u8 wpsie[100] = { 0x00 };
3169         u8 wpsielen = 0;
3170         __be32 p2poui = cpu_to_be32(P2POUI);
3171         u32                     p2pielen = 0;
3172         struct xmit_frame                       *pmgntframe;
3173         struct pkt_attrib                       *pattrib;
3174         unsigned char                                   *pframe;
3175         struct rtw_ieee80211_hdr        *pwlanhdr;
3176         __le16 *fctrl;
3177         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3178         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3179         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
3180
3181         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
3182         if (pmgntframe == NULL)
3183                 return;
3184
3185         DBG_88E("[%s] In\n", __func__);
3186         /* update attribute */
3187         pattrib = &pmgntframe->attrib;
3188         update_mgntframe_attrib(padapter, pattrib);
3189
3190         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3191
3192         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3193         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3194
3195         fctrl = &(pwlanhdr->frame_ctl);
3196         *(fctrl) = 0;
3197
3198         memcpy(pwlanhdr->addr1, pdev_raddr, ETH_ALEN);
3199         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
3200         memcpy(pwlanhdr->addr3, pdev_raddr, ETH_ALEN);
3201
3202         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3203         pmlmeext->mgnt_seq++;
3204         SetFrameSubType(pframe, WIFI_ACTION);
3205
3206         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
3207         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3208
3209         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
3210         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
3211         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
3212         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3213         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
3214
3215         p2pielen = build_prov_disc_request_p2p_ie(pwdinfo, pframe, pssid, ussidlen, pdev_raddr);
3216
3217         pframe += p2pielen;
3218         pattrib->pktlen += p2pielen;
3219
3220         wpsielen = 0;
3221         /*      WPS OUI */
3222         *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
3223         wpsielen += 4;
3224
3225         /*      WPS version */
3226         /*      Type: */
3227         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
3228         wpsielen += 2;
3229
3230         /*      Length: */
3231         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
3232         wpsielen += 2;
3233
3234         /*      Value: */
3235         wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */
3236
3237         /*      Config Method */
3238         /*      Type: */
3239         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
3240         wpsielen += 2;
3241
3242         /*      Length: */
3243         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
3244         wpsielen += 2;
3245
3246         /*      Value: */
3247         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->tx_prov_disc_info.wps_config_method_request);
3248         wpsielen += 2;
3249
3250         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
3251
3252         pattrib->last_txcmdsz = pattrib->pktlen;
3253
3254         dump_mgntframe(padapter, pmgntframe);
3255
3256         return;
3257 }
3258
3259 static u8 is_matched_in_profilelist(u8 *peermacaddr, struct profile_info *profileinfo)
3260 {
3261         u8 i, match_result = 0;
3262
3263         DBG_88E("[%s] peermac=%.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
3264                 peermacaddr[0], peermacaddr[1], peermacaddr[2], peermacaddr[3], peermacaddr[4], peermacaddr[5]);
3265
3266         for (i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++) {
3267                DBG_88E("[%s] profileinfo_mac=%.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
3268                             profileinfo->peermac[0], profileinfo->peermac[1], profileinfo->peermac[2], profileinfo->peermac[3], profileinfo->peermac[4], profileinfo->peermac[5]);
3269                 if (!memcmp(peermacaddr, profileinfo->peermac, ETH_ALEN)) {
3270                         match_result = 1;
3271                         DBG_88E("[%s] Match!\n", __func__);
3272                         break;
3273                 }
3274         }
3275         return match_result;
3276 }
3277
3278 void issue_probersp_p2p(struct adapter *padapter, unsigned char *da)
3279 {
3280         struct xmit_frame                       *pmgntframe;
3281         struct pkt_attrib                       *pattrib;
3282         unsigned char                                   *pframe;
3283         struct rtw_ieee80211_hdr        *pwlanhdr;
3284         __le16 *fctrl;
3285         unsigned char                                   *mac;
3286         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
3287         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3288         u16 beacon_interval = 100;
3289         u16 capInfo = 0;
3290         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
3291         u8 wpsie[255] = { 0x00 };
3292         u32                                     wpsielen = 0, p2pielen = 0;
3293
3294         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
3295         if (pmgntframe == NULL)
3296                 return;
3297
3298         /* update attribute */
3299         pattrib = &pmgntframe->attrib;
3300         update_mgntframe_attrib(padapter, pattrib);
3301
3302         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3303
3304         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3305         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3306
3307         mac = myid(&(padapter->eeprompriv));
3308
3309         fctrl = &(pwlanhdr->frame_ctl);
3310         *(fctrl) = 0;
3311         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
3312         memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
3313
3314         /*      Use the device address for BSSID field. */
3315         memcpy(pwlanhdr->addr3, mac, ETH_ALEN);
3316
3317         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3318         pmlmeext->mgnt_seq++;
3319         SetFrameSubType(fctrl, WIFI_PROBERSP);
3320
3321         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3322         pattrib->pktlen = pattrib->hdrlen;
3323         pframe += pattrib->hdrlen;
3324
3325         /* timestamp will be inserted by hardware */
3326         pframe += 8;
3327         pattrib->pktlen += 8;
3328
3329         /*  beacon interval: 2 bytes */
3330         memcpy(pframe, (unsigned char *)&beacon_interval, 2);
3331         pframe += 2;
3332         pattrib->pktlen += 2;
3333
3334         /*      capability info: 2 bytes */
3335         /*      ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of WiFi Direct Spec) */
3336         capInfo |= cap_ShortPremble;
3337         capInfo |= cap_ShortSlot;
3338
3339         memcpy(pframe, (unsigned char *)&capInfo, 2);
3340         pframe += 2;
3341         pattrib->pktlen += 2;
3342
3343
3344         /*  SSID */
3345         pframe = rtw_set_ie(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid, &pattrib->pktlen);
3346
3347         /*  supported rates... */
3348         /*      Use the OFDM rate in the P2P probe response frame. (6(B), 9(B), 12, 18, 24, 36, 48, 54) */
3349         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
3350
3351         /*  DS parameter set */
3352         pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&pwdinfo->listen_channel, &pattrib->pktlen);
3353
3354         /*      Todo: WPS IE */
3355         /*      Noted by Albert 20100907 */
3356         /*      According to the WPS specification, all the WPS attribute is presented by Big Endian. */
3357
3358         wpsielen = 0;
3359         /*      WPS OUI */
3360         *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
3361         wpsielen += 4;
3362
3363         /*      WPS version */
3364         /*      Type: */
3365         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
3366         wpsielen += 2;
3367
3368         /*      Length: */
3369         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
3370         wpsielen += 2;
3371
3372         /*      Value: */
3373         wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */
3374
3375         /*      WiFi Simple Config State */
3376         /*      Type: */
3377         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SIMPLE_CONF_STATE);
3378         wpsielen += 2;
3379
3380         /*      Length: */
3381         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
3382         wpsielen += 2;
3383
3384         /*      Value: */
3385         wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG;   /*      Not Configured. */
3386
3387         /*      Response Type */
3388         /*      Type: */
3389         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_RESP_TYPE);
3390         wpsielen += 2;
3391
3392         /*      Length: */
3393         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
3394         wpsielen += 2;
3395
3396         /*      Value: */
3397         wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
3398
3399         /*      UUID-E */
3400         /*      Type: */
3401         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
3402         wpsielen += 2;
3403
3404         /*      Length: */
3405         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010);
3406         wpsielen += 2;
3407
3408         /*      Value: */
3409         memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), ETH_ALEN);
3410         wpsielen += 0x10;
3411
3412         /*      Manufacturer */
3413         /*      Type: */
3414         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MANUFACTURER);
3415         wpsielen += 2;
3416
3417         /*      Length: */
3418         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0007);
3419         wpsielen += 2;
3420
3421         /*      Value: */
3422         memcpy(wpsie + wpsielen, "Realtek", 7);
3423         wpsielen += 7;
3424
3425         /*      Model Name */
3426         /*      Type: */
3427         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NAME);
3428         wpsielen += 2;
3429
3430         /*      Length: */
3431         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0006);
3432         wpsielen += 2;
3433
3434         /*      Value: */
3435         memcpy(wpsie + wpsielen, "8188EU", 6);
3436         wpsielen += 6;
3437
3438         /*      Model Number */
3439         /*      Type: */
3440         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NUMBER);
3441         wpsielen += 2;
3442
3443         /*      Length: */
3444         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
3445         wpsielen += 2;
3446
3447         /*      Value: */
3448         wpsie[wpsielen++] = 0x31;               /*      character 1 */
3449
3450         /*      Serial Number */
3451         /*      Type: */
3452         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SERIAL_NUMBER);
3453         wpsielen += 2;
3454
3455         /*      Length: */
3456         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(ETH_ALEN);
3457         wpsielen += 2;
3458
3459         /*      Value: */
3460         memcpy(wpsie + wpsielen, "123456" , ETH_ALEN);
3461         wpsielen += ETH_ALEN;
3462
3463         /*      Primary Device Type */
3464         /*      Type: */
3465         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
3466         wpsielen += 2;
3467
3468         /*      Length: */
3469         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008);
3470         wpsielen += 2;
3471
3472         /*      Value: */
3473         /*      Category ID */
3474         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
3475         wpsielen += 2;
3476
3477         /*      OUI */
3478         *(__be32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI);
3479         wpsielen += 4;
3480
3481         /*      Sub Category ID */
3482         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
3483         wpsielen += 2;
3484
3485         /*      Device Name */
3486         /*      Type: */
3487         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
3488         wpsielen += 2;
3489
3490         /*      Length: */
3491         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len);
3492         wpsielen += 2;
3493
3494         /*      Value: */
3495         if (pwdinfo->device_name_len) {
3496                 memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len);
3497                 wpsielen += pwdinfo->device_name_len;
3498         }
3499
3500         /*      Config Method */
3501         /*      Type: */
3502         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
3503         wpsielen += 2;
3504
3505         /*      Length: */
3506         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
3507         wpsielen += 2;
3508
3509         /*      Value: */
3510         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
3511         wpsielen += 2;
3512
3513
3514         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
3515
3516
3517         p2pielen = build_probe_resp_p2p_ie(pwdinfo, pframe);
3518         pframe += p2pielen;
3519         pattrib->pktlen += p2pielen;
3520
3521         pattrib->last_txcmdsz = pattrib->pktlen;
3522
3523         dump_mgntframe(padapter, pmgntframe);
3524
3525         return;
3526 }
3527
3528 static int _issue_probereq_p2p(struct adapter *padapter, u8 *da, int wait_ack)
3529 {
3530         int ret = _FAIL;
3531         struct xmit_frame               *pmgntframe;
3532         struct pkt_attrib               *pattrib;
3533         unsigned char                   *pframe;
3534         struct rtw_ieee80211_hdr        *pwlanhdr;
3535         __le16 *fctrl;
3536         unsigned char                   *mac;
3537         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
3538         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3539         u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
3540         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
3541         u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
3542         u16 wpsielen = 0, p2pielen = 0;
3543         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
3544
3545         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
3546         if (pmgntframe == NULL)
3547                 goto exit;
3548
3549         /* update attribute */
3550         pattrib = &pmgntframe->attrib;
3551         update_mgntframe_attrib(padapter, pattrib);
3552
3553
3554         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3555
3556         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3557         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3558
3559         mac = myid(&(padapter->eeprompriv));
3560
3561         fctrl = &(pwlanhdr->frame_ctl);
3562         *(fctrl) = 0;
3563
3564         if (da) {
3565                 memcpy(pwlanhdr->addr1, da, ETH_ALEN);
3566                 memcpy(pwlanhdr->addr3, da, ETH_ALEN);
3567         } else {
3568                 if ((pwdinfo->p2p_info.scan_op_ch_only) || (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
3569                         /*      This two flags will be set when this is only the P2P client mode. */
3570                         memcpy(pwlanhdr->addr1, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
3571                         memcpy(pwlanhdr->addr3, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
3572                 } else {
3573                         /*      broadcast probe request frame */
3574                         memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
3575                         memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
3576                 }
3577         }
3578         memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
3579
3580         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3581         pmlmeext->mgnt_seq++;
3582         SetFrameSubType(pframe, WIFI_PROBEREQ);
3583
3584         pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
3585         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
3586
3587         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
3588                 pframe = rtw_set_ie(pframe, _SSID_IE_, pwdinfo->tx_prov_disc_info.ssid.SsidLength, pwdinfo->tx_prov_disc_info.ssid.Ssid, &(pattrib->pktlen));
3589         else
3590                 pframe = rtw_set_ie(pframe, _SSID_IE_, P2P_WILDCARD_SSID_LEN, pwdinfo->p2p_wildcard_ssid, &(pattrib->pktlen));
3591
3592         /*      Use the OFDM rate in the P2P probe request frame. (6(B), 9(B), 12(B), 24(B), 36, 48, 54) */
3593         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
3594
3595
3596         /*      WPS IE */
3597         /*      Noted by Albert 20110221 */
3598         /*      According to the WPS specification, all the WPS attribute is presented by Big Endian. */
3599
3600         wpsielen = 0;
3601         /*      WPS OUI */
3602         *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
3603         wpsielen += 4;
3604
3605         /*      WPS version */
3606         /*      Type: */
3607         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
3608         wpsielen += 2;
3609
3610         /*      Length: */
3611         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
3612         wpsielen += 2;
3613
3614         /*      Value: */
3615         wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */
3616
3617         if (pmlmepriv->wps_probe_req_ie == NULL) {
3618                 /*      UUID-E */
3619                 /*      Type: */
3620                 *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
3621                 wpsielen += 2;
3622
3623                 /*      Length: */
3624                 *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010);
3625                 wpsielen += 2;
3626
3627                 /*      Value: */
3628                 memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), ETH_ALEN);
3629                 wpsielen += 0x10;
3630
3631                 /*      Config Method */
3632                 /*      Type: */
3633                 *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
3634                 wpsielen += 2;
3635
3636                 /*      Length: */
3637                 *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
3638                 wpsielen += 2;
3639
3640                 /*      Value: */
3641                 *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
3642                 wpsielen += 2;
3643         }
3644
3645         /*      Device Name */
3646         /*      Type: */
3647         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
3648         wpsielen += 2;
3649
3650         /*      Length: */
3651         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len);
3652         wpsielen += 2;
3653
3654         /*      Value: */
3655         memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len);
3656         wpsielen += pwdinfo->device_name_len;
3657
3658         /*      Primary Device Type */
3659         /*      Type: */
3660         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
3661         wpsielen += 2;
3662
3663         /*      Length: */
3664         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008);
3665         wpsielen += 2;
3666
3667         /*      Value: */
3668         /*      Category ID */
3669         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_RTK_WIDI);
3670         wpsielen += 2;
3671
3672         /*      OUI */
3673         *(__be32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI);
3674         wpsielen += 4;
3675
3676         /*      Sub Category ID */
3677         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_RTK_DMP);
3678         wpsielen += 2;
3679
3680         /*      Device Password ID */
3681         /*      Type: */
3682         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
3683         wpsielen += 2;
3684
3685         /*      Length: */
3686         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
3687         wpsielen += 2;
3688
3689         /*      Value: */
3690         *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);   /*      Registrar-specified */
3691         wpsielen += 2;
3692
3693         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
3694
3695         /*      P2P OUI */
3696         p2pielen = 0;
3697         p2pie[p2pielen++] = 0x50;
3698         p2pie[p2pielen++] = 0x6F;
3699         p2pie[p2pielen++] = 0x9A;
3700         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
3701
3702         /*      Commented by Albert 20110221 */
3703         /*      According to the P2P Specification, the probe request frame should contain 5 P2P attributes */
3704         /*      1. P2P Capability */
3705         /*      2. P2P Device ID if this probe request wants to find the specific P2P device */
3706         /*      3. Listen Channel */
3707         /*      4. Extended Listen Timing */
3708         /*      5. Operating Channel if this WiFi is working as the group owner now */
3709
3710         /*      P2P Capability */
3711         /*      Type: */
3712         p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
3713
3714         /*      Length: */
3715         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
3716         p2pielen += 2;
3717
3718         /*      Value: */
3719         /*      Device Capability Bitmap, 1 byte */
3720         p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
3721
3722         /*      Group Capability Bitmap, 1 byte */
3723         if (pwdinfo->persistent_supported)
3724                 p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
3725         else
3726                 p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
3727
3728         /*      Listen Channel */
3729         /*      Type: */
3730         p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
3731
3732         /*      Length: */
3733         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
3734         p2pielen += 2;
3735
3736         /*      Value: */
3737         /*      Country String */
3738         p2pie[p2pielen++] = 'X';
3739         p2pie[p2pielen++] = 'X';
3740
3741         /*      The third byte should be set to 0x04. */
3742         /*      Described in the "Operating Channel Attribute" section. */
3743         p2pie[p2pielen++] = 0x04;
3744
3745         /*      Operating Class */
3746         p2pie[p2pielen++] = 0x51;       /*      Copy from SD7 */
3747
3748         /*      Channel Number */
3749         p2pie[p2pielen++] = pwdinfo->listen_channel;    /*      listen channel */
3750
3751
3752         /*      Extended Listen Timing */
3753         /*      Type: */
3754         p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
3755
3756         /*      Length: */
3757         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
3758         p2pielen += 2;
3759
3760         /*      Value: */
3761         /*      Availability Period */
3762         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
3763         p2pielen += 2;
3764
3765         /*      Availability Interval */
3766         *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
3767         p2pielen += 2;
3768
3769         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
3770                 /*      Operating Channel (if this WiFi is working as the group owner now) */
3771                 /*      Type: */
3772                 p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
3773
3774                 /*      Length: */
3775                 *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
3776                 p2pielen += 2;
3777
3778                 /*      Value: */
3779                 /*      Country String */
3780                 p2pie[p2pielen++] = 'X';
3781                 p2pie[p2pielen++] = 'X';
3782
3783                 /*      The third byte should be set to 0x04. */
3784                 /*      Described in the "Operating Channel Attribute" section. */
3785                 p2pie[p2pielen++] = 0x04;
3786
3787                 /*      Operating Class */
3788                 p2pie[p2pielen++] = 0x51;       /*      Copy from SD7 */
3789
3790                 /*      Channel Number */
3791                 p2pie[p2pielen++] = pwdinfo->operating_channel; /*      operating channel number */
3792         }
3793
3794         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
3795
3796         if (pmlmepriv->wps_probe_req_ie != NULL) {
3797                 /* WPS IE */
3798                 memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
3799                 pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
3800                 pframe += pmlmepriv->wps_probe_req_ie_len;
3801         }
3802
3803         pattrib->last_txcmdsz = pattrib->pktlen;
3804
3805         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
3806
3807         if (wait_ack) {
3808                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
3809         } else {
3810                 dump_mgntframe(padapter, pmgntframe);
3811                 ret = _SUCCESS;
3812         }
3813
3814 exit:
3815         return ret;
3816 }
3817
3818 inline void issue_probereq_p2p(struct adapter *adapter, u8 *da)
3819 {
3820         _issue_probereq_p2p(adapter, da, false);
3821 }
3822
3823 int issue_probereq_p2p_ex(struct adapter *adapter, u8 *da, int try_cnt, int wait_ms)
3824 {
3825         int ret;
3826         int i = 0;
3827         u32 start = jiffies;
3828
3829         do {
3830                 ret = _issue_probereq_p2p(adapter, da, wait_ms > 0 ? true : false);
3831
3832                 i++;
3833
3834                 if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
3835                         break;
3836
3837                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
3838                         msleep(wait_ms);
3839         } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
3840
3841         if (ret != _FAIL) {
3842                 ret = _SUCCESS;
3843                 goto exit;
3844         }
3845
3846         if (try_cnt && wait_ms) {
3847                 if (da)
3848                         DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
3849                                 FUNC_ADPT_ARG(adapter), da, rtw_get_oper_ch(adapter),
3850                                 ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
3851                 else
3852                         DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
3853                                 FUNC_ADPT_ARG(adapter), rtw_get_oper_ch(adapter),
3854                                 ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
3855         }
3856 exit:
3857         return ret;
3858 }
3859
3860 #endif /* CONFIG_88EU_P2P */
3861
3862 static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
3863 {
3864         struct adapter *adapter = recv_frame->adapter;
3865         struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
3866         u8 *frame = recv_frame->rx_data;
3867         u16 seq_ctrl = ((recv_frame->attrib.seq_num&0xffff) << 4) |
3868                 (recv_frame->attrib.frag_num & 0xf);
3869
3870         if (GetRetry(frame)) {
3871                 if (token >= 0) {
3872                         if ((seq_ctrl == mlmeext->action_public_rxseq) && (token == mlmeext->action_public_dialog_token)) {
3873                                 DBG_88E(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x, token:%d\n",
3874                                         FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
3875                                 return _FAIL;
3876                         }
3877                 } else {
3878                         if (seq_ctrl == mlmeext->action_public_rxseq) {
3879                                 DBG_88E(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x\n",
3880                                         FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq);
3881                                 return _FAIL;
3882                         }
3883                 }
3884         }
3885
3886         mlmeext->action_public_rxseq = seq_ctrl;
3887
3888         if (token >= 0)
3889                 mlmeext->action_public_dialog_token = token;
3890
3891         return _SUCCESS;
3892 }
3893
3894 static unsigned int on_action_public_p2p(struct recv_frame *precv_frame)
3895 {
3896         u8 *pframe = precv_frame->rx_data;
3897         u8 *frame_body;
3898         u8 dialogToken = 0;
3899 #ifdef CONFIG_88EU_P2P
3900         struct adapter *padapter = precv_frame->adapter;
3901         uint len = precv_frame->len;
3902         u8 *p2p_ie;
3903         u32     p2p_ielen;
3904         struct  wifidirect_info *pwdinfo = &(padapter->wdinfo);
3905         u8      result = P2P_STATUS_SUCCESS;
3906         u8      empty_addr[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
3907 #endif /* CONFIG_88EU_P2P */
3908
3909         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
3910
3911         dialogToken = frame_body[7];
3912
3913         if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
3914                 return _FAIL;
3915
3916 #ifdef CONFIG_88EU_P2P
3917         _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
3918         /*      Do nothing if the driver doesn't enable the P2P function. */
3919         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
3920                 return _SUCCESS;
3921
3922         len -= sizeof(struct rtw_ieee80211_hdr_3addr);
3923
3924         switch (frame_body[6]) { /* OUI Subtype */
3925         case P2P_GO_NEGO_REQ:
3926                 DBG_88E("[%s] Got GO Nego Req Frame\n", __func__);
3927                 _rtw_memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
3928
3929                 if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
3930                         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
3931
3932                 if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL)) {
3933                         /*      Commented by Albert 20110526 */
3934                         /*      In this case, this means the previous nego fail doesn't be reset yet. */
3935                         _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
3936                         /*      Restore the previous p2p state */
3937                         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
3938                         DBG_88E("[%s] Restore the previous p2p state to %d\n", __func__, rtw_p2p_state(pwdinfo));
3939                 }
3940
3941                 /*      Commented by Kurt 20110902 */
3942                 /* Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
3943                 if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
3944                         rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
3945
3946                 /*      Commented by Kurt 20120113 */
3947                 /*      Get peer_dev_addr here if peer doesn't issue prov_disc frame. */
3948                 if (!memcmp(pwdinfo->rx_prov_disc_info.peerDevAddr, empty_addr,
3949                             ETH_ALEN))
3950                         memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
3951
3952                 result = process_p2p_group_negotation_req(pwdinfo, frame_body, len);
3953                 issue_p2p_GO_response(padapter, GetAddr2Ptr(pframe), frame_body, len, result);
3954
3955                 /*      Commented by Albert 20110718 */
3956                 /*      No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer. */
3957                 _set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
3958                 break;
3959         case P2P_GO_NEGO_RESP:
3960                 DBG_88E("[%s] Got GO Nego Resp Frame\n", __func__);
3961
3962                 if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
3963                         /*      Commented by Albert 20110425 */
3964                         /*      The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function. */
3965                         _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
3966                         pwdinfo->nego_req_info.benable = false;
3967                         result = process_p2p_group_negotation_resp(pwdinfo, frame_body, len);
3968                         issue_p2p_GO_confirm(pwdinfo->padapter, GetAddr2Ptr(pframe), result);
3969                         if (P2P_STATUS_SUCCESS == result) {
3970                                 if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT) {
3971                                         pwdinfo->p2p_info.operation_ch[0] = pwdinfo->peer_operating_ch;
3972                                         pwdinfo->p2p_info.scan_op_ch_only = 1;
3973                                         _set_timer(&pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH);
3974                                 }
3975                         }
3976                         /*      Reset the dialog token for group negotiation frames. */
3977                         pwdinfo->negotiation_dialog_token = 1;
3978                         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
3979                                 _set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
3980                 } else {
3981                         DBG_88E("[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __func__);
3982                 }
3983                 break;
3984         case P2P_GO_NEGO_CONF:
3985                 DBG_88E("[%s] Got GO Nego Confirm Frame\n", __func__);
3986                 result = process_p2p_group_negotation_confirm(pwdinfo, frame_body, len);
3987                 if (P2P_STATUS_SUCCESS == result) {
3988                         if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT) {
3989                                 pwdinfo->p2p_info.operation_ch[0] = pwdinfo->peer_operating_ch;
3990                                 pwdinfo->p2p_info.scan_op_ch_only = 1;
3991                                 _set_timer(&pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH);
3992                         }
3993                 }
3994                 break;
3995         case P2P_INVIT_REQ:
3996                 /*      Added by Albert 2010/10/05 */
3997                 /*      Received the P2P Invite Request frame. */
3998
3999                 DBG_88E("[%s] Got invite request frame!\n", __func__);
4000                 p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
4001                 if (p2p_ie) {
4002                         /*      Parse the necessary information from the P2P Invitation Request frame. */
4003                         /*      For example: The MAC address of sending this P2P Invitation Request frame. */
4004                         u32     attr_contentlen = 0;
4005                         u8      status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
4006                         struct group_id_info group_id;
4007                         u8      invitation_flag = 0;
4008
4009                         rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen);
4010                         if (attr_contentlen) {
4011                                 rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen);
4012                                 /*      Commented by Albert 20120510 */
4013                                 /*      Copy to the pwdinfo->p2p_peer_interface_addr. */
4014                                 /*      So that the WFD UI (or Sigma) can get the peer interface address by using the following command. */
4015                                 /*      #> iwpriv wlan0 p2p_get peer_ifa */
4016                                 /*      After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant. */
4017
4018                                 if (attr_contentlen) {
4019                                         DBG_88E("[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
4020                                                 pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1],
4021                                                 pwdinfo->p2p_peer_interface_addr[2], pwdinfo->p2p_peer_interface_addr[3],
4022                                                 pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5]);
4023                                 }
4024
4025                                 if (invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT) {
4026                                         /*      Re-invoke the persistent group. */
4027
4028                                         _rtw_memset(&group_id, 0x00, sizeof(struct group_id_info));
4029                                         rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8 *)&group_id, &attr_contentlen);
4030                                         if (attr_contentlen) {
4031                                                 if (!memcmp(group_id.go_device_addr, myid(&padapter->eeprompriv), ETH_ALEN)) {
4032                                                         /*      The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO. */
4033                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO);
4034                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
4035                                                         status_code = P2P_STATUS_SUCCESS;
4036                                                 } else {
4037                                                         /*      The p2p device sending this p2p invitation request wants to be the persistent GO. */
4038                                                         if (is_matched_in_profilelist(pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[0])) {
4039                                                                 u8 operatingch_info[5] = { 0x00 };
4040                                                                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) {
4041                                                                         if (rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, (u32)operatingch_info[4])) {
4042                                                                                 /*      The operating channel is acceptable for this device. */
4043                                                                                 pwdinfo->rx_invitereq_info.operation_ch[0] = operatingch_info[4];
4044                                                                                 pwdinfo->rx_invitereq_info.scan_op_ch_only = 1;
4045                                                                                 _set_timer(&pwdinfo->reset_ch_sitesurvey, P2P_RESET_SCAN_CH);
4046                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
4047                                                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
4048                                                                                 status_code = P2P_STATUS_SUCCESS;
4049                                                                         } else {
4050                                                                                 /*      The operating channel isn't supported by this device. */
4051                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
4052                                                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
4053                                                                                 status_code = P2P_STATUS_FAIL_NO_COMMON_CH;
4054                                                                                 _set_timer(&pwdinfo->restore_p2p_state_timer, 3000);
4055                                                                         }
4056                                                                 } else {
4057                                                                         /*      Commented by Albert 20121130 */
4058                                                                         /*      Intel will use the different P2P IE to store the operating channel information */
4059                                                                         /*      Workaround for Intel WiDi 3.5 */
4060                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
4061                                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
4062                                                                         status_code = P2P_STATUS_SUCCESS;
4063                                                                 }
4064                                                         } else {
4065                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
4066                                                                 status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
4067                                                         }
4068                                                 }
4069                                         } else {
4070                                                 DBG_88E("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
4071                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
4072                                         }
4073                                 } else {
4074                                         /*      Received the invitation to join a P2P group. */
4075
4076                                         _rtw_memset(&group_id, 0x00, sizeof(struct group_id_info));
4077                                         rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8 *)&group_id, &attr_contentlen);
4078                                         if (attr_contentlen) {
4079                                                 if (!memcmp(group_id.go_device_addr, myid(&padapter->eeprompriv), ETH_ALEN)) {
4080                                                         /*      In this case, the GO can't be myself. */
4081                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
4082                                                         status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
4083                                                 } else {
4084                                                         /*      The p2p device sending this p2p invitation request wants to join an existing P2P group */
4085                                                         /*      Commented by Albert 2012/06/28 */
4086                                                         /*      In this case, this Wi-Fi device should use the iwpriv command to get the peer device address. */
4087                                                         /*      The peer device address should be the destination address for the provisioning discovery request. */
4088                                                         /*      Then, this Wi-Fi device should use the iwpriv command to get the peer interface address. */
4089                                                         /*      The peer interface address should be the address for WPS mac address */
4090                                                         memcpy(pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN);
4091                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
4092                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN);
4093                                                         status_code = P2P_STATUS_SUCCESS;
4094                                                 }
4095                                         } else {
4096                                                 DBG_88E("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
4097                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
4098                                         }
4099                                 }
4100                         } else {
4101                                 DBG_88E("[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __func__);
4102                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
4103                         }
4104
4105                         DBG_88E("[%s] status_code = %d\n", __func__, status_code);
4106
4107                         pwdinfo->inviteresp_info.token = frame_body[7];
4108                         issue_p2p_invitation_response(padapter, GetAddr2Ptr(pframe), pwdinfo->inviteresp_info.token, status_code);
4109                 }
4110                 break;
4111         case P2P_INVIT_RESP: {
4112                 u8      attr_content = 0x00;
4113                 u32     attr_contentlen = 0;
4114
4115                 DBG_88E("[%s] Got invite response frame!\n", __func__);
4116                 _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
4117                 p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
4118                 if (p2p_ie) {
4119                         rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
4120
4121                         if (attr_contentlen == 1) {
4122                                 DBG_88E("[%s] Status = %d\n", __func__, attr_content);
4123                                 pwdinfo->invitereq_info.benable = false;
4124
4125                                 if (attr_content == P2P_STATUS_SUCCESS) {
4126                                         if (!memcmp(pwdinfo->invitereq_info.go_bssid, myid(&padapter->eeprompriv), ETH_ALEN)) {
4127                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
4128                                         } else {
4129                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
4130                                         }
4131                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_OK);
4132                                 } else {
4133                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
4134                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
4135                                 }
4136                         } else {
4137                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
4138                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
4139                         }
4140                 } else {
4141                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
4142                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
4143                 }
4144
4145                 if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL))
4146                         _set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
4147                 break;
4148         }
4149         case P2P_DEVDISC_REQ:
4150                 process_p2p_devdisc_req(pwdinfo, pframe, len);
4151                 break;
4152         case P2P_DEVDISC_RESP:
4153                 process_p2p_devdisc_resp(pwdinfo, pframe, len);
4154                 break;
4155         case P2P_PROVISION_DISC_REQ:
4156                 DBG_88E("[%s] Got Provisioning Discovery Request Frame\n", __func__);
4157                 process_p2p_provdisc_req(pwdinfo, pframe, len);
4158                 memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
4159
4160                 /* 20110902 Kurt */
4161                 /* Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
4162                 if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
4163                         rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
4164
4165                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ);
4166                 _set_timer(&pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT);
4167                 break;
4168         case P2P_PROVISION_DISC_RESP:
4169                 /*      Commented by Albert 20110707 */
4170                 /*      Should we check the pwdinfo->tx_prov_disc_info.bsent flag here?? */
4171                 DBG_88E("[%s] Got Provisioning Discovery Response Frame\n", __func__);
4172                 /*      Commented by Albert 20110426 */
4173                 /*      The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function. */
4174                 _cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
4175                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP);
4176                 process_p2p_provdisc_resp(pwdinfo, pframe);
4177                 _set_timer(&pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT);
4178                 break;
4179         }
4180 #endif /* CONFIG_88EU_P2P */
4181
4182         return _SUCCESS;
4183 }
4184
4185 static unsigned int on_action_public_vendor(struct recv_frame *precv_frame)
4186 {
4187         unsigned int ret = _FAIL;
4188         u8 *pframe = precv_frame->rx_data;
4189         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
4190
4191         if (!memcmp(frame_body + 2, P2P_OUI, 4))
4192                 ret = on_action_public_p2p(precv_frame);
4193
4194         return ret;
4195 }
4196
4197 static unsigned int on_action_public_default(struct recv_frame *precv_frame, u8 action)
4198 {
4199         unsigned int ret = _FAIL;
4200         u8 *pframe = precv_frame->rx_data;
4201         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
4202         u8 token;
4203
4204         token = frame_body[2];
4205
4206         if (rtw_action_public_decache(precv_frame, token) == _FAIL)
4207                 goto exit;
4208
4209         ret = _SUCCESS;
4210
4211 exit:
4212         return ret;
4213 }
4214
4215 unsigned int on_action_public(struct adapter *padapter, struct recv_frame *precv_frame)
4216 {
4217         unsigned int ret = _FAIL;
4218         u8 *pframe = precv_frame->rx_data;
4219         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
4220         u8 category, action;
4221
4222         /* check RA matches or not */
4223         if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
4224                 goto exit;
4225
4226         category = frame_body[0];
4227         if (category != RTW_WLAN_CATEGORY_PUBLIC)
4228                 goto exit;
4229
4230         action = frame_body[1];
4231         switch (action) {
4232         case ACT_PUBLIC_VENDOR:
4233                 ret = on_action_public_vendor(precv_frame);
4234                 break;
4235         default:
4236                 ret = on_action_public_default(precv_frame, action);
4237                 break;
4238         }
4239
4240 exit:
4241         return ret;
4242 }
4243
4244 unsigned int OnAction_ht(struct adapter *padapter, struct recv_frame *precv_frame)
4245 {
4246         return _SUCCESS;
4247 }
4248
4249 unsigned int OnAction_wmm(struct adapter *padapter, struct recv_frame *precv_frame)
4250 {
4251         return _SUCCESS;
4252 }
4253
4254 unsigned int OnAction_p2p(struct adapter *padapter, struct recv_frame *precv_frame)
4255 {
4256 #ifdef CONFIG_88EU_P2P
4257         u8 *frame_body;
4258         u8 category, OUI_Subtype;
4259         u8 *pframe = precv_frame->rx_data;
4260         uint len = precv_frame->len;
4261         struct  wifidirect_info *pwdinfo = &(padapter->wdinfo);
4262
4263
4264         DBG_88E("%s\n", __func__);
4265
4266         /* check RA matches or not */
4267         if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))/* for if1, sta/ap mode */
4268                 return _SUCCESS;
4269
4270         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
4271
4272         category = frame_body[0];
4273         if (category != RTW_WLAN_CATEGORY_P2P)
4274                 return _SUCCESS;
4275
4276         if (be32_to_cpu(*((__be32 *)(frame_body + 1))) != P2POUI)
4277                 return _SUCCESS;
4278
4279         len -= sizeof(struct rtw_ieee80211_hdr_3addr);
4280         OUI_Subtype = frame_body[5];
4281
4282         switch (OUI_Subtype) {
4283         case P2P_NOTICE_OF_ABSENCE:
4284                 break;
4285         case P2P_PRESENCE_REQUEST:
4286                 process_p2p_presence_req(pwdinfo, pframe, len);
4287                 break;
4288         case P2P_PRESENCE_RESPONSE:
4289                 break;
4290         case P2P_GO_DISC_REQUEST:
4291                 break;
4292         default:
4293                 break;
4294         }
4295 #endif /* CONFIG_88EU_P2P */
4296         return _SUCCESS;
4297 }
4298
4299 unsigned int OnAction(struct adapter *padapter, struct recv_frame *precv_frame)
4300 {
4301         int i;
4302         unsigned char   category;
4303         struct action_handler *ptable;
4304         unsigned char   *frame_body;
4305         u8 *pframe = precv_frame->rx_data;
4306
4307         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
4308
4309         category = frame_body[0];
4310
4311         for (i = 0; i < sizeof(OnAction_tbl)/sizeof(struct action_handler); i++) {
4312                 ptable = &OnAction_tbl[i];
4313                 if (category == ptable->num)
4314                         ptable->func(padapter, precv_frame);
4315         }
4316         return _SUCCESS;
4317 }
4318
4319 unsigned int DoReserved(struct adapter *padapter, struct recv_frame *precv_frame)
4320 {
4321         return _SUCCESS;
4322 }
4323
4324 struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
4325 {
4326         struct xmit_frame                       *pmgntframe;
4327         struct xmit_buf                         *pxmitbuf;
4328
4329         pmgntframe = rtw_alloc_xmitframe(pxmitpriv);
4330         if (pmgntframe == NULL) {
4331                 DBG_88E("%s, alloc xmitframe fail\n", __func__);
4332                 return NULL;
4333         }
4334
4335         pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv);
4336         if (pxmitbuf == NULL) {
4337                 DBG_88E("%s, alloc xmitbuf fail\n", __func__);
4338                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
4339                 return NULL;
4340         }
4341         pmgntframe->frame_tag = MGNT_FRAMETAG;
4342         pmgntframe->pxmitbuf = pxmitbuf;
4343         pmgntframe->buf_addr = pxmitbuf->pbuf;
4344         pxmitbuf->priv_data = pmgntframe;
4345         return pmgntframe;
4346 }
4347
4348 /****************************************************************************
4349
4350 Following are some TX functions for WiFi MLME
4351
4352 *****************************************************************************/
4353
4354 void update_mgnt_tx_rate(struct adapter *padapter, u8 rate)
4355 {
4356         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4357
4358         pmlmeext->tx_rate = rate;
4359         DBG_88E("%s(): rate = %x\n", __func__, rate);
4360 }
4361
4362 void update_mgntframe_attrib(struct adapter *padapter, struct pkt_attrib *pattrib)
4363 {
4364         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4365
4366         _rtw_memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib));
4367
4368         pattrib->hdrlen = 24;
4369         pattrib->nr_frags = 1;
4370         pattrib->priority = 7;
4371         pattrib->mac_id = 0;
4372         pattrib->qsel = 0x12;
4373
4374         pattrib->pktlen = 0;
4375
4376         if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
4377                 pattrib->raid = 6;/* b mode */
4378         else
4379                 pattrib->raid = 5;/* a/g mode */
4380
4381         pattrib->encrypt = _NO_PRIVACY_;
4382         pattrib->bswenc = false;
4383
4384         pattrib->qos_en = false;
4385         pattrib->ht_en = false;
4386         pattrib->bwmode = HT_CHANNEL_WIDTH_20;
4387         pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
4388         pattrib->sgi = false;
4389
4390         pattrib->seqnum = pmlmeext->mgnt_seq;
4391
4392         pattrib->retry_ctrl = true;
4393 }
4394
4395 void dump_mgntframe(struct adapter *padapter, struct xmit_frame *pmgntframe)
4396 {
4397         if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
4398                 return;
4399
4400         rtw_hal_mgnt_xmit(padapter, pmgntframe);
4401 }
4402
4403 s32 dump_mgntframe_and_wait(struct adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
4404 {
4405         s32 ret = _FAIL;
4406         struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
4407         struct submit_ctx sctx;
4408
4409         if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
4410                 return ret;
4411
4412         rtw_sctx_init(&sctx, timeout_ms);
4413         pxmitbuf->sctx = &sctx;
4414
4415         ret = rtw_hal_mgnt_xmit(padapter, pmgntframe);
4416
4417         if (ret == _SUCCESS)
4418                 ret = rtw_sctx_wait(&sctx);
4419
4420         return ret;
4421 }
4422
4423 s32 dump_mgntframe_and_wait_ack(struct adapter *padapter, struct xmit_frame *pmgntframe)
4424 {
4425         s32 ret = _FAIL;
4426         u32 timeout_ms = 500;/*   500ms */
4427         struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
4428
4429         if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
4430                 return -1;
4431
4432         _enter_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
4433         pxmitpriv->ack_tx = true;
4434
4435         pmgntframe->ack_report = 1;
4436         if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS) {
4437                 ret = rtw_ack_tx_wait(pxmitpriv, timeout_ms);
4438         }
4439
4440         pxmitpriv->ack_tx = false;
4441         mutex_unlock(&pxmitpriv->ack_tx_mutex);
4442
4443          return ret;
4444 }
4445
4446 static int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
4447 {
4448         u8 *ssid_ie;
4449         int ssid_len_ori;
4450         int len_diff = 0;
4451
4452         ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
4453
4454         if (ssid_ie && ssid_len_ori > 0) {
4455                 switch (hidden_ssid_mode) {
4456                 case 1: {
4457                         u8 *next_ie = ssid_ie + 2 + ssid_len_ori;
4458                         u32 remain_len = 0;
4459
4460                         remain_len = ies_len - (next_ie - ies);
4461
4462                         ssid_ie[1] = 0;
4463                         memcpy(ssid_ie+2, next_ie, remain_len);
4464                         len_diff -= ssid_len_ori;
4465
4466                         break;
4467                 }
4468                 case 2:
4469                         _rtw_memset(&ssid_ie[2], 0, ssid_len_ori);
4470                         break;
4471                 default:
4472                         break;
4473                 }
4474         }
4475
4476         return len_diff;
4477 }
4478
4479 void issue_beacon(struct adapter *padapter, int timeout_ms)
4480 {
4481         struct xmit_frame       *pmgntframe;
4482         struct pkt_attrib       *pattrib;
4483         unsigned char   *pframe;
4484         struct rtw_ieee80211_hdr *pwlanhdr;
4485         __le16 *fctrl;
4486         unsigned int    rate_len;
4487         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
4488         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
4489         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4490         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4491         struct wlan_bssid_ex            *cur_network = &(pmlmeinfo->network);
4492         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
4493 #ifdef CONFIG_88EU_P2P
4494         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
4495 #endif /* CONFIG_88EU_P2P */
4496
4497         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
4498         if (pmgntframe == NULL) {
4499                 DBG_88E("%s, alloc mgnt frame fail\n", __func__);
4500                 return;
4501         }
4502 #if defined (CONFIG_88EU_AP_MODE)
4503         spin_lock_bh(&pmlmepriv->bcn_update_lock);
4504 #endif /* if defined (CONFIG_88EU_AP_MODE) */
4505
4506         /* update attribute */
4507         pattrib = &pmgntframe->attrib;
4508         update_mgntframe_attrib(padapter, pattrib);
4509         pattrib->qsel = 0x10;
4510
4511         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4512
4513         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4514         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4515
4516
4517         fctrl = &(pwlanhdr->frame_ctl);
4518         *(fctrl) = 0;
4519
4520         memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
4521         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
4522         memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
4523
4524         SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
4525         /* pmlmeext->mgnt_seq++; */
4526         SetFrameSubType(pframe, WIFI_BEACON);
4527
4528         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4529         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
4530
4531         if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
4532 #ifdef CONFIG_88EU_P2P
4533                 /*  for P2P : Primary Device Type & Device Name */
4534                 u32 wpsielen = 0, insert_len = 0;
4535                 u8 *wpsie = NULL;
4536                 wpsie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wpsielen);
4537
4538                 if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wpsie && wpsielen > 0) {
4539                         uint wps_offset, remainder_ielen;
4540                         u8 *premainder_ie, *pframe_wscie;
4541
4542                         wps_offset = (uint)(wpsie - cur_network->IEs);
4543                         premainder_ie = wpsie + wpsielen;
4544                         remainder_ielen = cur_network->IELength - wps_offset - wpsielen;
4545                         pframe_wscie = pframe + wps_offset;
4546                         memcpy(pframe, cur_network->IEs, wps_offset+wpsielen);
4547                         pframe += (wps_offset + wpsielen);
4548                         pattrib->pktlen += (wps_offset + wpsielen);
4549
4550                         /* now pframe is end of wsc ie, insert Primary Device Type & Device Name */
4551                         /*      Primary Device Type */
4552                         /*      Type: */
4553                         *(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
4554                         insert_len += 2;
4555
4556                         /*      Length: */
4557                         *(__be16 *)(pframe + insert_len) = cpu_to_be16(0x0008);
4558                         insert_len += 2;
4559
4560                         /*      Value: */
4561                         /*      Category ID */
4562                         *(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
4563                         insert_len += 2;
4564
4565                         /*      OUI */
4566                         *(__be32 *)(pframe + insert_len) = cpu_to_be32(WPSOUI);
4567                         insert_len += 4;
4568
4569                         /*      Sub Category ID */
4570                         *(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
4571                         insert_len += 2;
4572
4573                         /*      Device Name */
4574                         /*      Type: */
4575                         *(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
4576                         insert_len += 2;
4577
4578                         /*      Length: */
4579                         *(__be16 *)(pframe + insert_len) = cpu_to_be16(pwdinfo->device_name_len);
4580                         insert_len += 2;
4581
4582                         /*      Value: */
4583                         memcpy(pframe + insert_len, pwdinfo->device_name, pwdinfo->device_name_len);
4584                         insert_len += pwdinfo->device_name_len;
4585
4586                         /* update wsc ie length */
4587                         *(pframe_wscie+1) = (wpsielen-2) + insert_len;
4588
4589                         /* pframe move to end */
4590                         pframe += insert_len;
4591                         pattrib->pktlen += insert_len;
4592
4593                         /* copy remainder_ie to pframe */
4594                         memcpy(pframe, premainder_ie, remainder_ielen);
4595                         pframe += remainder_ielen;
4596                         pattrib->pktlen += remainder_ielen;
4597                 } else
4598 #endif /* CONFIG_88EU_P2P */
4599                 {
4600                         int len_diff;
4601                         memcpy(pframe, cur_network->IEs, cur_network->IELength);
4602                         len_diff = update_hidden_ssid(
4603                                 pframe+_BEACON_IE_OFFSET_
4604                                 , cur_network->IELength-_BEACON_IE_OFFSET_
4605                                 , pmlmeinfo->hidden_ssid_mode
4606                         );
4607                         pframe += (cur_network->IELength+len_diff);
4608                         pattrib->pktlen += (cur_network->IELength+len_diff);
4609                 }
4610
4611                 {
4612                         u8 *wps_ie;
4613                         uint wps_ielen;
4614                         u8 sr = 0;
4615                         wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr+TXDESC_OFFSET+sizeof (struct rtw_ieee80211_hdr_3addr)+_BEACON_IE_OFFSET_,
4616                                 pattrib->pktlen-sizeof (struct rtw_ieee80211_hdr_3addr)-_BEACON_IE_OFFSET_, NULL, &wps_ielen);
4617                         if (wps_ie && wps_ielen > 0)
4618                                 rtw_get_wps_attr_content(wps_ie,  wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8 *)(&sr), NULL);
4619                         if (sr != 0)
4620                                 set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
4621                         else
4622                                 _clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
4623                 }
4624
4625 #ifdef CONFIG_88EU_P2P
4626                 if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
4627                         u32 len;
4628                         len = build_beacon_p2p_ie(pwdinfo, pframe);
4629
4630                         pframe += len;
4631                         pattrib->pktlen += len;
4632                 }
4633 #endif /* CONFIG_88EU_P2P */
4634
4635                 goto _issue_bcn;
4636         }
4637
4638         /* below for ad-hoc mode */
4639
4640         /* timestamp will be inserted by hardware */
4641         pframe += 8;
4642         pattrib->pktlen += 8;
4643
4644         /*  beacon interval: 2 bytes */
4645
4646         memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
4647
4648         pframe += 2;
4649         pattrib->pktlen += 2;
4650
4651         /*  capability info: 2 bytes */
4652
4653         memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
4654
4655         pframe += 2;
4656         pattrib->pktlen += 2;
4657
4658         /*  SSID */
4659         pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
4660
4661         /*  supported rates... */
4662         rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
4663         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen);
4664
4665         /*  DS parameter set */
4666         pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
4667
4668         {
4669                 u8 erpinfo = 0;
4670                 u32 ATIMWindow;
4671                 /*  IBSS Parameter Set... */
4672                 ATIMWindow = 0;
4673                 pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
4674
4675                 /* ERP IE */
4676                 pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
4677         }
4678
4679         /*  EXTERNDED SUPPORTED RATE */
4680         if (rate_len > 8)
4681                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
4682         /* todo:HT for adhoc */
4683 _issue_bcn:
4684
4685 #if defined (CONFIG_88EU_AP_MODE)
4686         pmlmepriv->update_bcn = false;
4687
4688         spin_unlock_bh(&pmlmepriv->bcn_update_lock);
4689 #endif /* if defined (CONFIG_88EU_AP_MODE) */
4690
4691         if ((pattrib->pktlen + TXDESC_SIZE) > 512) {
4692                 DBG_88E("beacon frame too large\n");
4693                 return;
4694         }
4695
4696         pattrib->last_txcmdsz = pattrib->pktlen;
4697
4698         /* DBG_88E("issue bcn_sz=%d\n", pattrib->last_txcmdsz); */
4699         if (timeout_ms > 0)
4700                 dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
4701         else
4702                 dump_mgntframe(padapter, pmgntframe);
4703 }
4704
4705 void issue_probersp(struct adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
4706 {
4707         struct xmit_frame                       *pmgntframe;
4708         struct pkt_attrib                       *pattrib;
4709         unsigned char                                   *pframe;
4710         struct rtw_ieee80211_hdr        *pwlanhdr;
4711         __le16 *fctrl;
4712         unsigned char                                   *mac, *bssid;
4713         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
4714 #if defined (CONFIG_88EU_AP_MODE)
4715         u8 *pwps_ie;
4716         uint wps_ielen;
4717         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
4718 #endif /* if defined (CONFIG_88EU_AP_MODE) */
4719         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4720         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4721         struct wlan_bssid_ex            *cur_network = &(pmlmeinfo->network);
4722         unsigned int    rate_len;
4723 #ifdef CONFIG_88EU_P2P
4724         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
4725 #endif /* CONFIG_88EU_P2P */
4726
4727         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
4728         if (pmgntframe == NULL) {
4729                 DBG_88E("%s, alloc mgnt frame fail\n", __func__);
4730                 return;
4731         }
4732
4733         /* update attribute */
4734         pattrib = &pmgntframe->attrib;
4735         update_mgntframe_attrib(padapter, pattrib);
4736
4737         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4738
4739         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4740         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4741
4742         mac = myid(&(padapter->eeprompriv));
4743         bssid = cur_network->MacAddress;
4744
4745         fctrl = &(pwlanhdr->frame_ctl);
4746         *(fctrl) = 0;
4747         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
4748         memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
4749         memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
4750
4751         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4752         pmlmeext->mgnt_seq++;
4753         SetFrameSubType(fctrl, WIFI_PROBERSP);
4754
4755         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4756         pattrib->pktlen = pattrib->hdrlen;
4757         pframe += pattrib->hdrlen;
4758
4759         if (cur_network->IELength > MAX_IE_SZ)
4760                 return;
4761
4762 #if defined(CONFIG_88EU_AP_MODE)
4763         if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
4764                 pwps_ie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
4765
4766                 /* inerset & update wps_probe_resp_ie */
4767                 if ((pmlmepriv->wps_probe_resp_ie != NULL) && pwps_ie && (wps_ielen > 0)) {
4768                         uint wps_offset, remainder_ielen;
4769                         u8 *premainder_ie;
4770
4771                         wps_offset = (uint)(pwps_ie - cur_network->IEs);
4772
4773                         premainder_ie = pwps_ie + wps_ielen;
4774
4775                         remainder_ielen = cur_network->IELength - wps_offset - wps_ielen;
4776
4777                         memcpy(pframe, cur_network->IEs, wps_offset);
4778                         pframe += wps_offset;
4779                         pattrib->pktlen += wps_offset;
4780
4781                         wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];/* to get ie data len */
4782                         if ((wps_offset+wps_ielen+2) <= MAX_IE_SZ) {
4783                                 memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen+2);
4784                                 pframe += wps_ielen+2;
4785                                 pattrib->pktlen += wps_ielen+2;
4786                         }
4787
4788                         if ((wps_offset+wps_ielen+2+remainder_ielen) <= MAX_IE_SZ) {
4789                                 memcpy(pframe, premainder_ie, remainder_ielen);
4790                                 pframe += remainder_ielen;
4791                                 pattrib->pktlen += remainder_ielen;
4792                         }
4793                 } else {
4794                         memcpy(pframe, cur_network->IEs, cur_network->IELength);
4795                         pframe += cur_network->IELength;
4796                         pattrib->pktlen += cur_network->IELength;
4797                 }
4798         } else
4799 #endif
4800         {
4801                 /* timestamp will be inserted by hardware */
4802                 pframe += 8;
4803                 pattrib->pktlen += 8;
4804
4805                 /*  beacon interval: 2 bytes */
4806
4807                 memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
4808
4809                 pframe += 2;
4810                 pattrib->pktlen += 2;
4811
4812                 /*  capability info: 2 bytes */
4813
4814                 memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
4815
4816                 pframe += 2;
4817                 pattrib->pktlen += 2;
4818
4819                 /* below for ad-hoc mode */
4820
4821                 /*  SSID */
4822                 pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
4823
4824                 /*  supported rates... */
4825                 rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
4826                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen);
4827
4828                 /*  DS parameter set */
4829                 pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
4830
4831                 if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
4832                         u8 erpinfo = 0;
4833                         u32 ATIMWindow;
4834                         /*  IBSS Parameter Set... */
4835                         /* ATIMWindow = cur->Configuration.ATIMWindow; */
4836                         ATIMWindow = 0;
4837                         pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
4838
4839                         /* ERP IE */
4840                         pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
4841                 }
4842
4843
4844                 /*  EXTERNDED SUPPORTED RATE */
4845                 if (rate_len > 8)
4846                         pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
4847                 /* todo:HT for adhoc */
4848         }
4849
4850 #ifdef CONFIG_88EU_P2P
4851         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && is_valid_p2p_probereq) {
4852                 u32 len;
4853                 len = build_probe_resp_p2p_ie(pwdinfo, pframe);
4854
4855                 pframe += len;
4856                 pattrib->pktlen += len;
4857         }
4858 #endif /* CONFIG_88EU_P2P */
4859
4860         pattrib->last_txcmdsz = pattrib->pktlen;
4861
4862         dump_mgntframe(padapter, pmgntframe);
4863
4864         return;
4865 }
4866
4867 static int _issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da, int wait_ack)
4868 {
4869         int ret = _FAIL;
4870         struct xmit_frame               *pmgntframe;
4871         struct pkt_attrib               *pattrib;
4872         unsigned char                   *pframe;
4873         struct rtw_ieee80211_hdr        *pwlanhdr;
4874         __le16 *fctrl;
4875         unsigned char                   *mac;
4876         unsigned char                   bssrate[NumRates];
4877         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
4878         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
4879         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4880         int     bssrate_len = 0;
4881         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
4882
4883         RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+issue_probereq\n"));
4884
4885         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
4886         if (pmgntframe == NULL)
4887                 goto exit;
4888
4889         /* update attribute */
4890         pattrib = &pmgntframe->attrib;
4891         update_mgntframe_attrib(padapter, pattrib);
4892
4893
4894         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4895
4896         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4897         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4898
4899         mac = myid(&(padapter->eeprompriv));
4900
4901         fctrl = &(pwlanhdr->frame_ctl);
4902         *(fctrl) = 0;
4903
4904         if (da) {
4905                 /*      unicast probe request frame */
4906                 memcpy(pwlanhdr->addr1, da, ETH_ALEN);
4907                 memcpy(pwlanhdr->addr3, da, ETH_ALEN);
4908         } else {
4909                 /*      broadcast probe request frame */
4910                 memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
4911                 memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
4912         }
4913
4914         memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
4915
4916         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4917         pmlmeext->mgnt_seq++;
4918         SetFrameSubType(pframe, WIFI_PROBEREQ);
4919
4920         pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
4921         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
4922
4923         if (pssid)
4924                 pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &(pattrib->pktlen));
4925         else
4926                 pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &(pattrib->pktlen));
4927
4928         get_rate_set(padapter, bssrate, &bssrate_len);
4929
4930         if (bssrate_len > 8) {
4931                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
4932                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
4933         } else {
4934                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
4935         }
4936
4937         /* add wps_ie for wps2.0 */
4938         if (pmlmepriv->wps_probe_req_ie_len > 0 && pmlmepriv->wps_probe_req_ie) {
4939                 memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
4940                 pframe += pmlmepriv->wps_probe_req_ie_len;
4941                 pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
4942         }
4943
4944         pattrib->last_txcmdsz = pattrib->pktlen;
4945
4946         RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
4947                  ("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
4948
4949         if (wait_ack) {
4950                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
4951         } else {
4952                 dump_mgntframe(padapter, pmgntframe);
4953                 ret = _SUCCESS;
4954         }
4955
4956 exit:
4957         return ret;
4958 }
4959
4960 inline void issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da)
4961 {
4962         _issue_probereq(padapter, pssid, da, false);
4963 }
4964
4965 int issue_probereq_ex(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da,
4966         int try_cnt, int wait_ms)
4967 {
4968         int ret;
4969         int i = 0;
4970         u32 start = jiffies;
4971
4972         do {
4973                 ret = _issue_probereq(padapter, pssid, da, wait_ms > 0 ? true : false);
4974
4975                 i++;
4976
4977                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
4978                         break;
4979
4980                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
4981                         msleep(wait_ms);
4982
4983         } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
4984
4985         if (ret != _FAIL) {
4986                 ret = _SUCCESS;
4987                 goto exit;
4988         }
4989
4990         if (try_cnt && wait_ms) {
4991                 if (da)
4992                         DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
4993                                 FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
4994                                 ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
4995                 else
4996                         DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
4997                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
4998                                 ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
4999         }
5000 exit:
5001         return ret;
5002 }
5003
5004 /*  if psta == NULL, indicate we are station(client) now... */
5005 void issue_auth(struct adapter *padapter, struct sta_info *psta, unsigned short status)
5006 {
5007         struct xmit_frame *pmgntframe;
5008         struct pkt_attrib *pattrib;
5009         unsigned char *pframe;
5010         struct rtw_ieee80211_hdr *pwlanhdr;
5011         __le16 *fctrl;
5012         unsigned int val32;
5013         u16 val16;
5014 #ifdef CONFIG_88EU_AP_MODE
5015         __le16 le_val16;
5016 #endif
5017         int use_shared_key = 0;
5018         struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
5019         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5020         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5021
5022         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
5023         if (pmgntframe == NULL)
5024                 return;
5025
5026         /* update attribute */
5027         pattrib = &pmgntframe->attrib;
5028         update_mgntframe_attrib(padapter, pattrib);
5029
5030         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5031
5032         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5033         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5034
5035         fctrl = &(pwlanhdr->frame_ctl);
5036         *(fctrl) = 0;
5037
5038         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5039         pmlmeext->mgnt_seq++;
5040         SetFrameSubType(pframe, WIFI_AUTH);
5041
5042         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
5043         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
5044
5045
5046         if (psta) {/*  for AP mode */
5047 #ifdef CONFIG_88EU_AP_MODE
5048
5049                 memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);
5050                 memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
5051                 memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
5052
5053
5054                 /*  setting auth algo number */
5055                 val16 = (u16)psta->authalg;
5056
5057                 if (status != _STATS_SUCCESSFUL_)
5058                         val16 = 0;
5059
5060                 if (val16) {
5061                         le_val16 = cpu_to_le16(val16);
5062                         use_shared_key = 1;
5063                 } else {
5064                         le_val16 = 0;
5065                 }
5066
5067                 pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_val16, &(pattrib->pktlen));
5068
5069                 /*  setting auth seq number */
5070                 val16 = (u16)psta->auth_seq;
5071                 le_val16 = cpu_to_le16(val16);
5072                 pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_val16, &(pattrib->pktlen));
5073
5074                 /*  setting status code... */
5075                 val16 = status;
5076                 le_val16 = cpu_to_le16(val16);
5077                 pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_val16, &(pattrib->pktlen));
5078
5079                 /*  added challenging text... */
5080                 if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1))
5081                         pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, psta->chg_txt, &(pattrib->pktlen));
5082 #endif
5083         } else {
5084                 __le32 le_tmp32;
5085                 __le16 le_tmp16;
5086                 memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
5087                 memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
5088                 memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
5089
5090                 /*  setting auth algo number */
5091                 val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) ? 1 : 0;/*  0:OPEN System, 1:Shared key */
5092                 if (val16)
5093                         use_shared_key = 1;
5094
5095                 /* setting IV for auth seq #3 */
5096                 if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
5097                         val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
5098                         le_tmp32 = cpu_to_le32(val32);
5099                         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&le_tmp32, &(pattrib->pktlen));
5100
5101                         pattrib->iv_len = 4;
5102                 }
5103
5104                 le_tmp16 = cpu_to_le16(val16);
5105                 pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_tmp16, &(pattrib->pktlen));
5106
5107                 /*  setting auth seq number */
5108                 val16 = pmlmeinfo->auth_seq;
5109                 le_tmp16 = cpu_to_le16(val16);
5110                 pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_tmp16, &(pattrib->pktlen));
5111
5112
5113                 /*  setting status code... */
5114                 le_tmp16 = cpu_to_le16(status);
5115                 pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_tmp16, &(pattrib->pktlen));
5116
5117                 /*  then checking to see if sending challenging text... */
5118                 if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
5119                         pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen));
5120
5121                         SetPrivacy(fctrl);
5122
5123                         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
5124
5125                         pattrib->encrypt = _WEP40_;
5126
5127                         pattrib->icv_len = 4;
5128
5129                         pattrib->pktlen += pattrib->icv_len;
5130                 }
5131         }
5132
5133         pattrib->last_txcmdsz = pattrib->pktlen;
5134
5135         rtw_wep_encrypt(padapter, (u8 *)pmgntframe);
5136         DBG_88E("%s\n", __func__);
5137         dump_mgntframe(padapter, pmgntframe);
5138
5139         return;
5140 }
5141
5142
5143 void issue_asocrsp(struct adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
5144 {
5145 #ifdef CONFIG_88EU_AP_MODE
5146         struct xmit_frame       *pmgntframe;
5147         struct rtw_ieee80211_hdr        *pwlanhdr;
5148         struct pkt_attrib *pattrib;
5149         unsigned char   *pbuf, *pframe;
5150         unsigned short val;
5151         __le16 *fctrl;
5152         struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
5153         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
5154         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5155         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5156         struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
5157         u8 *ie = pnetwork->IEs;
5158         __le16 lestatus, leval;
5159 #ifdef CONFIG_88EU_P2P
5160         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
5161 #endif /* CONFIG_88EU_P2P */
5162
5163         DBG_88E("%s\n", __func__);
5164
5165         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
5166         if (pmgntframe == NULL)
5167                 return;
5168
5169         /* update attribute */
5170         pattrib = &pmgntframe->attrib;
5171         update_mgntframe_attrib(padapter, pattrib);
5172
5173
5174         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5175
5176         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5177         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5178
5179         fctrl = &(pwlanhdr->frame_ctl);
5180         *(fctrl) = 0;
5181
5182         memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
5183         memcpy((void *)GetAddr2Ptr(pwlanhdr), myid(&(padapter->eeprompriv)), ETH_ALEN);
5184         memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
5185
5186
5187         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5188         pmlmeext->mgnt_seq++;
5189         if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
5190                 SetFrameSubType(pwlanhdr, pkt_type);
5191         else
5192                 return;
5193
5194         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
5195         pattrib->pktlen += pattrib->hdrlen;
5196         pframe += pattrib->hdrlen;
5197
5198         /* capability */
5199         val = *(unsigned short *)rtw_get_capability_from_ie(ie);
5200
5201         pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_ , (unsigned char *)&val, &(pattrib->pktlen));
5202
5203         lestatus = cpu_to_le16(status);
5204         pframe = rtw_set_fixed_ie(pframe , _STATUS_CODE_ , (unsigned char *)&lestatus, &(pattrib->pktlen));
5205
5206         leval = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
5207         pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_ , (unsigned char *)&leval, &(pattrib->pktlen));
5208
5209         if (pstat->bssratelen <= 8) {
5210                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen));
5211         } else {
5212                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &(pattrib->pktlen));
5213                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (pstat->bssratelen-8), pstat->bssrateset+8, &(pattrib->pktlen));
5214         }
5215
5216         if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) {
5217                 uint ie_len = 0;
5218
5219                 /* FILL HT CAP INFO IE */
5220                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
5221                 if (pbuf && ie_len > 0) {
5222                         memcpy(pframe, pbuf, ie_len+2);
5223                         pframe += (ie_len+2);
5224                         pattrib->pktlen += (ie_len+2);
5225                 }
5226
5227                 /* FILL HT ADD INFO IE */
5228                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
5229                 if (pbuf && ie_len > 0) {
5230                         memcpy(pframe, pbuf, ie_len+2);
5231                         pframe += (ie_len+2);
5232                         pattrib->pktlen += (ie_len+2);
5233                 }
5234         }
5235
5236         /* FILL WMM IE */
5237         if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option)) {
5238                 uint ie_len = 0;
5239                 unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
5240
5241                 for (pbuf = ie + _BEACON_IE_OFFSET_;; pbuf += (ie_len + 2)) {
5242                         pbuf = rtw_get_ie(pbuf, _VENDOR_SPECIFIC_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
5243                         if (pbuf && !memcmp(pbuf+2, WMM_PARA_IE, 6)) {
5244                                 memcpy(pframe, pbuf, ie_len+2);
5245                                 pframe += (ie_len+2);
5246                                 pattrib->pktlen += (ie_len+2);
5247                                 break;
5248                         }
5249
5250                         if ((pbuf == NULL) || (ie_len == 0))
5251                                 break;
5252                 }
5253         }
5254
5255         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
5256                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
5257
5258         /* add WPS IE ie for wps 2.0 */
5259         if (pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len > 0) {
5260                 memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len);
5261
5262                 pframe += pmlmepriv->wps_assoc_resp_ie_len;
5263                 pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
5264         }
5265
5266 #ifdef CONFIG_88EU_P2P
5267         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && (pstat->is_p2p_device)) {
5268                 u32 len;
5269
5270                 len = build_assoc_resp_p2p_ie(pwdinfo, pframe, pstat->p2p_status_code);
5271
5272                 pframe += len;
5273                 pattrib->pktlen += len;
5274         }
5275 #endif /* CONFIG_88EU_P2P */
5276         pattrib->last_txcmdsz = pattrib->pktlen;
5277         dump_mgntframe(padapter, pmgntframe);
5278 #endif
5279 }
5280
5281 void issue_assocreq(struct adapter *padapter)
5282 {
5283         int ret = _FAIL;
5284         struct xmit_frame       *pmgntframe;
5285         struct pkt_attrib       *pattrib;
5286         unsigned char           *pframe, *p;
5287         struct rtw_ieee80211_hdr        *pwlanhdr;
5288         __le16 *fctrl;
5289         __le16          le_tmp;
5290         unsigned int    i, j, ie_len, index = 0;
5291         unsigned char   rf_type, bssrate[NumRates], sta_bssrate[NumRates];
5292         struct ndis_802_11_var_ie *pIE;
5293         struct registry_priv    *pregpriv = &padapter->registrypriv;
5294         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
5295         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
5296         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5297         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5298         int     bssrate_len = 0, sta_bssrate_len = 0;
5299 #ifdef CONFIG_88EU_P2P
5300         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
5301         u8 p2pie[255] = { 0x00 };
5302         u16 p2pielen = 0;
5303 #endif /* CONFIG_88EU_P2P */
5304
5305         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
5306         if (pmgntframe == NULL)
5307                 goto exit;
5308
5309         /* update attribute */
5310         pattrib = &pmgntframe->attrib;
5311         update_mgntframe_attrib(padapter, pattrib);
5312
5313         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5314         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5315         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5316
5317         fctrl = &(pwlanhdr->frame_ctl);
5318         *(fctrl) = 0;
5319         memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
5320         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
5321         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
5322
5323         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5324         pmlmeext->mgnt_seq++;
5325         SetFrameSubType(pframe, WIFI_ASSOCREQ);
5326
5327         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
5328         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
5329
5330         /* caps */
5331
5332         memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
5333
5334         pframe += 2;
5335         pattrib->pktlen += 2;
5336
5337         /* listen interval */
5338         /* todo: listen interval for power saving */
5339         le_tmp = cpu_to_le16(3);
5340         memcpy(pframe , (unsigned char *)&le_tmp, 2);
5341         pframe += 2;
5342         pattrib->pktlen += 2;
5343
5344         /* SSID */
5345         pframe = rtw_set_ie(pframe, _SSID_IE_,  pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &(pattrib->pktlen));
5346
5347         /* supported rate & extended supported rate */
5348
5349         /*  Check if the AP's supported rates are also supported by STA. */
5350         get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);
5351
5352         if (pmlmeext->cur_channel == 14)/*  for JAPAN, channel 14 can only uses B Mode(CCK) */
5353                 sta_bssrate_len = 4;
5354
5355         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
5356                 if (pmlmeinfo->network.SupportedRates[i] == 0)
5357                         break;
5358                 DBG_88E("network.SupportedRates[%d]=%02X\n", i, pmlmeinfo->network.SupportedRates[i]);
5359         }
5360
5361         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
5362                 if (pmlmeinfo->network.SupportedRates[i] == 0)
5363                         break;
5364
5365                 /*  Check if the AP's supported rates are also supported by STA. */
5366                 for (j = 0; j < sta_bssrate_len; j++) {
5367                          /*  Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP */
5368                         if ((pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK)
5369                                         == (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK))
5370                                 break;
5371                 }
5372
5373                 if (j == sta_bssrate_len) {
5374                         /*  the rate is not supported by STA */
5375                         DBG_88E("%s(): the rate[%d]=%02X is not supported by STA!\n", __func__, i, pmlmeinfo->network.SupportedRates[i]);
5376                 } else {
5377                         /*  the rate is supported by STA */
5378                         bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
5379                 }
5380         }
5381
5382         bssrate_len = index;
5383         DBG_88E("bssrate_len=%d\n", bssrate_len);
5384
5385         if (bssrate_len == 0) {
5386                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
5387                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
5388                 goto exit; /* don't connect to AP if no joint supported rate */
5389         }
5390
5391
5392         if (bssrate_len > 8) {
5393                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
5394                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
5395         } else {
5396                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
5397         }
5398
5399         /* RSN */
5400         p = rtw_get_ie((pmlmeinfo->network.IEs + sizeof(struct ndis_802_11_fixed_ie)), _RSN_IE_2_, &ie_len, (pmlmeinfo->network.IELength - sizeof(struct ndis_802_11_fixed_ie)));
5401         if (p != NULL)
5402                 pframe = rtw_set_ie(pframe, _RSN_IE_2_, ie_len, (p + 2), &(pattrib->pktlen));
5403
5404         /* HT caps */
5405         if (padapter->mlmepriv.htpriv.ht_option) {
5406                 p = rtw_get_ie((pmlmeinfo->network.IEs + sizeof(struct ndis_802_11_fixed_ie)), _HT_CAPABILITY_IE_, &ie_len, (pmlmeinfo->network.IELength - sizeof(struct ndis_802_11_fixed_ie)));
5407                 if ((p != NULL) && (!(is_ap_in_tkip(padapter)))) {
5408                         memcpy(&(pmlmeinfo->HT_caps), (p + 2), sizeof(struct HT_caps_element));
5409
5410                         /* to disable 40M Hz support while gd_bw_40MHz_en = 0 */
5411                         if (pregpriv->cbw40_enable == 0)
5412                                 pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info &= cpu_to_le16(~(BIT(6) | BIT(1)));
5413                         else
5414                                 pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(BIT(1));
5415
5416                         /* todo: disable SM power save mode */
5417                         pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x000c);
5418
5419                         rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
5420                         switch (rf_type) {
5421                         case RF_1T1R:
5422                                 if (pregpriv->rx_stbc)
5423                                         pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0100);/* RX STBC One spatial stream */
5424                                 memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_1R, 16);
5425                                 break;
5426                         case RF_2T2R:
5427                         case RF_1T2R:
5428                         default:
5429                                 if ((pregpriv->rx_stbc == 0x3) ||/* enable for 2.4/5 GHz */
5430                                     ((pmlmeext->cur_wireless_mode & WIRELESS_11_24N) && (pregpriv->rx_stbc == 0x1)) || /* enable for 2.4GHz */
5431                                     (pregpriv->wifi_spec == 1)) {
5432                                         DBG_88E("declare supporting RX STBC\n");
5433                                         pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0200);/* RX STBC two spatial stream */
5434                                 }
5435                                 memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_2R, 16);
5436                                 break;
5437                         }
5438                         pframe = rtw_set_ie(pframe, _HT_CAPABILITY_IE_, ie_len , (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
5439                 }
5440         }
5441
5442         /* vendor specific IE, such as WPA, WMM, WPS */
5443         for (i = sizeof(struct ndis_802_11_fixed_ie); i < pmlmeinfo->network.IELength;) {
5444                 pIE = (struct ndis_802_11_var_ie *)(pmlmeinfo->network.IEs + i);
5445
5446                 switch (pIE->ElementID) {
5447                 case _VENDOR_SPECIFIC_IE_:
5448                         if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) ||
5449                             (!memcmp(pIE->data, WMM_OUI, 4)) ||
5450                             (!memcmp(pIE->data, WPS_OUI, 4))) {
5451                                 if (!padapter->registrypriv.wifi_spec) {
5452                                         /* Commented by Kurt 20110629 */
5453                                         /* In some older APs, WPS handshake */
5454                                         /* would be fail if we append vender extensions informations to AP */
5455                                         if (!memcmp(pIE->data, WPS_OUI, 4))
5456                                                 pIE->Length = 14;
5457                                 }
5458                                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, pIE->Length, pIE->data, &(pattrib->pktlen));
5459                         }
5460                         break;
5461                 default:
5462                         break;
5463                 }
5464                 i += (pIE->Length + 2);
5465         }
5466
5467         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
5468                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
5469
5470 #ifdef CONFIG_88EU_P2P
5471
5472         if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) {
5473                 /*      Should add the P2P IE in the association request frame. */
5474                 /*      P2P OUI */
5475
5476                 p2pielen = 0;
5477                 p2pie[p2pielen++] = 0x50;
5478                 p2pie[p2pielen++] = 0x6F;
5479                 p2pie[p2pielen++] = 0x9A;
5480                 p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
5481
5482                 /*      Commented by Albert 20101109 */
5483                 /*      According to the P2P Specification, the association request frame should contain 3 P2P attributes */
5484                 /*      1. P2P Capability */
5485                 /*      2. Extended Listen Timing */
5486                 /*      3. Device Info */
5487                 /*      Commented by Albert 20110516 */
5488                 /*      4. P2P Interface */
5489
5490                 /*      P2P Capability */
5491                 /*      Type: */
5492                 p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
5493
5494                 /*      Length: */
5495                 *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
5496                 p2pielen += 2;
5497
5498                 /*      Value: */
5499                 /*      Device Capability Bitmap, 1 byte */
5500                 p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
5501
5502                 /*      Group Capability Bitmap, 1 byte */
5503                 if (pwdinfo->persistent_supported)
5504                         p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
5505                 else
5506                         p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
5507
5508                 /*      Extended Listen Timing */
5509                 /*      Type: */
5510                 p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
5511
5512                 /*      Length: */
5513                 *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
5514                 p2pielen += 2;
5515
5516                 /*      Value: */
5517                 /*      Availability Period */
5518                 *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
5519                 p2pielen += 2;
5520
5521                 /*      Availability Interval */
5522                 *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
5523                 p2pielen += 2;
5524
5525                 /*      Device Info */
5526                 /*      Type: */
5527                 p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
5528
5529                 /*      Length: */
5530                 /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
5531                 /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
5532                 *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
5533                 p2pielen += 2;
5534
5535                 /*      Value: */
5536                 /*      P2P Device Address */
5537                 memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
5538                 p2pielen += ETH_ALEN;
5539
5540                 /*      Config Method */
5541                 /*      This field should be big endian. Noted by P2P specification. */
5542                 if ((pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN) ||
5543                     (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN))
5544                         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
5545                 else
5546                         *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC);
5547
5548                 p2pielen += 2;
5549
5550                 /*      Primary Device Type */
5551                 /*      Category ID */
5552                 *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
5553                 p2pielen += 2;
5554
5555                 /*      OUI */
5556                 *(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
5557                 p2pielen += 4;
5558
5559                 /*      Sub Category ID */
5560                 *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
5561                 p2pielen += 2;
5562
5563                 /*      Number of Secondary Device Types */
5564                 p2pie[p2pielen++] = 0x00;       /*      No Secondary Device Type List */
5565
5566                 /*      Device Name */
5567                 /*      Type: */
5568                 *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
5569                 p2pielen += 2;
5570
5571                 /*      Length: */
5572                 *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
5573                 p2pielen += 2;
5574
5575                 /*      Value: */
5576                 memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
5577                 p2pielen += pwdinfo->device_name_len;
5578
5579                 /*      P2P Interface */
5580                 /*      Type: */
5581                 p2pie[p2pielen++] = P2P_ATTR_INTERFACE;
5582
5583                 /*      Length: */
5584                 *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x000D);
5585                 p2pielen += 2;
5586
5587                 /*      Value: */
5588                 memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);       /*      P2P Device Address */
5589                 p2pielen += ETH_ALEN;
5590
5591                 p2pie[p2pielen++] = 1;  /*      P2P Interface Address Count */
5592
5593                 memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);       /*      P2P Interface Address List */
5594                 p2pielen += ETH_ALEN;
5595
5596                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
5597         }
5598
5599 #endif /* CONFIG_88EU_P2P */
5600
5601         pattrib->last_txcmdsz = pattrib->pktlen;
5602         dump_mgntframe(padapter, pmgntframe);
5603
5604         ret = _SUCCESS;
5605
5606 exit:
5607         if (ret == _SUCCESS)
5608                 rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen);
5609         else
5610                 rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
5611
5612         return;
5613 }
5614
5615 /* when wait_ack is true, this function should be called at process context */
5616 static int _issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
5617 {
5618         int ret = _FAIL;
5619         struct xmit_frame                       *pmgntframe;
5620         struct pkt_attrib                       *pattrib;
5621         unsigned char                                   *pframe;
5622         struct rtw_ieee80211_hdr        *pwlanhdr;
5623         __le16 *fctrl;
5624         struct xmit_priv        *pxmitpriv;
5625         struct mlme_ext_priv    *pmlmeext;
5626         struct mlme_ext_info    *pmlmeinfo;
5627
5628         if (!padapter)
5629                 goto exit;
5630
5631         pxmitpriv = &(padapter->xmitpriv);
5632         pmlmeext = &(padapter->mlmeextpriv);
5633         pmlmeinfo = &(pmlmeext->mlmext_info);
5634
5635         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
5636         if (pmgntframe == NULL)
5637                 goto exit;
5638
5639         /* update attribute */
5640         pattrib = &pmgntframe->attrib;
5641         update_mgntframe_attrib(padapter, pattrib);
5642         pattrib->retry_ctrl = false;
5643
5644         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5645
5646         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5647         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5648
5649         fctrl = &(pwlanhdr->frame_ctl);
5650         *(fctrl) = 0;
5651
5652         if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
5653                 SetFrDs(fctrl);
5654         else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
5655                 SetToDs(fctrl);
5656
5657         if (power_mode)
5658                 SetPwrMgt(fctrl);
5659
5660         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
5661         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
5662         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
5663
5664         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5665         pmlmeext->mgnt_seq++;
5666         SetFrameSubType(pframe, WIFI_DATA_NULL);
5667
5668         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
5669         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
5670
5671         pattrib->last_txcmdsz = pattrib->pktlen;
5672
5673         if (wait_ack) {
5674                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
5675         } else {
5676                 dump_mgntframe(padapter, pmgntframe);
5677                 ret = _SUCCESS;
5678         }
5679
5680 exit:
5681         return ret;
5682 }
5683
5684
5685 /* when wait_ms > 0 , this function should be called at process context */
5686 /* da == NULL for station mode */
5687 int issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
5688 {
5689         int ret;
5690         int i = 0;
5691         u32 start = jiffies;
5692         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5693         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5694
5695         /* da == NULL, assume it's null data for sta to ap*/
5696         if (da == NULL)
5697                 da = get_my_bssid(&(pmlmeinfo->network));
5698
5699         do {
5700                 ret = _issue_nulldata(padapter, da, power_mode, wait_ms > 0 ? true : false);
5701
5702                 i++;
5703
5704                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
5705                         break;
5706
5707                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
5708                         msleep(wait_ms);
5709         } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
5710
5711         if (ret != _FAIL) {
5712                 ret = _SUCCESS;
5713                 goto exit;
5714         }
5715
5716         if (try_cnt && wait_ms) {
5717                 if (da)
5718                         DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
5719                                 FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
5720                                 ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
5721                 else
5722                         DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
5723                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
5724                                 ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
5725         }
5726 exit:
5727         return ret;
5728 }
5729
5730 /* when wait_ack is true, this function should be called at process context */
5731 static int _issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int wait_ack)
5732 {
5733         int ret = _FAIL;
5734         struct xmit_frame                       *pmgntframe;
5735         struct pkt_attrib                       *pattrib;
5736         unsigned char                                   *pframe;
5737         struct rtw_ieee80211_hdr        *pwlanhdr;
5738         __le16 *fctrl;
5739         unsigned short *qc;
5740         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
5741         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5742         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5743
5744         DBG_88E("%s\n", __func__);
5745
5746         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
5747         if (pmgntframe == NULL)
5748                 goto exit;
5749
5750         /* update attribute */
5751         pattrib = &pmgntframe->attrib;
5752         update_mgntframe_attrib(padapter, pattrib);
5753
5754         pattrib->hdrlen += 2;
5755         pattrib->qos_en = true;
5756         pattrib->eosp = 1;
5757         pattrib->ack_policy = 0;
5758         pattrib->mdata = 0;
5759
5760         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5761
5762         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5763         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5764
5765         fctrl = &(pwlanhdr->frame_ctl);
5766         *(fctrl) = 0;
5767
5768         if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
5769                 SetFrDs(fctrl);
5770         else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
5771                 SetToDs(fctrl);
5772
5773         if (pattrib->mdata)
5774                 SetMData(fctrl);
5775
5776         qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
5777
5778         SetPriority(qc, tid);
5779
5780         SetEOSP(qc, pattrib->eosp);
5781
5782         SetAckpolicy(qc, pattrib->ack_policy);
5783
5784         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
5785         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
5786         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
5787
5788         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5789         pmlmeext->mgnt_seq++;
5790         SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
5791
5792         pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos);
5793         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos);
5794
5795         pattrib->last_txcmdsz = pattrib->pktlen;
5796
5797         if (wait_ack) {
5798                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
5799         } else {
5800                 dump_mgntframe(padapter, pmgntframe);
5801                 ret = _SUCCESS;
5802         }
5803
5804 exit:
5805         return ret;
5806 }
5807
5808 /* when wait_ms > 0 , this function should be called at process context */
5809 /* da == NULL for station mode */
5810 int issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms)
5811 {
5812         int ret;
5813         int i = 0;
5814         u32 start = jiffies;
5815         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5816         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5817
5818         /* da == NULL, assume it's null data for sta to ap*/
5819         if (da == NULL)
5820                 da = get_my_bssid(&(pmlmeinfo->network));
5821
5822         do {
5823                 ret = _issue_qos_nulldata(padapter, da, tid, wait_ms > 0 ? true : false);
5824
5825                 i++;
5826
5827                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
5828                         break;
5829
5830                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
5831                         msleep(wait_ms);
5832         } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
5833
5834         if (ret != _FAIL) {
5835                 ret = _SUCCESS;
5836                 goto exit;
5837         }
5838
5839         if (try_cnt && wait_ms) {
5840                 if (da)
5841                         DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
5842                                 FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
5843                                 ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
5844                 else
5845                         DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
5846                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
5847                                 ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
5848         }
5849 exit:
5850         return ret;
5851 }
5852
5853 static int _issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack)
5854 {
5855         struct xmit_frame                       *pmgntframe;
5856         struct pkt_attrib                       *pattrib;
5857         unsigned char                                   *pframe;
5858         struct rtw_ieee80211_hdr        *pwlanhdr;
5859         __le16 *fctrl;
5860         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
5861         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5862         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5863         int ret = _FAIL;
5864         __le16 le_tmp;
5865 #ifdef CONFIG_88EU_P2P
5866         struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
5867 #endif /* CONFIG_88EU_P2P */
5868
5869 #ifdef CONFIG_88EU_P2P
5870         if (!(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) && (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
5871                 _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
5872                 _set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
5873         }
5874 #endif /* CONFIG_88EU_P2P */
5875
5876         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
5877         if (pmgntframe == NULL)
5878                 goto exit;
5879
5880         /* update attribute */
5881         pattrib = &pmgntframe->attrib;
5882         update_mgntframe_attrib(padapter, pattrib);
5883         pattrib->retry_ctrl = false;
5884
5885         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5886
5887         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5888         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5889
5890         fctrl = &(pwlanhdr->frame_ctl);
5891         *(fctrl) = 0;
5892
5893         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
5894         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
5895         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
5896
5897         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5898         pmlmeext->mgnt_seq++;
5899         SetFrameSubType(pframe, WIFI_DEAUTH);
5900
5901         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
5902         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
5903
5904         le_tmp = cpu_to_le16(reason);
5905         pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_ , (unsigned char *)&le_tmp, &(pattrib->pktlen));
5906
5907         pattrib->last_txcmdsz = pattrib->pktlen;
5908
5909
5910         if (wait_ack) {
5911                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
5912         } else {
5913                 dump_mgntframe(padapter, pmgntframe);
5914                 ret = _SUCCESS;
5915         }
5916
5917 exit:
5918         return ret;
5919 }
5920
5921 int issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason)
5922 {
5923         DBG_88E("%s to %pM\n", __func__, da);
5924         return _issue_deauth(padapter, da, reason, false);
5925 }
5926
5927 int issue_deauth_ex(struct adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
5928         int wait_ms)
5929 {
5930         int ret;
5931         int i = 0;
5932         u32 start = jiffies;
5933
5934         do {
5935                 ret = _issue_deauth(padapter, da, reason, wait_ms > 0 ? true : false);
5936
5937                 i++;
5938
5939                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
5940                         break;
5941
5942                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
5943                         msleep(wait_ms);
5944         } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
5945
5946         if (ret != _FAIL) {
5947                 ret = _SUCCESS;
5948                 goto exit;
5949         }
5950
5951         if (try_cnt && wait_ms) {
5952                 if (da)
5953                         DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
5954                                 FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
5955                                 ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
5956                 else
5957                         DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
5958                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
5959                                 ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
5960         }
5961 exit:
5962         return ret;
5963 }
5964
5965 void issue_action_spct_ch_switch (struct adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset)
5966 {
5967         struct xmit_frame                       *pmgntframe;
5968         struct pkt_attrib                       *pattrib;
5969         unsigned char                           *pframe;
5970         struct rtw_ieee80211_hdr        *pwlanhdr;
5971         __le16 *fctrl;
5972         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
5973         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5974
5975
5976         DBG_88E(FUNC_NDEV_FMT" ra =%pM, ch:%u, offset:%u\n",
5977                 FUNC_NDEV_ARG(padapter->pnetdev), ra, new_ch, ch_offset);
5978
5979         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
5980         if (pmgntframe == NULL)
5981                 return;
5982
5983         /* update attribute */
5984         pattrib = &pmgntframe->attrib;
5985         update_mgntframe_attrib(padapter, pattrib);
5986
5987         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5988
5989         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5990         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5991
5992         fctrl = &(pwlanhdr->frame_ctl);
5993         *(fctrl) = 0;
5994
5995         memcpy(pwlanhdr->addr1, ra, ETH_ALEN); /* RA */
5996         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); /* TA */
5997         memcpy(pwlanhdr->addr3, ra, ETH_ALEN); /* DA = RA */
5998
5999         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
6000         pmlmeext->mgnt_seq++;
6001         SetFrameSubType(pframe, WIFI_ACTION);
6002
6003         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
6004         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
6005
6006         /* category, action */
6007         {
6008                 u8 category, action;
6009                 category = RTW_WLAN_CATEGORY_SPECTRUM_MGMT;
6010                 action = RTW_WLAN_ACTION_SPCT_CHL_SWITCH;
6011
6012                 pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
6013                 pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
6014         }
6015
6016         pframe = rtw_set_ie_ch_switch (pframe, &(pattrib->pktlen), 0, new_ch, 0);
6017         pframe = rtw_set_ie_secondary_ch_offset(pframe, &(pattrib->pktlen),
6018                 hal_ch_offset_to_secondary_ch_offset(ch_offset));
6019
6020         pattrib->last_txcmdsz = pattrib->pktlen;
6021
6022         dump_mgntframe(padapter, pmgntframe);
6023 }
6024
6025 void issue_action_BA(struct adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short status)
6026 {
6027         u8 category = RTW_WLAN_CATEGORY_BACK;
6028         u16 start_seq;
6029         u16 BA_para_set;
6030         u16 reason_code;
6031         u16 BA_timeout_value;
6032         __le16  le_tmp;
6033         u16 BA_starting_seqctrl = 0;
6034         enum ht_cap_ampdu_factor max_rx_ampdu_factor;
6035         struct xmit_frame *pmgntframe;
6036         struct pkt_attrib *pattrib;
6037         u8 *pframe;
6038         struct rtw_ieee80211_hdr *pwlanhdr;
6039         __le16 *fctrl;
6040         struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
6041         struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
6042         struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
6043         struct sta_info *psta;
6044         struct sta_priv *pstapriv = &padapter->stapriv;
6045         struct registry_priv *pregpriv = &padapter->registrypriv;
6046
6047         DBG_88E("%s, category=%d, action=%d, status=%d\n", __func__, category, action, status);
6048
6049         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
6050         if (pmgntframe == NULL)
6051                 return;
6052
6053         /* update attribute */
6054         pattrib = &pmgntframe->attrib;
6055         update_mgntframe_attrib(padapter, pattrib);
6056
6057         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
6058
6059         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6060         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
6061
6062         fctrl = &(pwlanhdr->frame_ctl);
6063         *(fctrl) = 0;
6064
6065         /* memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); */
6066         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
6067         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
6068         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
6069
6070         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
6071         pmlmeext->mgnt_seq++;
6072         SetFrameSubType(pframe, WIFI_ACTION);
6073
6074         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
6075         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
6076
6077         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
6078         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
6079
6080         if (category == 3) {
6081                 switch (action) {
6082                 case 0: /* ADDBA req */
6083                         do {
6084                                 pmlmeinfo->dialogToken++;
6085                         } while (pmlmeinfo->dialogToken == 0);
6086                         pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen));
6087
6088                         BA_para_set = (0x1002 | ((status & 0xf) << 2)); /* immediate ack & 64 buffer size */
6089                         le_tmp = cpu_to_le16(BA_para_set);
6090                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
6091
6092                         BA_timeout_value = 5000;/*  5ms */
6093                         le_tmp = cpu_to_le16(BA_timeout_value);
6094                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
6095
6096                         psta = rtw_get_stainfo(pstapriv, raddr);
6097                         if (psta != NULL) {
6098                                 start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07]&0xfff) + 1;
6099
6100                                 DBG_88E("BA_starting_seqctrl=%d for TID=%d\n", start_seq, status & 0x07);
6101
6102                                 psta->BA_starting_seqctrl[status & 0x07] = start_seq;
6103
6104                                 BA_starting_seqctrl = start_seq << 4;
6105                         }
6106                         le_tmp = cpu_to_le16(BA_starting_seqctrl);
6107                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
6108                         break;
6109                 case 1: /* ADDBA rsp */
6110                         pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen));
6111                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&status), &(pattrib->pktlen));
6112
6113                         BA_para_set = le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f;
6114                         rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
6115                         switch (max_rx_ampdu_factor) {
6116                         case MAX_AMPDU_FACTOR_64K:
6117                                 BA_para_set |= 0x1000; /* 64 buffer size */
6118                                 break;
6119                         case MAX_AMPDU_FACTOR_32K:
6120                                 BA_para_set |= 0x0800; /* 32 buffer size */
6121                                 break;
6122                         case MAX_AMPDU_FACTOR_16K:
6123                                 BA_para_set |= 0x0400; /* 16 buffer size */
6124                                 break;
6125                         case MAX_AMPDU_FACTOR_8K:
6126                                 BA_para_set |= 0x0200; /* 8 buffer size */
6127                                 break;
6128                         default:
6129                                 BA_para_set |= 0x1000; /* 64 buffer size */
6130                                 break;
6131                         }
6132
6133                         if (pregpriv->ampdu_amsdu == 0)/* disabled */
6134                                 BA_para_set = BA_para_set & ~BIT(0);
6135                         else if (pregpriv->ampdu_amsdu == 1)/* enabled */
6136                                 BA_para_set = BA_para_set | BIT(0);
6137                         le_tmp = cpu_to_le16(BA_para_set);
6138
6139                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
6140                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen));
6141                         break;
6142                 case 2:/* DELBA */
6143                         BA_para_set = (status & 0x1F) << 3;
6144                         le_tmp = cpu_to_le16(BA_para_set);
6145                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
6146
6147                         reason_code = 37;/* Requested from peer STA as it does not want to use the mechanism */
6148                         le_tmp = cpu_to_le16(reason_code);
6149                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
6150                         break;
6151                 default:
6152                         break;
6153                 }
6154         }
6155
6156         pattrib->last_txcmdsz = pattrib->pktlen;
6157
6158         dump_mgntframe(padapter, pmgntframe);
6159 }
6160
6161 static void issue_action_BSSCoexistPacket(struct adapter *padapter)
6162 {
6163         struct list_head *plist, *phead;
6164         unsigned char category, action;
6165         struct xmit_frame                       *pmgntframe;
6166         struct pkt_attrib                       *pattrib;
6167         unsigned char                           *pframe;
6168         struct rtw_ieee80211_hdr        *pwlanhdr;
6169         __le16 *fctrl;
6170         struct  wlan_network    *pnetwork = NULL;
6171         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
6172         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
6173         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6174         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6175         struct __queue *queue   = &(pmlmepriv->scanned_queue);
6176         u8 InfoContent[16] = {0};
6177         u8 ICS[8][15];
6178         if ((pmlmepriv->num_FortyMHzIntolerant == 0) || (pmlmepriv->num_sta_no_ht == 0))
6179                 return;
6180
6181         if (pmlmeinfo->bwmode_updated)
6182                 return;
6183
6184
6185         DBG_88E("%s\n", __func__);
6186
6187
6188         category = RTW_WLAN_CATEGORY_PUBLIC;
6189         action = ACT_PUBLIC_BSSCOEXIST;
6190
6191         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
6192         if (pmgntframe == NULL)
6193                 return;
6194
6195         /* update attribute */
6196         pattrib = &pmgntframe->attrib;
6197         update_mgntframe_attrib(padapter, pattrib);
6198
6199         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
6200
6201         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6202         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
6203
6204         fctrl = &(pwlanhdr->frame_ctl);
6205         *(fctrl) = 0;
6206
6207         memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
6208         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
6209         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
6210
6211         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
6212         pmlmeext->mgnt_seq++;
6213         SetFrameSubType(pframe, WIFI_ACTION);
6214
6215         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
6216         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
6217
6218         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
6219         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
6220
6221
6222         /*  */
6223         if (pmlmepriv->num_FortyMHzIntolerant > 0) {
6224                 u8 iedata = 0;
6225
6226                 iedata |= BIT(2);/* 20 MHz BSS Width Request */
6227
6228                 pframe = rtw_set_ie(pframe, EID_BSSCoexistence,  1, &iedata, &(pattrib->pktlen));
6229         }
6230
6231
6232         /*  */
6233         _rtw_memset(ICS, 0, sizeof(ICS));
6234         if (pmlmepriv->num_sta_no_ht > 0) {
6235                 int i;
6236
6237                 spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
6238
6239                 phead = get_list_head(queue);
6240                 plist = phead->next;
6241
6242                 while (1) {
6243                         int len;
6244                         u8 *p;
6245                         struct wlan_bssid_ex *pbss_network;
6246
6247                         if (rtw_end_of_queue_search(phead, plist))
6248                                 break;
6249
6250                         pnetwork = container_of(plist, struct wlan_network, list);
6251
6252                         plist = plist->next;
6253
6254                         pbss_network = (struct wlan_bssid_ex *)&pnetwork->network;
6255
6256                         p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
6257                         if ((p == NULL) || (len == 0)) { /* non-HT */
6258                                 if ((pbss_network->Configuration.DSConfig <= 0) || (pbss_network->Configuration.DSConfig > 14))
6259                                         continue;
6260
6261                                 ICS[0][pbss_network->Configuration.DSConfig] = 1;
6262
6263                                 if (ICS[0][0] == 0)
6264                                         ICS[0][0] = 1;
6265                         }
6266                 }
6267                 spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
6268
6269                 for (i = 0; i < 8; i++) {
6270                         if (ICS[i][0] == 1) {
6271                                 int j, k = 0;
6272
6273                                 InfoContent[k] = i;
6274                                 /* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent, i); */
6275                                 k++;
6276
6277                                 for (j = 1; j <= 14; j++) {
6278                                         if (ICS[i][j] == 1) {
6279                                                 if (k < 16) {
6280                                                         InfoContent[k] = j; /* channel number */
6281                                                         /* SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j); */
6282                                                         k++;
6283                                                 }
6284                                         }
6285                                 }
6286
6287                                 pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &(pattrib->pktlen));
6288                         }
6289                 }
6290         }
6291
6292
6293         pattrib->last_txcmdsz = pattrib->pktlen;
6294
6295         dump_mgntframe(padapter, pmgntframe);
6296 }
6297
6298 unsigned int send_delba(struct adapter *padapter, u8 initiator, u8 *addr)
6299 {
6300         struct sta_priv *pstapriv = &padapter->stapriv;
6301         struct sta_info *psta = NULL;
6302         /* struct recv_reorder_ctrl *preorder_ctrl; */
6303         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
6304         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6305         u16 tid;
6306
6307         if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
6308                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
6309                         return _SUCCESS;
6310
6311         psta = rtw_get_stainfo(pstapriv, addr);
6312         if (psta == NULL)
6313                 return _SUCCESS;
6314
6315         if (initiator == 0) { /*  recipient */
6316                 for (tid = 0; tid < MAXTID; tid++) {
6317                         if (psta->recvreorder_ctrl[tid].enable) {
6318                                 DBG_88E("rx agg disable tid(%d)\n", tid);
6319                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid << 1) | initiator)&0x1F));
6320                                 psta->recvreorder_ctrl[tid].enable = false;
6321                                 psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
6322                         }
6323                 }
6324         } else if (initiator == 1) { /*  originator */
6325                 for (tid = 0; tid < MAXTID; tid++) {
6326                         if (psta->htpriv.agg_enable_bitmap & BIT(tid)) {
6327                                 DBG_88E("tx agg disable tid(%d)\n", tid);
6328                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid << 1) | initiator)&0x1F));
6329                                 psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
6330                                 psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
6331                         }
6332                 }
6333         }
6334
6335         return _SUCCESS;
6336 }
6337
6338 unsigned int send_beacon(struct adapter *padapter)
6339 {
6340         u8 bxmitok = false;
6341         int     issue = 0;
6342         int poll = 0;
6343
6344         u32 start = jiffies;
6345
6346         rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
6347         do {
6348                 issue_beacon(padapter, 100);
6349                 issue++;
6350                 do {
6351                         yield();
6352                         rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
6353                         poll++;
6354                 } while ((poll%10) != 0 && !bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
6355         } while (!bxmitok && issue < 100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
6356
6357         if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
6358                 return _FAIL;
6359         if (!bxmitok) {
6360                 DBG_88E("%s fail! %u ms\n", __func__, rtw_get_passing_time_ms(start));
6361                 return _FAIL;
6362         } else {
6363                 u32 passing_time = rtw_get_passing_time_ms(start);
6364
6365                 if (passing_time > 100 || issue > 3)
6366                         DBG_88E("%s success, issue:%d, poll:%d, %u ms\n", __func__, issue, poll, rtw_get_passing_time_ms(start));
6367                 return _SUCCESS;
6368         }
6369 }
6370
6371 /****************************************************************************
6372
6373 Following are some utility functions for WiFi MLME
6374
6375 *****************************************************************************/
6376
6377 void site_survey(struct adapter *padapter)
6378 {
6379         unsigned char           survey_channel = 0, val8;
6380         enum rt_scan_type ScanType = SCAN_PASSIVE;
6381         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
6382         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6383         u32 initialgain = 0;
6384
6385 #ifdef CONFIG_88EU_P2P
6386         struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
6387
6388         if ((pwdinfo->rx_invitereq_info.scan_op_ch_only) || (pwdinfo->p2p_info.scan_op_ch_only)) {
6389                 if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
6390                         survey_channel = pwdinfo->rx_invitereq_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
6391                 } else {
6392                         survey_channel = pwdinfo->p2p_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
6393                 }
6394                 ScanType = SCAN_ACTIVE;
6395         } else if (rtw_p2p_findphase_ex_is_social(pwdinfo)) {
6396                 /*      Commented by Albert 2011/06/03 */
6397                 /*      The driver is in the find phase, it should go through the social channel. */
6398                 int ch_set_idx;
6399                 survey_channel = pwdinfo->social_chan[pmlmeext->sitesurvey_res.channel_idx];
6400                 ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, survey_channel);
6401                 if (ch_set_idx >= 0)
6402                         ScanType = pmlmeext->channel_set[ch_set_idx].ScanType;
6403                 else
6404                         ScanType = SCAN_ACTIVE;
6405         } else
6406 #endif /* CONFIG_88EU_P2P */
6407         {
6408                 struct rtw_ieee80211_channel *ch;
6409                 if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
6410                         ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
6411                         survey_channel = ch->hw_value;
6412                         ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
6413                 }
6414         }
6415
6416         if (survey_channel != 0) {
6417                 /* PAUSE 4-AC Queue when site_survey */
6418                 /* rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
6419                 /* val8 |= 0x0f; */
6420                 /* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
6421                 if (pmlmeext->sitesurvey_res.channel_idx == 0)
6422                         set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
6423                 else
6424                         SelectChannel(padapter, survey_channel);
6425
6426                 if (ScanType == SCAN_ACTIVE) { /* obey the channel plan setting... */
6427                         #ifdef CONFIG_88EU_P2P
6428                         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) ||
6429                             rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)) {
6430                                 issue_probereq_p2p(padapter, NULL);
6431                                 issue_probereq_p2p(padapter, NULL);
6432                                 issue_probereq_p2p(padapter, NULL);
6433                         } else
6434                         #endif /* CONFIG_88EU_P2P */
6435                         {
6436                                 int i;
6437                                 for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
6438                                         if (pmlmeext->sitesurvey_res.ssid[i].SsidLength) {
6439                                                 /* todo: to issue two probe req??? */
6440                                                 issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
6441                                                 /* msleep(SURVEY_TO>>1); */
6442                                                 issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
6443                                         }
6444                                 }
6445
6446                                 if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
6447                                         /* todo: to issue two probe req??? */
6448                                         issue_probereq(padapter, NULL, NULL);
6449                                         /* msleep(SURVEY_TO>>1); */
6450                                         issue_probereq(padapter, NULL, NULL);
6451                                 }
6452                         }
6453                 }
6454
6455                 set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
6456         } else {
6457                 /*      channel number is 0 or this channel is not valid. */
6458
6459 #ifdef CONFIG_88EU_P2P
6460                 if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)) {
6461                         if ((pwdinfo->rx_invitereq_info.scan_op_ch_only) || (pwdinfo->p2p_info.scan_op_ch_only)) {
6462                                 /*      Set the find_phase_state_exchange_cnt to P2P_FINDPHASE_EX_CNT. */
6463                                 /*      This will let the following flow to run the scanning end. */
6464                                 rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
6465                         }
6466                 }
6467
6468                 if (rtw_p2p_findphase_ex_is_needed(pwdinfo)) {
6469                         /*      Set the P2P State to the listen state of find phase and set the current channel to the listen channel */
6470                         set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
6471                         rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN);
6472                         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
6473
6474                         initialgain = 0xff; /* restore RX GAIN */
6475                         rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
6476                         /* turn on dynamic functions */
6477                         Restore_DM_Func_Flag(padapter);
6478                         /* Switch_DM_Func(padapter, DYNAMIC_FUNC_DIG|DYNAMIC_FUNC_HP|DYNAMIC_FUNC_SS, true); */
6479
6480                         _set_timer(&pwdinfo->find_phase_timer, (u32)((u32)(pwdinfo->listen_dwell) * 100));
6481                 } else
6482 #endif /* CONFIG_88EU_P2P */
6483                 {
6484                         /*  20100721:Interrupt scan operation here. */
6485                         /*  For SW antenna diversity before link, it needs to switch to another antenna and scan again. */
6486                         /*  It compares the scan result and select better one to do connection. */
6487                         if (rtw_hal_antdiv_before_linked(padapter)) {
6488                                 pmlmeext->sitesurvey_res.bss_cnt = 0;
6489                                 pmlmeext->sitesurvey_res.channel_idx = -1;
6490                                 pmlmeext->chan_scan_time = SURVEY_TO / 2;
6491                                 set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
6492                                 return;
6493                         }
6494 #ifdef CONFIG_88EU_P2P
6495                         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
6496                                 rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
6497                         rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
6498 #endif /* CONFIG_88EU_P2P */
6499
6500                         pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
6501
6502                         /* switch back to the original channel */
6503
6504 #ifdef CONFIG_88EU_P2P
6505                         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN))
6506                                 set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
6507                         else
6508                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
6509 #endif /* CONFIG_88EU_P2P */
6510
6511                         /* flush 4-AC Queue after site_survey */
6512                         /* val8 = 0; */
6513                         /* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
6514
6515                         /* config MSR */
6516                         Set_MSR(padapter, (pmlmeinfo->state & 0x3));
6517
6518                         initialgain = 0xff; /* restore RX GAIN */
6519                         rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
6520                         /* turn on dynamic functions */
6521                         Restore_DM_Func_Flag(padapter);
6522                         /* Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); */
6523
6524                         if (is_client_associated_to_ap(padapter))
6525                                 issue_nulldata(padapter, NULL, 0, 3, 500);
6526
6527                         val8 = 0; /* survey done */
6528                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
6529
6530                         report_surveydone_event(padapter);
6531
6532                         pmlmeext->chan_scan_time = SURVEY_TO;
6533                         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
6534
6535                         issue_action_BSSCoexistPacket(padapter);
6536                         issue_action_BSSCoexistPacket(padapter);
6537                         issue_action_BSSCoexistPacket(padapter);
6538                 }
6539         }
6540         return;
6541 }
6542
6543 /* collect bss info from Beacon and Probe request/response frames. */
6544 u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame, struct wlan_bssid_ex *bssid)
6545 {
6546         int     i;
6547         u32     len;
6548         u8 *p;
6549         u16 val16, subtype;
6550         u8 *pframe = precv_frame->rx_data;
6551         u32     packet_len = precv_frame->len;
6552         u8 ie_offset;
6553         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
6554         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
6555         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6556         __le32 le32_tmp;
6557
6558         len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
6559
6560         if (len > MAX_IE_SZ)
6561                 return _FAIL;
6562
6563         _rtw_memset(bssid, 0, sizeof(struct wlan_bssid_ex));
6564
6565         subtype = GetFrameSubType(pframe);
6566
6567         if (subtype == WIFI_BEACON) {
6568                 bssid->Reserved[0] = 1;
6569                 ie_offset = _BEACON_IE_OFFSET_;
6570         } else {
6571                 /*  FIXME : more type */
6572                 if (subtype == WIFI_PROBEREQ) {
6573                         ie_offset = _PROBEREQ_IE_OFFSET_;
6574                         bssid->Reserved[0] = 2;
6575                 } else if (subtype == WIFI_PROBERSP) {
6576                         ie_offset = _PROBERSP_IE_OFFSET_;
6577                         bssid->Reserved[0] = 3;
6578                 } else {
6579                         bssid->Reserved[0] = 0;
6580                         ie_offset = _FIXED_IE_LENGTH_;
6581                 }
6582         }
6583
6584         bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
6585
6586         /* below is to copy the information element */
6587         bssid->IELength = len;
6588         memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength);
6589
6590         /* get the signal strength in dBM.raw data */
6591         bssid->Rssi = precv_frame->attrib.phy_info.recvpower;
6592         bssid->PhyInfo.SignalQuality = precv_frame->attrib.phy_info.SignalQuality;/* in percentage */
6593         bssid->PhyInfo.SignalStrength = precv_frame->attrib.phy_info.SignalStrength;/* in percentage */
6594         rtw_hal_get_def_var(padapter, HAL_DEF_CURRENT_ANTENNA,  &bssid->PhyInfo.Optimum_antenna);
6595
6596         /*  checking SSID */
6597         p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset);
6598         if (p == NULL) {
6599                 DBG_88E("marc: cannot find SSID for survey event\n");
6600                 return _FAIL;
6601         }
6602
6603         if (*(p + 1)) {
6604                 if (len > NDIS_802_11_LENGTH_SSID) {
6605                         DBG_88E("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
6606                         return _FAIL;
6607                 }
6608                 memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
6609                 bssid->Ssid.SsidLength = *(p + 1);
6610         } else {
6611                 bssid->Ssid.SsidLength = 0;
6612         }
6613
6614         _rtw_memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
6615
6616         /* checking rate info... */
6617         i = 0;
6618         p = rtw_get_ie(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
6619         if (p != NULL) {
6620                 if (len > NDIS_802_11_LENGTH_RATES_EX) {
6621                         DBG_88E("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
6622                         return _FAIL;
6623                 }
6624                 memcpy(bssid->SupportedRates, (p + 2), len);
6625                 i = len;
6626         }
6627
6628         p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
6629         if (p != NULL) {
6630                 if (len > (NDIS_802_11_LENGTH_RATES_EX-i)) {
6631                         DBG_88E("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
6632                         return _FAIL;
6633                 }
6634                 memcpy(bssid->SupportedRates + i, (p + 2), len);
6635         }
6636
6637         /* todo: */
6638         bssid->NetworkTypeInUse = Ndis802_11OFDM24;
6639
6640         if (bssid->IELength < 12)
6641                 return _FAIL;
6642
6643         /*  Checking for DSConfig */
6644         p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
6645
6646         bssid->Configuration.DSConfig = 0;
6647         bssid->Configuration.Length = 0;
6648
6649         if (p) {
6650                 bssid->Configuration.DSConfig = *(p + 2);
6651         } else {/*  In 5G, some ap do not have DSSET IE */
6652                 /*  checking HT info for channel */
6653                 p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
6654                 if (p) {
6655                         struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
6656                         bssid->Configuration.DSConfig = HT_info->primary_channel;
6657                 } else { /*  use current channel */
6658                         bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter);
6659                 }
6660         }
6661
6662         if (subtype == WIFI_PROBEREQ) {
6663                 /*  FIXME */
6664                 bssid->InfrastructureMode = Ndis802_11Infrastructure;
6665                 memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
6666                 bssid->Privacy = 1;
6667                 return _SUCCESS;
6668         }
6669
6670         memcpy(&le32_tmp, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
6671         bssid->Configuration.BeaconPeriod = le32_to_cpu(le32_tmp);
6672
6673         val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
6674
6675         if (val16 & BIT(0)) {
6676                 bssid->InfrastructureMode = Ndis802_11Infrastructure;
6677                 memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
6678         } else {
6679                 bssid->InfrastructureMode = Ndis802_11IBSS;
6680                 memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN);
6681         }
6682
6683         if (val16 & BIT(4))
6684                 bssid->Privacy = 1;
6685         else
6686                 bssid->Privacy = 0;
6687
6688         bssid->Configuration.ATIMWindow = 0;
6689
6690         /* 20/40 BSS Coexistence check */
6691         if ((pregistrypriv->wifi_spec == 1) && (!pmlmeinfo->bwmode_updated)) {
6692                 struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
6693                 p = rtw_get_ie(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset);
6694                 if (p && len > 0) {
6695                         struct HT_caps_element  *pHT_caps;
6696                         pHT_caps = (struct HT_caps_element *)(p + 2);
6697
6698                         if (le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info)&BIT(14))
6699                                 pmlmepriv->num_FortyMHzIntolerant++;
6700                 } else {
6701                         pmlmepriv->num_sta_no_ht++;
6702                 }
6703         }
6704
6705         /*  mark bss info receiving from nearby channel as SignalQuality 101 */
6706         if (bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
6707                 bssid->PhyInfo.SignalQuality = 101;
6708         return _SUCCESS;
6709 }
6710
6711 void start_create_ibss(struct adapter *padapter)
6712 {
6713         unsigned short  caps;
6714         u8 val8;
6715         u8 join_type;
6716         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
6717         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6718         struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
6719         pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
6720         pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
6721
6722         /* update wireless mode */
6723         update_wireless_mode(padapter);
6724
6725         /* update capability */
6726         caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork);
6727         update_capinfo(padapter, caps);
6728         if (caps&cap_IBSS) {/* adhoc master */
6729                 val8 = 0xcf;
6730                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
6731
6732                 /* switch channel */
6733                 /* SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */
6734                 set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
6735
6736                 beacon_timing_control(padapter);
6737
6738                 /* set msr to WIFI_FW_ADHOC_STATE */
6739                 pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
6740                 Set_MSR(padapter, (pmlmeinfo->state & 0x3));
6741
6742                 /* issue beacon */
6743                 if (send_beacon(padapter) == _FAIL) {
6744                         RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("issuing beacon frame fail....\n"));
6745
6746                         report_join_res(padapter, -1);
6747                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
6748                 } else {
6749                         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
6750                         join_type = 0;
6751                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
6752
6753                         report_join_res(padapter, 1);
6754                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
6755                 }
6756         } else {
6757                 DBG_88E("start_create_ibss, invalid cap:%x\n", caps);
6758                 return;
6759         }
6760 }
6761
6762 void start_clnt_join(struct adapter *padapter)
6763 {
6764         unsigned short  caps;
6765         u8 val8;
6766         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
6767         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6768         struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
6769         int beacon_timeout;
6770
6771         pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
6772         pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
6773
6774         /* update wireless mode */
6775         update_wireless_mode(padapter);
6776
6777         /* update capability */
6778         caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork);
6779         update_capinfo(padapter, caps);
6780         if (caps&cap_ESS) {
6781                 Set_MSR(padapter, WIFI_FW_STATION_STATE);
6782
6783                 val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf;
6784
6785                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
6786
6787                 /* switch channel */
6788                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
6789
6790                 /* here wait for receiving the beacon to start auth */
6791                 /* and enable a timer */
6792                 beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
6793                 set_link_timer(pmlmeext, beacon_timeout);
6794                 _set_timer(&padapter->mlmepriv.assoc_timer,
6795                            (REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) + beacon_timeout);
6796
6797                 pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
6798         } else if (caps&cap_IBSS) { /* adhoc client */
6799                 Set_MSR(padapter, WIFI_FW_ADHOC_STATE);
6800
6801                 val8 = 0xcf;
6802                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
6803
6804                 /* switch channel */
6805                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
6806
6807                 beacon_timing_control(padapter);
6808
6809                 pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
6810
6811                 report_join_res(padapter, 1);
6812         } else {
6813                 return;
6814         }
6815 }
6816
6817 void start_clnt_auth(struct adapter *padapter)
6818 {
6819         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
6820         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6821
6822         _cancel_timer_ex(&pmlmeext->link_timer);
6823
6824         pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
6825         pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
6826
6827         pmlmeinfo->auth_seq = 1;
6828         pmlmeinfo->reauth_count = 0;
6829         pmlmeinfo->reassoc_count = 0;
6830         pmlmeinfo->link_count = 0;
6831         pmlmeext->retry = 0;
6832
6833
6834         /*  Because of AP's not receiving deauth before */
6835         /*  AP may: 1)not response auth or 2)deauth us after link is complete */
6836         /*  issue deauth before issuing auth to deal with the situation */
6837         /*      Commented by Albert 2012/07/21 */
6838         /*      For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it. */
6839         issue_deauth(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
6840
6841         DBG_88E_LEVEL(_drv_info_, "start auth\n");
6842         issue_auth(padapter, NULL, 0);
6843
6844         set_link_timer(pmlmeext, REAUTH_TO);
6845 }
6846
6847
6848 void start_clnt_assoc(struct adapter *padapter)
6849 {
6850         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
6851         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6852
6853         _cancel_timer_ex(&pmlmeext->link_timer);
6854
6855         pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
6856         pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
6857
6858         issue_assocreq(padapter);
6859
6860         set_link_timer(pmlmeext, REASSOC_TO);
6861 }
6862
6863 unsigned int receive_disconnect(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason)
6864 {
6865         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
6866         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6867
6868         /* check A3 */
6869         if (memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
6870                 return _SUCCESS;
6871
6872         DBG_88E("%s\n", __func__);
6873
6874         if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) {
6875                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
6876                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
6877                         report_del_sta_event(padapter, MacAddr, reason);
6878                 } else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) {
6879                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
6880                         report_join_res(padapter, -2);
6881                 }
6882         }
6883         return _SUCCESS;
6884 }
6885
6886 static void process_80211d(struct adapter *padapter, struct wlan_bssid_ex *bssid)
6887 {
6888         struct registry_priv *pregistrypriv;
6889         struct mlme_ext_priv *pmlmeext;
6890         struct rt_channel_info *chplan_new;
6891         u8 channel;
6892         u8 i;
6893
6894         pregistrypriv = &padapter->registrypriv;
6895         pmlmeext = &padapter->mlmeextpriv;
6896
6897         /*  Adjust channel plan by AP Country IE */
6898         if (pregistrypriv->enable80211d &&
6899             (!pmlmeext->update_channel_plan_by_ap_done)) {
6900                 u8 *ie, *p;
6901                 u32 len;
6902                 struct rt_channel_plan chplan_ap;
6903                 struct rt_channel_info chplan_sta[MAX_CHANNEL_NUM];
6904                 u8 country[4];
6905                 u8 fcn; /*  first channel number */
6906                 u8 noc; /*  number of channel */
6907                 u8 j, k;
6908
6909                 ie = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
6910                 if (!ie)
6911                         return;
6912                 if (len < 6)
6913                         return;
6914                 ie += 2;
6915                 p = ie;
6916                 ie += len;
6917
6918                 _rtw_memset(country, 0, 4);
6919                 memcpy(country, p, 3);
6920                 p += 3;
6921                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
6922                          ("%s: 802.11d country =%s\n", __func__, country));
6923
6924                 i = 0;
6925                 while ((ie - p) >= 3) {
6926                         fcn = *(p++);
6927                         noc = *(p++);
6928                         p++;
6929
6930                         for (j = 0; j < noc; j++) {
6931                                 if (fcn <= 14)
6932                                         channel = fcn + j; /*  2.4 GHz */
6933                                 else
6934                                         channel = fcn + j*4; /*  5 GHz */
6935
6936                                 chplan_ap.Channel[i++] = channel;
6937                         }
6938                 }
6939                 chplan_ap.Len = i;
6940
6941                 memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
6942
6943                 _rtw_memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
6944                 chplan_new = pmlmeext->channel_set;
6945
6946                 i = 0;
6947                 j = 0;
6948                 k = 0;
6949                 if (pregistrypriv->wireless_mode & WIRELESS_11G) {
6950                         do {
6951                                 if ((i == MAX_CHANNEL_NUM) ||
6952                                     (chplan_sta[i].ChannelNum == 0) ||
6953                                     (chplan_sta[i].ChannelNum > 14))
6954                                         break;
6955
6956                                 if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
6957                                         break;
6958
6959                                 if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
6960                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
6961                                         chplan_new[k].ScanType = SCAN_ACTIVE;
6962                                         i++;
6963                                         j++;
6964                                         k++;
6965                                 } else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
6966                                         chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
6967                                         chplan_new[k].ScanType = SCAN_PASSIVE;
6968                                         i++;
6969                                         k++;
6970                                 } else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
6971                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
6972                                         chplan_new[k].ScanType = SCAN_ACTIVE;
6973                                         j++;
6974                                         k++;
6975                                 }
6976                         } while (1);
6977
6978                         /*  change AP not support channel to Passive scan */
6979                         while ((i < MAX_CHANNEL_NUM) &&
6980                                (chplan_sta[i].ChannelNum != 0) &&
6981                                (chplan_sta[i].ChannelNum <= 14)) {
6982                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
6983                                 chplan_new[k].ScanType = SCAN_PASSIVE;
6984                                 i++;
6985                                 k++;
6986                         }
6987
6988                         /*  add channel AP supported */
6989                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
6990                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
6991                                 chplan_new[k].ScanType = SCAN_ACTIVE;
6992                                 j++;
6993                                 k++;
6994                         }
6995                 } else {
6996                         /*  keep original STA 2.4G channel plan */
6997                         while ((i < MAX_CHANNEL_NUM) &&
6998                                (chplan_sta[i].ChannelNum != 0) &&
6999                                (chplan_sta[i].ChannelNum <= 14)) {
7000                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
7001                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
7002                                 i++;
7003                                 k++;
7004                         }
7005
7006                         /*  skip AP 2.4G channel plan */
7007                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
7008                                 j++;
7009                 }
7010
7011                 /*  keep original STA 5G channel plan */
7012                 while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
7013                         chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
7014                         chplan_new[k].ScanType = chplan_sta[i].ScanType;
7015                         i++;
7016                         k++;
7017                 }
7018
7019                 pmlmeext->update_channel_plan_by_ap_done = 1;
7020         }
7021
7022         /*  If channel is used by AP, set channel scan type to active */
7023         channel = bssid->Configuration.DSConfig;
7024         chplan_new = pmlmeext->channel_set;
7025         i = 0;
7026         while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0)) {
7027                 if (chplan_new[i].ChannelNum == channel) {
7028                         if (chplan_new[i].ScanType == SCAN_PASSIVE) {
7029                                 chplan_new[i].ScanType = SCAN_ACTIVE;
7030                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
7031                                          ("%s: change channel %d scan type from passive to active\n",
7032                                          __func__, channel));
7033                         }
7034                         break;
7035                 }
7036                 i++;
7037         }
7038 }
7039
7040 /****************************************************************************
7041
7042 Following are the functions to report events
7043
7044 *****************************************************************************/
7045
7046 void report_survey_event(struct adapter *padapter,
7047                          struct recv_frame *precv_frame)
7048 {
7049         struct cmd_obj *pcmd_obj;
7050         u8 *pevtcmd;
7051         u32 cmdsz;
7052         struct survey_event     *psurvey_evt;
7053         struct C2HEvent_Header *pc2h_evt_hdr;
7054         struct mlme_ext_priv *pmlmeext;
7055         struct cmd_priv *pcmdpriv;
7056
7057         if (!padapter)
7058                 return;
7059
7060         pmlmeext = &padapter->mlmeextpriv;
7061         pcmdpriv = &padapter->cmdpriv;
7062
7063
7064         pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
7065         if (pcmd_obj == NULL)
7066                 return;
7067
7068         cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
7069         pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
7070         if (pevtcmd == NULL) {
7071                 kfree(pcmd_obj);
7072                 return;
7073         }
7074
7075         _rtw_init_listhead(&pcmd_obj->list);
7076
7077         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
7078         pcmd_obj->cmdsz = cmdsz;
7079         pcmd_obj->parmbuf = pevtcmd;
7080
7081         pcmd_obj->rsp = NULL;
7082         pcmd_obj->rspsz  = 0;
7083
7084         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
7085         pc2h_evt_hdr->len = sizeof(struct survey_event);
7086         pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
7087         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
7088
7089         psurvey_evt = (struct survey_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
7090
7091         if (collect_bss_info(padapter, precv_frame, (struct wlan_bssid_ex *)&psurvey_evt->bss) == _FAIL) {
7092                 kfree(pcmd_obj);
7093                 kfree(pevtcmd);
7094                 return;
7095         }
7096
7097         process_80211d(padapter, &psurvey_evt->bss);
7098
7099         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
7100
7101         pmlmeext->sitesurvey_res.bss_cnt++;
7102
7103         return;
7104 }
7105
7106 void report_surveydone_event(struct adapter *padapter)
7107 {
7108         struct cmd_obj *pcmd_obj;
7109         u8 *pevtcmd;
7110         u32 cmdsz;
7111         struct surveydone_event *psurveydone_evt;
7112         struct C2HEvent_Header  *pc2h_evt_hdr;
7113         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
7114         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
7115
7116         pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
7117         if (pcmd_obj == NULL)
7118                 return;
7119
7120         cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
7121         pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
7122         if (pevtcmd == NULL) {
7123                 kfree(pcmd_obj);
7124                 return;
7125         }
7126
7127         _rtw_init_listhead(&pcmd_obj->list);
7128
7129         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
7130         pcmd_obj->cmdsz = cmdsz;
7131         pcmd_obj->parmbuf = pevtcmd;
7132
7133         pcmd_obj->rsp = NULL;
7134         pcmd_obj->rspsz  = 0;
7135
7136         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
7137         pc2h_evt_hdr->len = sizeof(struct surveydone_event);
7138         pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
7139         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
7140
7141         psurveydone_evt = (struct surveydone_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
7142         psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
7143
7144         DBG_88E("survey done event(%x)\n", psurveydone_evt->bss_cnt);
7145
7146         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
7147
7148         return;
7149 }
7150
7151 void report_join_res(struct adapter *padapter, int res)
7152 {
7153         struct cmd_obj *pcmd_obj;
7154         u8 *pevtcmd;
7155         u32 cmdsz;
7156         struct joinbss_event            *pjoinbss_evt;
7157         struct C2HEvent_Header  *pc2h_evt_hdr;
7158         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
7159         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7160         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
7161
7162         pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
7163         if (pcmd_obj == NULL)
7164                 return;
7165
7166         cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
7167         pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
7168         if (pevtcmd == NULL) {
7169                 kfree(pcmd_obj);
7170                 return;
7171         }
7172
7173         _rtw_init_listhead(&pcmd_obj->list);
7174
7175         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
7176         pcmd_obj->cmdsz = cmdsz;
7177         pcmd_obj->parmbuf = pevtcmd;
7178
7179         pcmd_obj->rsp = NULL;
7180         pcmd_obj->rspsz  = 0;
7181
7182         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
7183         pc2h_evt_hdr->len = sizeof(struct joinbss_event);
7184         pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
7185         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
7186
7187         pjoinbss_evt = (struct joinbss_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
7188         memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(struct wlan_bssid_ex));
7189         pjoinbss_evt->network.join_res  = res;
7190         pjoinbss_evt->network.aid = res;
7191
7192         DBG_88E("report_join_res(%d)\n", res);
7193
7194
7195         rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network);
7196
7197
7198         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
7199
7200         return;
7201 }
7202
7203 void report_del_sta_event(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason)
7204 {
7205         struct cmd_obj *pcmd_obj;
7206         u8 *pevtcmd;
7207         u32 cmdsz;
7208         struct sta_info *psta;
7209         int     mac_id;
7210         struct stadel_event                     *pdel_sta_evt;
7211         struct C2HEvent_Header  *pc2h_evt_hdr;
7212         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
7213         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
7214
7215         pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
7216         if (pcmd_obj == NULL)
7217                 return;
7218
7219         cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
7220         pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
7221         if (pevtcmd == NULL) {
7222                 kfree(pcmd_obj);
7223                 return;
7224         }
7225
7226         _rtw_init_listhead(&pcmd_obj->list);
7227
7228         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
7229         pcmd_obj->cmdsz = cmdsz;
7230         pcmd_obj->parmbuf = pevtcmd;
7231
7232         pcmd_obj->rsp = NULL;
7233         pcmd_obj->rspsz  = 0;
7234
7235         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
7236         pc2h_evt_hdr->len = sizeof(struct stadel_event);
7237         pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
7238         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
7239
7240         pdel_sta_evt = (struct stadel_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
7241         memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
7242         memcpy((unsigned char *)(pdel_sta_evt->rsvd), (unsigned char *)(&reason), 2);
7243
7244
7245         psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
7246         if (psta)
7247                 mac_id = (int)psta->mac_id;
7248         else
7249                 mac_id = (-1);
7250
7251         pdel_sta_evt->mac_id = mac_id;
7252
7253         DBG_88E("report_del_sta_event: delete STA, mac_id =%d\n", mac_id);
7254
7255         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
7256
7257         return;
7258 }
7259
7260 void report_add_sta_event(struct adapter *padapter, unsigned char *MacAddr, int cam_idx)
7261 {
7262         struct cmd_obj *pcmd_obj;
7263         u8 *pevtcmd;
7264         u32 cmdsz;
7265         struct stassoc_event            *padd_sta_evt;
7266         struct C2HEvent_Header  *pc2h_evt_hdr;
7267         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
7268         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
7269
7270         pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
7271         if (pcmd_obj == NULL)
7272                 return;
7273
7274         cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
7275         pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
7276         if (pevtcmd == NULL) {
7277                 kfree(pcmd_obj);
7278                 return;
7279         }
7280
7281         _rtw_init_listhead(&pcmd_obj->list);
7282
7283         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
7284         pcmd_obj->cmdsz = cmdsz;
7285         pcmd_obj->parmbuf = pevtcmd;
7286
7287         pcmd_obj->rsp = NULL;
7288         pcmd_obj->rspsz  = 0;
7289
7290         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
7291         pc2h_evt_hdr->len = sizeof(struct stassoc_event);
7292         pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
7293         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
7294
7295         padd_sta_evt = (struct stassoc_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
7296         memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN);
7297         padd_sta_evt->cam_id = cam_idx;
7298
7299         DBG_88E("report_add_sta_event: add STA\n");
7300
7301         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
7302
7303         return;
7304 }
7305
7306
7307 /****************************************************************************
7308
7309 Following are the event callback functions
7310
7311 *****************************************************************************/
7312
7313 /* for sta/adhoc mode */
7314 void update_sta_info(struct adapter *padapter, struct sta_info *psta)
7315 {
7316         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
7317         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
7318         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7319
7320         /* ERP */
7321         VCS_update(padapter, psta);
7322
7323         /* HT */
7324         if (pmlmepriv->htpriv.ht_option) {
7325                 psta->htpriv.ht_option = true;
7326
7327                 psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
7328
7329                 if (support_short_GI(padapter, &(pmlmeinfo->HT_caps)))
7330                         psta->htpriv.sgi = true;
7331
7332                 psta->qos_option = true;
7333         } else {
7334                 psta->htpriv.ht_option = false;
7335
7336                 psta->htpriv.ampdu_enable = false;
7337
7338                 psta->htpriv.sgi = false;
7339                 psta->qos_option = false;
7340         }
7341         psta->htpriv.bwmode = pmlmeext->cur_bwmode;
7342         psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
7343
7344         psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
7345         psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
7346
7347         /* QoS */
7348         if (pmlmepriv->qospriv.qos_option)
7349                 psta->qos_option = true;
7350
7351
7352         psta->state = _FW_LINKED;
7353 }
7354
7355 void mlmeext_joinbss_event_callback(struct adapter *padapter, int join_res)
7356 {
7357         struct sta_info         *psta, *psta_bmc;
7358         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
7359         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7360         struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
7361         struct sta_priv         *pstapriv = &padapter->stapriv;
7362         u8 join_type;
7363         u16 media_status;
7364
7365         if (join_res < 0) {
7366                 join_type = 1;
7367                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
7368                 rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
7369
7370                 /* restore to initial setting. */
7371                 update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
7372
7373                 goto exit_mlmeext_joinbss_event_callback;
7374         }
7375
7376         if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
7377                 /* for bc/mc */
7378                 psta_bmc = rtw_get_bcmc_stainfo(padapter);
7379                 if (psta_bmc) {
7380                         pmlmeinfo->FW_sta_info[psta_bmc->mac_id].psta = psta_bmc;
7381                         update_bmc_sta_support_rate(padapter, psta_bmc->mac_id);
7382                         Update_RA_Entry(padapter, psta_bmc->mac_id);
7383                 }
7384         }
7385
7386
7387         /* turn on dynamic functions */
7388         Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true);
7389
7390         /*  update IOT-related issue */
7391         update_IOT_info(padapter);
7392
7393         rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates);
7394
7395         /* BCN interval */
7396         rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
7397
7398         /* update capability */
7399         update_capinfo(padapter, pmlmeinfo->capability);
7400
7401         /* WMM, Update EDCA param */
7402         WMMOnAssocRsp(padapter);
7403
7404         /* HT */
7405         HTOnAssocRsp(padapter);
7406
7407         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
7408
7409         psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
7410         if (psta) { /* only for infra. mode */
7411                 pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
7412
7413                 psta->wireless_mode = pmlmeext->cur_wireless_mode;
7414
7415                 /* set per sta rate after updating HT cap. */
7416                 set_sta_rate(padapter, psta);
7417                 rtw_hal_set_hwreg(padapter, HW_VAR_TX_RPT_MAX_MACID, (u8 *)&psta->mac_id);
7418                 media_status = (psta->mac_id<<8)|1; /*   MACID|OPMODE: 1 means connect */
7419                 rtw_hal_set_hwreg(padapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
7420         }
7421
7422         join_type = 2;
7423         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
7424
7425         if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) {
7426                 /*  correcting TSF */
7427                 correct_TSF(padapter, pmlmeext);
7428         }
7429         rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
7430
7431 exit_mlmeext_joinbss_event_callback:
7432
7433         DBG_88E("=>%s\n", __func__);
7434 }
7435
7436 void mlmeext_sta_add_event_callback(struct adapter *padapter, struct sta_info *psta)
7437 {
7438         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7439         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7440         u8 join_type;
7441
7442         DBG_88E("%s\n", __func__);
7443
7444         if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
7445                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {/* adhoc master or sta_count>1 */
7446                         /* nothing to do */
7447                 } else { /* adhoc client */
7448                         /*  correcting TSF */
7449                         correct_TSF(padapter, pmlmeext);
7450
7451                         /* start beacon */
7452                         if (send_beacon(padapter) == _FAIL) {
7453                                 pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
7454                                 pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
7455                                 return;
7456                         }
7457                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
7458                 }
7459
7460                 join_type = 2;
7461                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
7462         }
7463
7464         pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
7465
7466         /* rate radaptive */
7467         Update_RA_Entry(padapter, psta->mac_id);
7468
7469         /* update adhoc sta_info */
7470         update_sta_info(padapter, psta);
7471 }
7472
7473 void mlmeext_sta_del_event_callback(struct adapter *padapter)
7474 {
7475         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
7476         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7477
7478         if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter)) {
7479                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
7480                 rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
7481
7482                 /* restore to initial setting. */
7483                 update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
7484
7485                 /* switch to the 20M Hz mode after disconnect */
7486                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
7487                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
7488
7489                 /* SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset); */
7490                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
7491
7492
7493                 flush_all_cam_entry(padapter);
7494
7495                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
7496
7497                 /* set MSR to no link state -> infra. mode */
7498                 Set_MSR(padapter, _HW_STATE_STATION_);
7499
7500                 _cancel_timer_ex(&pmlmeext->link_timer);
7501         }
7502 }
7503
7504 /****************************************************************************
7505
7506 Following are the functions for the timer handlers
7507
7508 *****************************************************************************/
7509 void _linked_rx_signal_strehgth_display(struct adapter *padapter);
7510 void _linked_rx_signal_strehgth_display(struct adapter *padapter)
7511 {
7512         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
7513       struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7514         u8 mac_id;
7515         int UndecoratedSmoothedPWDB;
7516         if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
7517                 mac_id = 0;
7518         else if ((pmlmeinfo->state&0x03) == _HW_STATE_AP_)
7519                 mac_id = 2;
7520
7521         rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, &mac_id);
7522
7523         rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
7524         DBG_88E("UndecoratedSmoothedPWDB:%d\n", UndecoratedSmoothedPWDB);
7525 }
7526
7527 static u8 chk_ap_is_alive(struct adapter *padapter, struct sta_info *psta)
7528 {
7529         u8 ret = false;
7530
7531         if ((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta)) &&
7532             sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta) &&
7533             sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta))
7534                 ret = false;
7535         else
7536                 ret = true;
7537
7538         sta_update_last_rx_pkts(psta);
7539
7540         return ret;
7541 }
7542
7543 void linked_status_chk(struct adapter *padapter)
7544 {
7545         u32     i;
7546         struct sta_info         *psta;
7547         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
7548         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
7549         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7550         struct sta_priv         *pstapriv = &padapter->stapriv;
7551
7552         if (padapter->bRxRSSIDisplay)
7553                 _linked_rx_signal_strehgth_display(padapter);
7554
7555         rtw_hal_sreset_linked_status_check(padapter);
7556
7557         if (is_client_associated_to_ap(padapter)) {
7558                 /* linked infrastructure client mode */
7559
7560                 int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
7561                 int rx_chk_limit;
7562
7563                 rx_chk_limit = 4;
7564                 psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
7565                 if (psta != NULL) {
7566                         bool is_p2p_enable = false;
7567                         #ifdef CONFIG_88EU_P2P
7568                         is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
7569                         #endif
7570
7571                         if (!chk_ap_is_alive(padapter, psta))
7572                                 rx_chk = _FAIL;
7573
7574                         if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
7575                                 tx_chk = _FAIL;
7576
7577                         if (pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL)) {
7578                                 u8 backup_oper_channel = 0;
7579
7580                                 /* switch to correct channel of current network  before issue keep-alive frames */
7581                                 if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
7582                                         backup_oper_channel = rtw_get_oper_ch(padapter);
7583                                         SelectChannel(padapter, pmlmeext->cur_channel);
7584                                 }
7585
7586                                 if (rx_chk != _SUCCESS)
7587                                         issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
7588
7589                                 if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) || rx_chk != _SUCCESS) {
7590                                         tx_chk = issue_nulldata(padapter, psta->hwaddr, 0, 3, 1);
7591                                         /* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */
7592                                         if (tx_chk == _SUCCESS && !is_p2p_enable)
7593                                                 rx_chk = _SUCCESS;
7594                                 }
7595
7596                                 /* back to the original operation channel */
7597                                 if (backup_oper_channel > 0)
7598                                         SelectChannel(padapter, backup_oper_channel);
7599                         } else {
7600                                 if (rx_chk != _SUCCESS) {
7601                                         if (pmlmeext->retry == 0) {
7602                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
7603                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
7604                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
7605                                         }
7606                                 }
7607
7608                                 if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) {
7609                                         tx_chk = issue_nulldata(padapter, NULL, 0, 1, 0);
7610                                 }
7611                         }
7612
7613                         if (rx_chk == _FAIL) {
7614                                 pmlmeext->retry++;
7615                                 if (pmlmeext->retry > rx_chk_limit) {
7616                                         DBG_88E_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n",
7617                                                       FUNC_ADPT_ARG(padapter));
7618                                         receive_disconnect(padapter, pmlmeinfo->network.MacAddress,
7619                                                            WLAN_REASON_EXPIRATION_CHK);
7620                                         return;
7621                                 }
7622                         } else {
7623                                 pmlmeext->retry = 0;
7624                         }
7625
7626                         if (tx_chk == _FAIL) {
7627                                 pmlmeinfo->link_count &= 0xf;
7628                         } else {
7629                                 pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
7630                                 pmlmeinfo->link_count = 0;
7631                         }
7632                 } /* end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL) */
7633         } else if (is_client_associated_to_ibss(padapter)) {
7634                 /* linked IBSS mode */
7635                 /* for each assoc list entry to check the rx pkt counter */
7636                 for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
7637                         if (pmlmeinfo->FW_sta_info[i].status == 1) {
7638                                 psta = pmlmeinfo->FW_sta_info[i].psta;
7639
7640                                 if (NULL == psta)
7641                                         continue;
7642                                 if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta)) {
7643                                         if (pmlmeinfo->FW_sta_info[i].retry < 3) {
7644                                                 pmlmeinfo->FW_sta_info[i].retry++;
7645                                         } else {
7646                                                 pmlmeinfo->FW_sta_info[i].retry = 0;
7647                                                 pmlmeinfo->FW_sta_info[i].status = 0;
7648                                                 report_del_sta_event(padapter, psta->hwaddr
7649                                                         , 65535/*  indicate disconnect caused by no rx */
7650                                         );
7651                                         }
7652                                 } else {
7653                                         pmlmeinfo->FW_sta_info[i].retry = 0;
7654                                         pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
7655                                 }
7656                         }
7657                 }
7658         }
7659 }
7660
7661 void survey_timer_hdl(struct adapter *padapter)
7662 {
7663         struct cmd_obj  *ph2c;
7664         struct sitesurvey_parm  *psurveyPara;
7665         struct cmd_priv                                 *pcmdpriv = &padapter->cmdpriv;
7666         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
7667 #ifdef CONFIG_88EU_P2P
7668         struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
7669 #endif
7670
7671         /* issue rtw_sitesurvey_cmd */
7672         if (pmlmeext->sitesurvey_res.state > SCAN_START) {
7673                 if (pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
7674                         pmlmeext->sitesurvey_res.channel_idx++;
7675
7676                 if (pmlmeext->scan_abort) {
7677                         #ifdef CONFIG_88EU_P2P
7678                         if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE)) {
7679                                 rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
7680                                 pmlmeext->sitesurvey_res.channel_idx = 3;
7681                                 DBG_88E("%s idx:%d, cnt:%u\n", __func__
7682                                         , pmlmeext->sitesurvey_res.channel_idx
7683                                         , pwdinfo->find_phase_state_exchange_cnt
7684                         );
7685                         } else
7686                         #endif
7687                         {
7688                                 pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
7689                                 DBG_88E("%s idx:%d\n", __func__
7690                                         , pmlmeext->sitesurvey_res.channel_idx
7691                         );
7692                         }
7693
7694                         pmlmeext->scan_abort = false;/* reset */
7695                 }
7696
7697                 ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
7698                 if (ph2c == NULL)
7699                         goto exit_survey_timer_hdl;
7700
7701                 psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm));
7702                 if (psurveyPara == NULL) {
7703                         kfree(ph2c);
7704                         goto exit_survey_timer_hdl;
7705                 }
7706
7707                 init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
7708                 rtw_enqueue_cmd(pcmdpriv, ph2c);
7709         }
7710
7711
7712 exit_survey_timer_hdl:
7713         return;
7714 }
7715
7716 void link_timer_hdl(struct adapter *padapter)
7717 {
7718         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
7719         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7720
7721         if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
7722                 DBG_88E("link_timer_hdl:no beacon while connecting\n");
7723                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
7724                 report_join_res(padapter, -3);
7725         } else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) {
7726                 /* re-auth timer */
7727                 if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) {
7728                         pmlmeinfo->state = 0;
7729                         report_join_res(padapter, -1);
7730                         return;
7731                 }
7732
7733                 DBG_88E("link_timer_hdl: auth timeout and try again\n");
7734                 pmlmeinfo->auth_seq = 1;
7735                 issue_auth(padapter, NULL, 0);
7736                 set_link_timer(pmlmeext, REAUTH_TO);
7737         } else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) {
7738                 /* re-assoc timer */
7739                 if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) {
7740                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
7741                         report_join_res(padapter, -2);
7742                         return;
7743                 }
7744
7745                 DBG_88E("link_timer_hdl: assoc timeout and try again\n");
7746                 issue_assocreq(padapter);
7747                 set_link_timer(pmlmeext, REASSOC_TO);
7748         }
7749         return;
7750 }
7751
7752 void addba_timer_hdl(struct sta_info *psta)
7753 {
7754         struct ht_priv  *phtpriv;
7755
7756         if (!psta)
7757                 return;
7758
7759         phtpriv = &psta->htpriv;
7760
7761         if ((phtpriv->ht_option) && (phtpriv->ampdu_enable)) {
7762                 if (phtpriv->candidate_tid_bitmap)
7763                         phtpriv->candidate_tid_bitmap = 0x0;
7764         }
7765 }
7766
7767 u8 NULL_hdl(struct adapter *padapter, u8 *pbuf)
7768 {
7769         return H2C_SUCCESS;
7770 }
7771
7772 u8 setopmode_hdl(struct adapter *padapter, u8 *pbuf)
7773 {
7774         u8 type;
7775         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
7776         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7777         struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
7778
7779         if (psetop->mode == Ndis802_11APMode) {
7780                 pmlmeinfo->state = WIFI_FW_AP_STATE;
7781                 type = _HW_STATE_AP_;
7782         } else if (psetop->mode == Ndis802_11Infrastructure) {
7783                 pmlmeinfo->state &= ~(BIT(0)|BIT(1));/*  clear state */
7784                 pmlmeinfo->state |= WIFI_FW_STATION_STATE;/* set to     STATION_STATE */
7785                 type = _HW_STATE_STATION_;
7786         } else if (psetop->mode == Ndis802_11IBSS) {
7787                 type = _HW_STATE_ADHOC_;
7788         } else {
7789                 type = _HW_STATE_NOLINK_;
7790         }
7791
7792         rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
7793         /* Set_NETYPE0_MSR(padapter, type); */
7794
7795         return H2C_SUCCESS;
7796 }
7797
7798 u8 createbss_hdl(struct adapter *padapter, u8 *pbuf)
7799 {
7800         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
7801         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7802         struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
7803         struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
7804         /* u32  initialgain; */
7805
7806
7807         if (pparm->network.InfrastructureMode == Ndis802_11APMode) {
7808 #ifdef CONFIG_88EU_AP_MODE
7809
7810                 if (pmlmeinfo->state == WIFI_FW_AP_STATE) {
7811                         /* todo: */
7812                         return H2C_SUCCESS;
7813                 }
7814 #endif
7815         }
7816
7817         /* below is for ad-hoc master */
7818         if (pparm->network.InfrastructureMode == Ndis802_11IBSS) {
7819                 rtw_joinbss_reset(padapter);
7820
7821                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
7822                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
7823                 pmlmeinfo->ERP_enable = 0;
7824                 pmlmeinfo->WMM_enable = 0;
7825                 pmlmeinfo->HT_enable = 0;
7826                 pmlmeinfo->HT_caps_enable = 0;
7827                 pmlmeinfo->HT_info_enable = 0;
7828                 pmlmeinfo->agg_enable_bitmap = 0;
7829                 pmlmeinfo->candidate_tid_bitmap = 0;
7830
7831                 /* disable dynamic functions, such as high power, DIG */
7832                 Save_DM_Func_Flag(padapter);
7833                 Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false);
7834
7835                 /* config the initial gain under linking, need to write the BB registers */
7836                 /* initialgain = 0x1E; */
7837                 /* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
7838
7839                 /* cancel link timer */
7840                 _cancel_timer_ex(&pmlmeext->link_timer);
7841
7842                 /* clear CAM */
7843                 flush_all_cam_entry(padapter);
7844
7845                 memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, IELength));
7846                 pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength;
7847
7848                 if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
7849                         return H2C_PARAMETERS_ERROR;
7850
7851                 memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength);
7852
7853                 start_create_ibss(padapter);
7854         }
7855
7856         return H2C_SUCCESS;
7857 }
7858
7859 u8 join_cmd_hdl(struct adapter *padapter, u8 *pbuf)
7860 {
7861         u8 join_type;
7862         struct ndis_802_11_var_ie *pIE;
7863         struct registry_priv    *pregpriv = &padapter->registrypriv;
7864         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
7865         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7866         struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
7867         struct joinbss_parm     *pparm = (struct joinbss_parm *)pbuf;
7868         u32 i;
7869
7870         /* check already connecting to AP or not */
7871         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
7872                 if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
7873                         issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 5, 100);
7874
7875                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
7876
7877                 /* clear CAM */
7878                 flush_all_cam_entry(padapter);
7879
7880                 _cancel_timer_ex(&pmlmeext->link_timer);
7881
7882                 /* set MSR to nolink -> infra. mode */
7883                 Set_MSR(padapter, _HW_STATE_STATION_);
7884
7885
7886                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
7887         }
7888
7889         rtw_antenna_select_cmd(padapter, pparm->network.PhyInfo.Optimum_antenna, false);
7890
7891         rtw_joinbss_reset(padapter);
7892
7893         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
7894         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
7895         pmlmeinfo->ERP_enable = 0;
7896         pmlmeinfo->WMM_enable = 0;
7897         pmlmeinfo->HT_enable = 0;
7898         pmlmeinfo->HT_caps_enable = 0;
7899         pmlmeinfo->HT_info_enable = 0;
7900         pmlmeinfo->agg_enable_bitmap = 0;
7901         pmlmeinfo->candidate_tid_bitmap = 0;
7902         pmlmeinfo->bwmode_updated = false;
7903
7904         memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, IELength));
7905         pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength;
7906
7907         if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
7908                 return H2C_PARAMETERS_ERROR;
7909
7910         memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength);
7911
7912         /* Check AP vendor to move rtw_joinbss_cmd() */
7913
7914         for (i = sizeof(struct ndis_802_11_fixed_ie); i < pnetwork->IELength;) {
7915                 pIE = (struct ndis_802_11_var_ie *)(pnetwork->IEs + i);
7916
7917                 switch (pIE->ElementID) {
7918                 case _VENDOR_SPECIFIC_IE_:/* Get WMM IE. */
7919                         if (!memcmp(pIE->data, WMM_OUI, 4))
7920                                 pmlmeinfo->WMM_enable = 1;
7921                         break;
7922                 case _HT_CAPABILITY_IE_:        /* Get HT Cap IE. */
7923                         pmlmeinfo->HT_caps_enable = 1;
7924                         break;
7925                 case _HT_EXTRA_INFO_IE_:        /* Get HT Info IE. */
7926                         pmlmeinfo->HT_info_enable = 1;
7927
7928                         /* spec case only for cisco's ap because cisco's ap issue assoc rsp using mcs rate @40MHz or @20MHz */
7929                         {
7930                                 struct HT_info_element *pht_info = (struct HT_info_element *)(pIE->data);
7931
7932                                 if ((pregpriv->cbw40_enable) &&  (pht_info->infos[0] & BIT(2))) {
7933                                         /* switch to the 40M Hz mode according to the AP */
7934                                         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
7935                                         switch (pht_info->infos[0] & 0x3) {
7936                                         case 1:
7937                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
7938                                                 break;
7939                                         case 3:
7940                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
7941                                                 break;
7942                                         default:
7943                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
7944                                                 break;
7945                                 }
7946
7947                                         DBG_88E("set ch/bw before connected\n");
7948                                 }
7949                         }
7950                         break;
7951                 default:
7952                         break;
7953                 }
7954
7955                 i += (pIE->Length + 2);
7956         }
7957         /* disable dynamic functions, such as high power, DIG */
7958
7959         /* config the initial gain under linking, need to write the BB registers */
7960
7961         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
7962         join_type = 0;
7963         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
7964
7965         /* cancel link timer */
7966         _cancel_timer_ex(&pmlmeext->link_timer);
7967
7968         start_clnt_join(padapter);
7969
7970         return H2C_SUCCESS;
7971 }
7972
7973 u8 disconnect_hdl(struct adapter *padapter, unsigned char *pbuf)
7974 {
7975         struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
7976         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
7977         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7978         struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
7979         u8 val8;
7980
7981         if (is_client_associated_to_ap(padapter))
7982                 issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100);
7983
7984         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
7985         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
7986
7987         /* restore to initial setting. */
7988         update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
7989
7990         if (((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)) {
7991                 /* Stop BCN */
7992                 val8 = 0;
7993                 rtw_hal_set_hwreg(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8));
7994         }
7995
7996
7997         /* set MSR to no link state -> infra. mode */
7998         Set_MSR(padapter, _HW_STATE_STATION_);
7999
8000         pmlmeinfo->state = WIFI_FW_NULL_STATE;
8001
8002         /* switch to the 20M Hz mode after disconnect */
8003         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
8004         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
8005
8006         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
8007
8008         flush_all_cam_entry(padapter);
8009
8010         _cancel_timer_ex(&pmlmeext->link_timer);
8011
8012         rtw_free_uc_swdec_pending_queue(padapter);
8013
8014         return  H2C_SUCCESS;
8015 }
8016
8017 static int rtw_scan_ch_decision(struct adapter *padapter, struct rtw_ieee80211_channel *out,
8018         u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
8019 {
8020         int i, j;
8021         int set_idx;
8022         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8023
8024         /* clear out first */
8025         _rtw_memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num);
8026
8027         /* acquire channels from in */
8028         j = 0;
8029         for (i = 0; i < in_num; i++) {
8030                 set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value);
8031                 if (in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED) &&
8032                     set_idx >= 0) {
8033                         out[j] = in[i];
8034
8035                         if (pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
8036                                 out[j].flags &= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
8037
8038                         j++;
8039                 }
8040                 if (j >= out_num)
8041                         break;
8042         }
8043
8044         /* if out is empty, use channel_set as default */
8045         if (j == 0) {
8046                 for (i = 0; i < pmlmeext->max_chan_nums; i++) {
8047                         out[i].hw_value = pmlmeext->channel_set[i].ChannelNum;
8048
8049                         if (pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
8050                                 out[i].flags &= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
8051
8052                         j++;
8053                 }
8054         }
8055
8056         return j;
8057 }
8058
8059 u8 sitesurvey_cmd_hdl(struct adapter *padapter, u8 *pbuf)
8060 {
8061         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8062         struct sitesurvey_parm  *pparm = (struct sitesurvey_parm *)pbuf;
8063         u8 bdelayscan = false;
8064         u8 val8;
8065         u32     initialgain;
8066         u32     i;
8067
8068 #ifdef CONFIG_88EU_P2P
8069         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
8070 #endif
8071
8072         if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE) {
8073                 /* for first time sitesurvey_cmd */
8074                 rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, NULL);
8075
8076                 pmlmeext->sitesurvey_res.state = SCAN_START;
8077                 pmlmeext->sitesurvey_res.bss_cnt = 0;
8078                 pmlmeext->sitesurvey_res.channel_idx = 0;
8079
8080                 for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
8081                         if (pparm->ssid[i].SsidLength) {
8082                                 memcpy(pmlmeext->sitesurvey_res.ssid[i].Ssid, pparm->ssid[i].Ssid, IW_ESSID_MAX_SIZE);
8083                                 pmlmeext->sitesurvey_res.ssid[i].SsidLength = pparm->ssid[i].SsidLength;
8084                         } else {
8085                                 pmlmeext->sitesurvey_res.ssid[i].SsidLength = 0;
8086                         }
8087                 }
8088
8089                 pmlmeext->sitesurvey_res.ch_num = rtw_scan_ch_decision(padapter
8090                         , pmlmeext->sitesurvey_res.ch, RTW_CHANNEL_SCAN_AMOUNT
8091                         , pparm->ch, pparm->ch_num
8092         );
8093
8094                 pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
8095
8096                 /* issue null data if associating to the AP */
8097                 if (is_client_associated_to_ap(padapter)) {
8098                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
8099
8100                         issue_nulldata(padapter, NULL, 1, 3, 500);
8101
8102                         bdelayscan = true;
8103                 }
8104                 if (bdelayscan) {
8105                         /* delay 50ms to protect nulldata(1). */
8106                         set_survey_timer(pmlmeext, 50);
8107                         return H2C_SUCCESS;
8108                 }
8109         }
8110
8111         if ((pmlmeext->sitesurvey_res.state == SCAN_START) || (pmlmeext->sitesurvey_res.state == SCAN_TXNULL)) {
8112                 /* disable dynamic functions, such as high power, DIG */
8113                 Save_DM_Func_Flag(padapter);
8114                 Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false);
8115
8116                 /* config the initial gain under scanning, need to write the BB registers */
8117 #ifdef CONFIG_88EU_P2P
8118                 if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
8119                         initialgain = 0x1E;
8120                 else
8121                         initialgain = 0x28;
8122 #else   /*  CONFIG_88EU_P2P */
8123                 initialgain = 0x1E;
8124 #endif /*  CONFIG_88EU_P2P */
8125
8126                 rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
8127
8128                 /* set MSR to no link state */
8129                 Set_MSR(padapter, _HW_STATE_NOLINK_);
8130
8131                 val8 = 1; /* under site survey */
8132                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
8133
8134                 pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
8135         }
8136
8137         site_survey(padapter);
8138
8139         return H2C_SUCCESS;
8140 }
8141
8142 u8 setauth_hdl(struct adapter *padapter, unsigned char *pbuf)
8143 {
8144         struct setauth_parm             *pparm = (struct setauth_parm *)pbuf;
8145         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8146         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8147
8148         if (pparm->mode < 4)
8149                 pmlmeinfo->auth_algo = pparm->mode;
8150         return  H2C_SUCCESS;
8151 }
8152
8153 u8 setkey_hdl(struct adapter *padapter, u8 *pbuf)
8154 {
8155         unsigned short                          ctrl;
8156         struct setkey_parm              *pparm = (struct setkey_parm *)pbuf;
8157         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8158         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8159         unsigned char                                   null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
8160
8161         /* main tx key for wep. */
8162         if (pparm->set_tx)
8163                 pmlmeinfo->key_index = pparm->keyid;
8164
8165         /* write cam */
8166         ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;
8167
8168         DBG_88E_LEVEL(_drv_info_, "set group key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) "
8169                         "keyid:%d\n", pparm->algorithm, pparm->keyid);
8170         write_cam(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
8171
8172         return H2C_SUCCESS;
8173 }
8174
8175 u8 set_stakey_hdl(struct adapter *padapter, u8 *pbuf)
8176 {
8177         u16 ctrl = 0;
8178         u8 cam_id;/* cam_entry */
8179         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8180         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8181         struct set_stakey_parm  *pparm = (struct set_stakey_parm *)pbuf;
8182
8183         /* cam_entry: */
8184         /* 0~3 for default key */
8185
8186         /* for concurrent mode (ap+sta): */
8187         /* default key is disable, using sw encrypt/decrypt */
8188         /* cam_entry = 4 for sta mode (macid = 0) */
8189         /* cam_entry(macid+3) = 5 ~ N for ap mode (aid = 1~N, macid = 2 ~N) */
8190
8191         /* for concurrent mode (sta+sta): */
8192         /* default key is disable, using sw encrypt/decrypt */
8193         /* cam_entry = 4 mapping to macid = 0 */
8194         /* cam_entry = 5 mapping to macid = 2 */
8195
8196         cam_id = 4;
8197
8198         DBG_88E_LEVEL(_drv_info_, "set pairwise key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) camid:%d\n",
8199                       pparm->algorithm, cam_id);
8200         if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
8201                 struct sta_info *psta;
8202                 struct sta_priv *pstapriv = &padapter->stapriv;
8203
8204                 if (pparm->algorithm == _NO_PRIVACY_)   /*  clear cam entry */ {
8205                         clear_cam_entry(padapter, pparm->id);
8206                         return H2C_SUCCESS_RSP;
8207                 }
8208
8209                 psta = rtw_get_stainfo(pstapriv, pparm->addr);
8210                 if (psta) {
8211                         ctrl = (BIT(15) | ((pparm->algorithm) << 2));
8212
8213                         DBG_88E("r871x_set_stakey_hdl(): enc_algorithm=%d\n", pparm->algorithm);
8214
8215                         if ((psta->mac_id < 1) || (psta->mac_id > (NUM_STA-4))) {
8216                                 DBG_88E("r871x_set_stakey_hdl():set_stakey failed, mac_id(aid)=%d\n", psta->mac_id);
8217                                 return H2C_REJECTED;
8218                         }
8219
8220                         cam_id = (psta->mac_id + 3);/* 0~3 for default key, cmd_id = macid + 3, macid = aid+1; */
8221
8222                         DBG_88E("Write CAM, mac_addr =%x:%x:%x:%x:%x:%x, cam_entry=%d\n", pparm->addr[0],
8223                                 pparm->addr[1], pparm->addr[2], pparm->addr[3], pparm->addr[4],
8224                                 pparm->addr[5], cam_id);
8225
8226                         write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
8227
8228                         return H2C_SUCCESS_RSP;
8229                 } else {
8230                         DBG_88E("r871x_set_stakey_hdl(): sta has been free\n");
8231                         return H2C_REJECTED;
8232                 }
8233         }
8234
8235         /* below for sta mode */
8236
8237         if (pparm->algorithm == _NO_PRIVACY_) { /*  clear cam entry */
8238                 clear_cam_entry(padapter, pparm->id);
8239                 return H2C_SUCCESS;
8240         }
8241         ctrl = BIT(15) | ((pparm->algorithm) << 2);
8242         write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
8243         pmlmeinfo->enc_algo = pparm->algorithm;
8244         return H2C_SUCCESS;
8245 }
8246
8247 u8 add_ba_hdl(struct adapter *padapter, unsigned char *pbuf)
8248 {
8249         struct addBaReq_parm    *pparm = (struct addBaReq_parm *)pbuf;
8250         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8251         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8252
8253         struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr);
8254
8255         if (!psta)
8256                 return  H2C_SUCCESS;
8257
8258         if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
8259             ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)) {
8260                 issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
8261                 _set_timer(&psta->addba_retry_timer, ADDBA_TO);
8262         } else {
8263                 psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
8264         }
8265         return  H2C_SUCCESS;
8266 }
8267
8268 u8 set_tx_beacon_cmd(struct adapter *padapter)
8269 {
8270         struct cmd_obj  *ph2c;
8271         struct Tx_Beacon_param  *ptxBeacon_parm;
8272         struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
8273         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8274         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8275         u8 res = _SUCCESS;
8276         int len_diff = 0;
8277
8278
8279         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
8280         if (ph2c == NULL) {
8281                 res = _FAIL;
8282                 goto exit;
8283         }
8284
8285         ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param));
8286         if (ptxBeacon_parm == NULL) {
8287                 kfree(ph2c);
8288                 res = _FAIL;
8289                 goto exit;
8290         }
8291
8292         memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(struct wlan_bssid_ex));
8293
8294         len_diff = update_hidden_ssid(ptxBeacon_parm->network.IEs+_BEACON_IE_OFFSET_,
8295                                       ptxBeacon_parm->network.IELength-_BEACON_IE_OFFSET_,
8296                                       pmlmeinfo->hidden_ssid_mode);
8297         ptxBeacon_parm->network.IELength += len_diff;
8298
8299         init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
8300
8301         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
8302
8303
8304 exit:
8305
8306
8307         return res;
8308 }
8309
8310 u8 mlme_evt_hdl(struct adapter *padapter, unsigned char *pbuf)
8311 {
8312         u8 evt_code;
8313         u16 evt_sz;
8314         uint    *peventbuf;
8315         void (*event_callback)(struct adapter *dev, u8 *pbuf);
8316         struct evt_priv *pevt_priv = &(padapter->evtpriv);
8317
8318         peventbuf = (uint *)pbuf;
8319         evt_sz = (u16)(*peventbuf&0xffff);
8320         evt_code = (u8)((*peventbuf>>16)&0xff);
8321
8322         /*  checking if event code is valid */
8323         if (evt_code >= MAX_C2HEVT) {
8324                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent Code(%d) mismatch!\n", evt_code));
8325                 goto _abort_event_;
8326         }
8327
8328         /*  checking if event size match the event parm size */
8329         if ((wlanevents[evt_code].parmsize != 0) &&
8330             (wlanevents[evt_code].parmsize != evt_sz)) {
8331                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
8332                          ("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
8333                          evt_code, wlanevents[evt_code].parmsize, evt_sz));
8334                 goto _abort_event_;
8335         }
8336
8337         atomic_inc(&pevt_priv->event_seq);
8338
8339         peventbuf += 2;
8340
8341         if (peventbuf) {
8342                 event_callback = wlanevents[evt_code].event_callback;
8343                 event_callback(padapter, (u8 *)peventbuf);
8344
8345                 pevt_priv->evt_done_cnt++;
8346         }
8347
8348 _abort_event_:
8349         return H2C_SUCCESS;
8350 }
8351
8352 u8 h2c_msg_hdl(struct adapter *padapter, unsigned char *pbuf)
8353 {
8354         if (!pbuf)
8355                 return H2C_PARAMETERS_ERROR;
8356
8357         return H2C_SUCCESS;
8358 }
8359
8360 u8 tx_beacon_hdl(struct adapter *padapter, unsigned char *pbuf)
8361 {
8362         if (send_beacon(padapter) == _FAIL) {
8363                 DBG_88E("issue_beacon, fail!\n");
8364                 return H2C_PARAMETERS_ERROR;
8365         }
8366 #ifdef CONFIG_88EU_AP_MODE
8367         else { /* tx bc/mc frames after update TIM */
8368                 struct sta_info *psta_bmc;
8369                 struct list_head *xmitframe_plist, *xmitframe_phead;
8370                 struct xmit_frame *pxmitframe = NULL;
8371                 struct sta_priv  *pstapriv = &padapter->stapriv;
8372
8373                 /* for BC/MC Frames */
8374                 psta_bmc = rtw_get_bcmc_stainfo(padapter);
8375                 if (!psta_bmc)
8376                         return H2C_SUCCESS;
8377
8378                 if ((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len > 0)) {
8379                         msleep(10);/*  10ms, ATIM(HIQ) Windows */
8380                         spin_lock_bh(&psta_bmc->sleep_q.lock);
8381
8382                         xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
8383                         xmitframe_plist = xmitframe_phead->next;
8384
8385                         while (!rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) {
8386                                 pxmitframe = container_of(xmitframe_plist, struct xmit_frame, list);
8387
8388                                 xmitframe_plist = xmitframe_plist->next;
8389
8390                                 rtw_list_delete(&pxmitframe->list);
8391
8392                                 psta_bmc->sleepq_len--;
8393                                 if (psta_bmc->sleepq_len > 0)
8394                                         pxmitframe->attrib.mdata = 1;
8395                                 else
8396                                         pxmitframe->attrib.mdata = 0;
8397
8398                                 pxmitframe->attrib.triggered = 1;
8399
8400                                 pxmitframe->attrib.qsel = 0x11;/* HIQ */
8401
8402                                 spin_unlock_bh(&psta_bmc->sleep_q.lock);
8403                                 if (rtw_hal_xmit(padapter, pxmitframe))
8404                                         rtw_os_xmit_complete(padapter, pxmitframe);
8405                                 spin_lock_bh(&psta_bmc->sleep_q.lock);
8406                         }
8407                         spin_unlock_bh(&psta_bmc->sleep_q.lock);
8408                 }
8409         }
8410 #endif
8411         return H2C_SUCCESS;
8412 }
8413
8414 u8 set_ch_hdl(struct adapter *padapter, u8 *pbuf)
8415 {
8416         struct set_ch_parm *set_ch_parm;
8417         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8418
8419         if (!pbuf)
8420                 return H2C_PARAMETERS_ERROR;
8421
8422         set_ch_parm = (struct set_ch_parm *)pbuf;
8423
8424         DBG_88E(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
8425                 FUNC_NDEV_ARG(padapter->pnetdev),
8426                 set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
8427
8428         pmlmeext->cur_channel = set_ch_parm->ch;
8429         pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
8430         pmlmeext->cur_bwmode = set_ch_parm->bw;
8431
8432         set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
8433
8434         return  H2C_SUCCESS;
8435 }
8436
8437 u8 set_chplan_hdl(struct adapter *padapter, unsigned char *pbuf)
8438 {
8439         struct SetChannelPlan_param *setChannelPlan_param;
8440         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8441
8442         if (!pbuf)
8443                 return H2C_PARAMETERS_ERROR;
8444
8445         setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
8446
8447         pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
8448         init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
8449
8450         return  H2C_SUCCESS;
8451 }
8452
8453 u8 led_blink_hdl(struct adapter *padapter, unsigned char *pbuf)
8454 {
8455         if (!pbuf)
8456                 return H2C_PARAMETERS_ERROR;
8457         return  H2C_SUCCESS;
8458 }
8459
8460 u8 set_csa_hdl(struct adapter *padapter, unsigned char *pbuf)
8461 {
8462         return  H2C_REJECTED;
8463 }
8464
8465 /*  TDLS_WRCR           : write RCR DATA BIT */
8466 /*  TDLS_SD_PTI         : issue peer traffic indication */
8467 /*  TDLS_CS_OFF         : go back to the channel linked with AP, terminating channel switch procedure */
8468 /*  TDLS_INIT_CH_SEN    : init channel sensing, receive all data and mgnt frame */
8469 /*  TDLS_DONE_CH_SEN: channel sensing and report candidate channel */
8470 /*  TDLS_OFF_CH         : first time set channel to off channel */
8471 /*  TDLS_BASE_CH                : go back tp the channel linked with AP when set base channel as target channel */
8472 /*  TDLS_P_OFF_CH       : periodically go to off channel */
8473 /*  TDLS_P_BASE_CH      : periodically go back to base channel */
8474 /*  TDLS_RS_RCR         : restore RCR */
8475 /*  TDLS_CKALV_PH1      : check alive timer phase1 */
8476 /*  TDLS_CKALV_PH2      : check alive timer phase2 */
8477 /*  TDLS_FREE_STA       : free tdls sta */
8478 u8 tdls_hdl(struct adapter *padapter, unsigned char *pbuf)
8479 {
8480         return H2C_REJECTED;
8481 }