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