drivers/staging/rtl8723au/core/rtw_mlme_ext.c

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