drivers/staging/r8188eu/core/rtw_mlme_ext.c

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