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 <linux/ieee80211.h>
  25 #include <rtl8723a_hal.h>
  26
  27 static int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  28 static int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  29 static int OnProbeReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  30 static int OnProbeRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  31 static int DoReserved23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  32 static int OnBeacon23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  33 static int OnAtim23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  34 static int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  35 static int OnAuth23aClient23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  36 static int OnDeAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  37 static int OnAction23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  38
  39 static int on_action_spct23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  40 static int OnAction23a_qos(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  41 static int OnAction23a_dls(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  42 static int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  43 static int on_action_public23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  44 static int OnAction23a_ht(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  45 static int OnAction23a_wmm(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  46 static int OnAction23a_p2p(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  47
  48 static void issue_assocreq(struct rtw_adapter *padapter);
  49 static void issue_probereq(struct rtw_adapter *padapter,
  50                            struct cfg80211_ssid *pssid, u8 *da);
  51 static int issue_probereq_ex(struct rtw_adapter *padapter,
  52                              struct cfg80211_ssid *pssid,
  53                              u8 *da, int try_cnt, int wait_ms);
  54 static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da);
  55 static void issue_auth(struct rtw_adapter *padapter, struct sta_info *psta,
  56                        unsigned short status);
  57 static int issue_deauth_ex(struct rtw_adapter *padapter, u8 *da,
  58                            unsigned short reason, int try_cnt, int wait_ms);
  59 static void start_clnt_assoc(struct rtw_adapter *padapter);
  60 static void start_clnt_auth(struct rtw_adapter *padapter);
  61 static void start_clnt_join(struct rtw_adapter *padapter);
  62 static void start_create_ibss(struct rtw_adapter *padapter);
  63 static struct wlan_bssid_ex *collect_bss_info(struct rtw_adapter *padapter,
  64                                               struct recv_frame *precv_frame);
  65
  66 #ifdef CONFIG_8723AU_AP_MODE
  67 static int OnAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
  68 static void issue_assocrsp(struct rtw_adapter *padapter, unsigned short status,
  69                            struct sta_info *pstat, u16 pkt_type);
  70 #endif
  71
  72 static struct mlme_handler mlme_sta_tbl[]={
  73         {"OnAssocReq23a",               &OnAssocReq23a},
  74         {"OnAssocRsp23a",               &OnAssocRsp23a},
  75         {"OnReAssocReq",        &OnAssocReq23a},
  76         {"OnReAssocRsp",        &OnAssocRsp23a},
  77         {"OnProbeReq23a",               &OnProbeReq23a},
  78         {"OnProbeRsp23a",               &OnProbeRsp23a},
  79
  80         /*----------------------------------------------------------
  81                                         below 2 are reserved
  82         -----------------------------------------------------------*/
  83         {"DoReserved23a",               &DoReserved23a},
  84         {"DoReserved23a",               &DoReserved23a},
  85         {"OnBeacon23a",         &OnBeacon23a},
  86         {"OnATIM",              &OnAtim23a},
  87         {"OnDisassoc23a",               &OnDisassoc23a},
  88         {"OnAuth23a",           &OnAuth23aClient23a},
  89         {"OnDeAuth23a",         &OnDeAuth23a},
  90         {"OnAction23a",         &OnAction23a},
  91 };
  92
  93 static struct action_handler OnAction23a_tbl[]={
  94         {WLAN_CATEGORY_SPECTRUM_MGMT, "ACTION_SPECTRUM_MGMT", on_action_spct23a},
  95         {WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction23a_qos},
  96         {WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction23a_dls},
  97         {WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction23a_back23a},
  98         {WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public23a},
  99         {WLAN_CATEGORY_HT, "ACTION_HT", &OnAction23a_ht},
 100         {WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved23a},
 101         {WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction23a_wmm},
 102         {WLAN_CATEGORY_VENDOR_SPECIFIC, "ACTION_P2P", &OnAction23a_p2p},
 103 };
 104
 105 static u8       null_addr[ETH_ALEN]= {0, 0, 0, 0, 0, 0};
 106
 107 /**************************************************
 108 OUI definitions for the vendor specific IE
 109 ***************************************************/
 110 unsigned char WMM_OUI23A[] = {0x00, 0x50, 0xf2, 0x02};
 111 unsigned char WPS_OUI23A[] = {0x00, 0x50, 0xf2, 0x04};
 112 unsigned char P2P_OUI23A[] = {0x50, 0x6F, 0x9A, 0x09};
 113 unsigned char WFD_OUI23A[] = {0x50, 0x6F, 0x9A, 0x0A};
 114
 115 unsigned char WMM_INFO_OUI23A[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
 116 unsigned char WMM_PARA_OUI23A[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
 117
 118 static unsigned char REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
 119
 120 /********************************************************
 121 MCS rate definitions
 122 *********************************************************/
 123 unsigned char MCS_rate_2R23A[16] = {
 124         0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0,
 125         0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 126 unsigned char MCS_rate_1R23A[16] = {
 127         0xff, 0x00, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0,
 128         0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 129
 130 /********************************************************
 131 ChannelPlan definitions
 132 *********************************************************/
 133
 134 static struct rt_channel_plan_2g RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
 135         /*  0x00, RT_CHANNEL_DOMAIN_2G_WORLD , Passive scan CH 12, 13 */
 136         {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
 137         /*  0x01, RT_CHANNEL_DOMAIN_2G_ETSI1 */
 138         {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
 139         /*  0x02, RT_CHANNEL_DOMAIN_2G_FCC1 */
 140         {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11},
 141         /*  0x03, RT_CHANNEL_DOMAIN_2G_MIKK1 */
 142         {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14},
 143         /*  0x04, RT_CHANNEL_DOMAIN_2G_ETSI2 */
 144         {{10, 11, 12, 13}, 4},
 145         /*  0x05, RT_CHANNEL_DOMAIN_2G_NULL */
 146         {{}, 0},
 147 };
 148
 149 static struct rt_channel_plan_5g RTW_ChannelPlan5G[RT_CHANNEL_DOMAIN_5G_MAX] = {
 150         /*  0x00, RT_CHANNEL_DOMAIN_5G_NULL */
 151         {{}, 0},
 152         /*  0x01, RT_CHANNEL_DOMAIN_5G_ETSI1 */
 153         {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112,
 154           116, 120, 124, 128, 132, 136, 140}, 19},
 155         /*  0x02, RT_CHANNEL_DOMAIN_5G_ETSI2 */
 156         {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112,
 157           116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165}, 24},
 158         /*  0x03, RT_CHANNEL_DOMAIN_5G_ETSI3 */
 159         {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112,
 160           116, 120, 124, 128, 132, 149, 153, 157, 161, 165}, 22},
 161         /*  0x04, RT_CHANNEL_DOMAIN_5G_FCC1 */
 162         {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112,
 163           116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165}, 24},
 164         /*  0x05, RT_CHANNEL_DOMAIN_5G_FCC2 */
 165         {{36, 40, 44, 48, 149, 153, 157, 161, 165}, 9},
 166         /*  0x06, RT_CHANNEL_DOMAIN_5G_FCC3 */
 167         {{36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165}, 13},
 168         /*  0x07, RT_CHANNEL_DOMAIN_5G_FCC4 */
 169         {{36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161}, 12},
 170         /*  0x08, RT_CHANNEL_DOMAIN_5G_FCC5 */
 171         {{149, 153, 157, 161, 165}, 5},
 172         /*  0x09, RT_CHANNEL_DOMAIN_5G_FCC6 */
 173         {{36, 40, 44, 48, 52, 56, 60, 64}, 8},
 174         /*  0x0A, RT_CHANNEL_DOMAIN_5G_FCC7_IC1 */
 175         {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112,
 176           116, 136, 140, 149, 153, 157, 161, 165}, 20},
 177         /*  0x0B, RT_CHANNEL_DOMAIN_5G_KCC1 */
 178         {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112,
 179           116, 120, 124, 149, 153, 157, 161, 165}, 20},
 180         /*  0x0C, RT_CHANNEL_DOMAIN_5G_MKK1 */
 181         {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112,
 182           116, 120, 124, 128, 132, 136, 140}, 19},
 183         /*  0x0D, RT_CHANNEL_DOMAIN_5G_MKK2 */
 184         {{36, 40, 44, 48, 52, 56, 60, 64}, 8},
 185         /*  0x0E, RT_CHANNEL_DOMAIN_5G_MKK3 */
 186         {{100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 11},
 187         /*  0x0F, RT_CHANNEL_DOMAIN_5G_NCC1 */
 188         {{56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149,
 189           153, 157, 161, 165}, 15},
 190         /*  0x10, RT_CHANNEL_DOMAIN_5G_NCC2 */
 191         {{56, 60, 64, 149, 153, 157, 161, 165}, 8},
 192
 193         /*  Driver self defined for old channel plan Compatible,
 194             Remember to modify if have new channel plan definition ===== */
 195         /*  0x11, RT_CHANNEL_DOMAIN_5G_FCC */
 196         {{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112,
 197           116, 132, 136, 140, 149, 153, 157, 161, 165}, 21},
 198         /*  0x12, RT_CHANNEL_DOMAIN_5G_JAPAN_NO_DFS */
 199         {{36, 40, 44, 48}, 4},
 200         /*  0x13, RT_CHANNEL_DOMAIN_5G_FCC4_NO_DFS */
 201         {{36, 40, 44, 48, 149, 153, 157, 161}, 8},
 202 };
 203
 204 static struct rt_channel_plan_map RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
 205         /*  0x00 ~ 0x1F , Old Define ===== */
 206         {0x02, 0x11},   /* 0x00, RT_CHANNEL_DOMAIN_FCC */
 207         {0x02, 0x0A},   /* 0x01, RT_CHANNEL_DOMAIN_IC */
 208         {0x01, 0x01},   /* 0x02, RT_CHANNEL_DOMAIN_ETSI */
 209         {0x01, 0x00},   /* 0x03, RT_CHANNEL_DOMAIN_SPAIN */
 210         {0x01, 0x00},   /* 0x04, RT_CHANNEL_DOMAIN_FRANCE */
 211         {0x03, 0x00},   /* 0x05, RT_CHANNEL_DOMAIN_MKK */
 212         {0x03, 0x00},   /* 0x06, RT_CHANNEL_DOMAIN_MKK1 */
 213         {0x01, 0x09},   /* 0x07, RT_CHANNEL_DOMAIN_ISRAEL */
 214         {0x03, 0x09},   /* 0x08, RT_CHANNEL_DOMAIN_TELEC */
 215         {0x03, 0x00},   /* 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN */
 216         {0x00, 0x00},   /* 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13 */
 217         {0x02, 0x0F},   /* 0x0B, RT_CHANNEL_DOMAIN_TAIWAN */
 218         {0x01, 0x08},   /* 0x0C, RT_CHANNEL_DOMAIN_CHINA */
 219         {0x02, 0x06},   /* 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO */
 220         {0x02, 0x0B},   /* 0x0E, RT_CHANNEL_DOMAIN_KOREA */
 221         {0x02, 0x09},   /* 0x0F, RT_CHANNEL_DOMAIN_TURKEY */
 222         {0x01, 0x01},   /* 0x10, RT_CHANNEL_DOMAIN_JAPAN */
 223         {0x02, 0x05},   /* 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS */
 224         {0x01, 0x12},   /* 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
 225         {0x00, 0x04},   /* 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G */
 226         {0x02, 0x10},   /* 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS */
 227         {0x00, 0x12},   /* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */
 228         {0x00, 0x13},   /* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */
 229         {0x03, 0x12},   /* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
 230         {0x05, 0x08},   /* 0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS */
 231         {0x02, 0x08},   /* 0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS */
 232         {0x00, 0x00},   /* 0x1A, */
 233         {0x00, 0x00},   /* 0x1B, */
 234         {0x00, 0x00},   /* 0x1C, */
 235         {0x00, 0x00},   /* 0x1D, */
 236         {0x00, 0x00},   /* 0x1E, */
 237         {0x05, 0x04},   /* 0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G */
 238         /*  0x20 ~ 0x7F , New Define ===== */
 239         {0x00, 0x00},   /* 0x20, RT_CHANNEL_DOMAIN_WORLD_NULL */
 240         {0x01, 0x00},   /* 0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL */
 241         {0x02, 0x00},   /* 0x22, RT_CHANNEL_DOMAIN_FCC1_NULL */
 242         {0x03, 0x00},   /* 0x23, RT_CHANNEL_DOMAIN_MKK1_NULL */
 243         {0x04, 0x00},   /* 0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL */
 244         {0x02, 0x04},   /* 0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1 */
 245         {0x00, 0x01},   /* 0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1 */
 246         {0x03, 0x0C},   /* 0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1 */
 247         {0x00, 0x0B},   /* 0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1 */
 248         {0x00, 0x05},   /* 0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2 */
 249         {0x00, 0x00},   /* 0x2A, */
 250         {0x00, 0x00},   /* 0x2B, */
 251         {0x00, 0x00},   /* 0x2C, */
 252         {0x00, 0x00},   /* 0x2D, */
 253         {0x00, 0x00},   /* 0x2E, */
 254         {0x00, 0x00},   /* 0x2F, */
 255         {0x00, 0x06},   /* 0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3 */
 256         {0x00, 0x07},   /* 0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4 */
 257         {0x00, 0x08},   /* 0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5 */
 258         {0x00, 0x09},   /* 0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6 */
 259         {0x02, 0x0A},   /* 0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7 */
 260         {0x00, 0x02},   /* 0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2 */
 261         {0x00, 0x03},   /* 0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3 */
 262         {0x03, 0x0D},   /* 0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2 */
 263         {0x03, 0x0E},   /* 0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3 */
 264         {0x02, 0x0F},   /* 0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1 */
 265         {0x00, 0x00},   /* 0x3A, */
 266         {0x00, 0x00},   /* 0x3B, */
 267         {0x00, 0x00},   /* 0x3C, */
 268         {0x00, 0x00},   /* 0x3D, */
 269         {0x00, 0x00},   /* 0x3E, */
 270         {0x00, 0x00},   /* 0x3F, */
 271         {0x02, 0x10},   /* 0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2 */
 272         {0x03, 0x00},   /* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G */
 273 };
 274
 275 static struct rt_channel_plan_map RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE =
 276 {0x03, 0x02}; /* use the conbination for max channel numbers */
 277
 278 static void dummy_event_callback(struct rtw_adapter *adapter, const u8 *pbuf)
 279 {
 280 }
 281
 282 static struct fwevent wlanevents[] =
 283 {
 284         {0, &dummy_event_callback},     /*0*/
 285         {0, NULL},
 286         {0, NULL},
 287         {0, NULL},
 288         {0, NULL},
 289         {0, NULL},
 290         {0, NULL},
 291         {0, NULL},
 292         {0, &rtw_survey_event_cb23a},           /*8*/
 293         {sizeof (struct surveydone_event), &rtw_surveydone_event_callback23a},
 294         {0, &rtw23a_joinbss_event_cb},          /*10*/
 295         {sizeof(struct stassoc_event), &rtw_stassoc_event_callback23a},
 296         {sizeof(struct stadel_event), &rtw_stadel_event_callback23a},
 297         {0, &dummy_event_callback},
 298         {0, &dummy_event_callback},
 299         {0, NULL},      /*15*/
 300         {0, NULL},
 301         {0, NULL},
 302         {0, NULL},
 303         {0, &dummy_event_callback},
 304         {0, NULL},       /*20*/
 305         {0, NULL},
 306         {0, NULL},
 307         {0, &dummy_event_callback},
 308         {0, NULL},
 309 };
 310
 311
 312 static void rtw_correct_TSF(struct rtw_adapter *padapter)
 313 {
 314         hw_var_set_correct_tsf(padapter);
 315 }
 316
 317 static void
 318 rtw_update_TSF(struct mlme_ext_priv *pmlmeext, struct ieee80211_mgmt *mgmt)
 319 {
 320         pmlmeext->TSFValue = get_unaligned_le64(&mgmt->u.beacon.timestamp);
 321 }
 322
 323 /*
 324  * Search the @param channel_num in given @param channel_set
 325  * @ch_set: the given channel set
 326  * @ch: the given channel number
 327  *
 328  * return the index of channel_num in channel_set, -1 if not found
 329  */
 330 int rtw_ch_set_search_ch23a(struct rt_channel_info *ch_set, const u32 ch)
 331 {
 332         int i;
 333
 334         for (i = 0; ch_set[i]. ChannelNum != 0; i++) {
 335                 if (ch == ch_set[i].ChannelNum)
 336                         break;
 337         }
 338
 339         if (i >= ch_set[i].ChannelNum)
 340                 return -1;
 341         return i;
 342 }
 343
 344 /****************************************************************************
 345
 346 Following are the initialization functions for WiFi MLME
 347
 348 *****************************************************************************/
 349
 350 int init_hw_mlme_ext23a(struct rtw_adapter *padapter)
 351 {
 352         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 353
 354         set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
 355                               pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 356         return _SUCCESS;
 357 }
 358
 359 static void init_mlme_ext_priv23a_value(struct rtw_adapter *padapter)
 360 {
 361         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 362         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 363         unsigned char   mixed_datarate[NumRates] = {
 364                 _1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
 365                 _9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_,
 366                 _48M_RATE_, _54M_RATE_, 0xff};
 367         unsigned char   mixed_basicrate[NumRates] = {
 368                 _1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
 369                 _12M_RATE_, _24M_RATE_, 0xff,};
 370
 371         atomic_set(&pmlmeext->event_seq, 0);
 372         /* reset to zero when disconnect at client mode */
 373         pmlmeext->mgnt_seq = 0;
 374
 375         pmlmeext->cur_channel = padapter->registrypriv.channel;
 376         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
 377         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 378
 379         pmlmeext->retry = 0;
 380
 381         pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
 382
 383         memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
 384         memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
 385
 386         if (pmlmeext->cur_channel > 14)
 387                 pmlmeext->tx_rate = IEEE80211_OFDM_RATE_6MB;
 388         else
 389                 pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
 390
 391         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
 392         pmlmeext->sitesurvey_res.channel_idx = 0;
 393         pmlmeext->sitesurvey_res.bss_cnt = 0;
 394         pmlmeext->scan_abort = false;
 395
 396         pmlmeinfo->state = MSR_NOLINK;
 397         pmlmeinfo->reauth_count = 0;
 398         pmlmeinfo->reassoc_count = 0;
 399         pmlmeinfo->link_count = 0;
 400         pmlmeinfo->auth_seq = 0;
 401         pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
 402         pmlmeinfo->key_index = 0;
 403         pmlmeinfo->iv = 0;
 404
 405         pmlmeinfo->enc_algo = 0;
 406         pmlmeinfo->authModeToggle = 0;
 407
 408         memset(pmlmeinfo->chg_txt, 0, 128);
 409
 410         pmlmeinfo->slotTime = SHORT_SLOT_TIME;
 411         pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
 412
 413         pmlmeinfo->dialogToken = 0;
 414
 415         pmlmeext->action_public_rxseq = 0xffff;
 416         pmlmeext->action_public_dialog_token = 0xff;
 417 }
 418
 419 static int has_channel(struct rt_channel_info *channel_set,
 420                        u8 chanset_size, u8 chan) {
 421         int i;
 422
 423         for (i = 0; i < chanset_size; i++) {
 424                 if (channel_set[i].ChannelNum == chan)
 425                         return 1;
 426         }
 427
 428         return 0;
 429 }
 430
 431 static void init_channel_list(struct rtw_adapter *padapter,
 432                               struct rt_channel_info *channel_set,
 433                               u8 chanset_size,
 434                               struct p2p_channels *channel_list)
 435 {
 436         struct p2p_oper_class_map op_class[] = {
 437                 { IEEE80211G,  81,   1,  13,  1, BW20 },
 438                 { IEEE80211G,  82,  14,  14,  1, BW20 },
 439                 { IEEE80211A, 115,  36,  48,  4, BW20 },
 440                 { IEEE80211A, 116,  36,  44,  8, BW40PLUS },
 441                 { IEEE80211A, 117,  40,  48,  8, BW40MINUS },
 442                 { IEEE80211A, 124, 149, 161,  4, BW20 },
 443                 { IEEE80211A, 125, 149, 169,  4, BW20 },
 444                 { IEEE80211A, 126, 149, 157,  8, BW40PLUS },
 445                 { IEEE80211A, 127, 153, 161,  8, BW40MINUS },
 446                 { -1, 0, 0, 0, 0, BW20 }
 447         };
 448
 449         int cla, op;
 450
 451         cla = 0;
 452
 453         for (op = 0; op_class[op].op_class; op++) {
 454                 u8 ch;
 455                 struct p2p_oper_class_map *o = &op_class[op];
 456                 struct p2p_reg_class *reg = NULL;
 457
 458                 for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
 459                         if (!has_channel(channel_set, chanset_size, ch))
 460                                 continue;
 461
 462                         if ((0 == padapter->registrypriv.ht_enable) &&
 463                             (o->inc == 8))
 464                                 continue;
 465
 466                         if ((0 == (padapter->registrypriv.cbw40_enable & BIT(1))) &&
 467                                 ((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
 468                                 continue;
 469
 470                         if (reg == NULL) {
 471                                 reg = &channel_list->reg_class[cla];
 472                                 cla++;
 473                                 reg->reg_class = o->op_class;
 474                                 reg->channels = 0;
 475                         }
 476                         reg->channel[reg->channels] = ch;
 477                         reg->channels++;
 478                 }
 479         }
 480         channel_list->reg_classes = cla;
 481 }
 482
 483 static u8 init_channel_set(struct rtw_adapter *padapter, u8 cplan,
 484                            struct rt_channel_info *c_set)
 485 {
 486         u8 i, ch_size = 0;
 487         u8 b5GBand = false, b2_4GBand = false;
 488         u8 Index2G = 0, Index5G = 0;
 489
 490         memset(c_set, 0, sizeof(struct rt_channel_info) * MAX_CHANNEL_NUM);
 491
 492         if (cplan >= RT_CHANNEL_DOMAIN_MAX &&
 493             cplan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
 494                 DBG_8723A("ChannelPlan ID %x error !!!!!\n", cplan);
 495                 return ch_size;
 496         }
 497
 498         if (padapter->registrypriv.wireless_mode & WIRELESS_11G) {
 499                 b2_4GBand = true;
 500                 if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == cplan)
 501                         Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
 502                 else
 503                         Index2G = RTW_ChannelPlanMap[cplan].Index2G;
 504         }
 505
 506         if (padapter->registrypriv.wireless_mode & WIRELESS_11A) {
 507                 b5GBand = true;
 508                 if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == cplan)
 509                         Index5G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index5G;
 510                 else
 511                         Index5G = RTW_ChannelPlanMap[cplan].Index5G;
 512         }
 513
 514         if (b2_4GBand) {
 515                 for (i = 0; i < RTW_ChannelPlan2G[Index2G].Len; i++) {
 516                         c_set[ch_size].ChannelNum =
 517                                 RTW_ChannelPlan2G[Index2G].Channel[i];
 518
 519                         if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == cplan) ||
 520                             /* Channel 1~11 is active, and 12~14 is passive */
 521                             RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == cplan) {
 522                                 if (c_set[ch_size].ChannelNum >= 1 &&
 523                                     c_set[ch_size].ChannelNum <= 11)
 524                                         c_set[ch_size].ScanType = SCAN_ACTIVE;
 525                                 else if (c_set[ch_size].ChannelNum >= 12 &&
 526                                          c_set[ch_size].ChannelNum  <= 14)
 527                                         c_set[ch_size].ScanType = SCAN_PASSIVE;
 528                         } else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == cplan ||
 529                                    RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == cplan ||
 530                                    RT_CHANNEL_DOMAIN_2G_WORLD == Index2G) {
 531                                 /*  channel 12~13, passive scan */
 532                                 if (c_set[ch_size].ChannelNum <= 11)
 533                                         c_set[ch_size].ScanType = SCAN_ACTIVE;
 534                                 else
 535                                         c_set[ch_size].ScanType = SCAN_PASSIVE;
 536                         } else
 537                                 c_set[ch_size].ScanType = SCAN_ACTIVE;
 538
 539                         ch_size++;
 540                 }
 541         }
 542
 543         if (b5GBand) {
 544                 for (i = 0; i < RTW_ChannelPlan5G[Index5G].Len; i++) {
 545                         if (RTW_ChannelPlan5G[Index5G].Channel[i] <= 48 ||
 546                             RTW_ChannelPlan5G[Index5G].Channel[i] >= 149) {
 547                                 c_set[ch_size].ChannelNum =
 548                                         RTW_ChannelPlan5G[Index5G].Channel[i];
 549                                 if (RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == cplan) {
 550                                         /* passive scan for all 5G channels */
 551                                         c_set[ch_size].ScanType =
 552                                                 SCAN_PASSIVE;
 553                                 } else
 554                                         c_set[ch_size].ScanType =
 555                                                 SCAN_ACTIVE;
 556                                 DBG_8723A("%s(): channel_set[%d].ChannelNum = "
 557                                           "%d\n", __func__, ch_size,
 558                                           c_set[ch_size].ChannelNum);
 559                                 ch_size++;
 560                         }
 561                 }
 562         }
 563
 564         return ch_size;
 565 }
 566
 567 int init_mlme_ext_priv23a(struct rtw_adapter *padapter)
 568 {
 569         struct registry_priv *pregistrypriv = &padapter->registrypriv;
 570         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 571         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 572         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 573
 574         pmlmeext->padapter = padapter;
 575
 576         init_mlme_ext_priv23a_value(padapter);
 577         pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
 578
 579         init_mlme_ext_timer23a(padapter);
 580
 581 #ifdef CONFIG_8723AU_AP_MODE
 582         init_mlme_ap_info23a(padapter);
 583 #endif
 584
 585         pmlmeext->max_chan_nums = init_channel_set(padapter,
 586                                                    pmlmepriv->ChannelPlan,
 587                                                    pmlmeext->channel_set);
 588         init_channel_list(padapter, pmlmeext->channel_set,
 589                           pmlmeext->max_chan_nums, &pmlmeext->channel_list);
 590
 591         pmlmeext->chan_scan_time = SURVEY_TO;
 592         pmlmeext->mlmeext_init = true;
 593
 594         pmlmeext->active_keep_alive_check = true;
 595         return _SUCCESS;
 596 }
 597
 598 void free_mlme_ext_priv23a (struct mlme_ext_priv *pmlmeext)
 599 {
 600         struct rtw_adapter *padapter = pmlmeext->padapter;
 601
 602         if (!padapter)
 603                 return;
 604
 605         if (padapter->bDriverStopped == true) {
 606                 del_timer_sync(&pmlmeext->survey_timer);
 607                 del_timer_sync(&pmlmeext->link_timer);
 608                 /* del_timer_sync(&pmlmeext->ADDBA_timer); */
 609         }
 610 }
 611
 612 static void
 613 _mgt_dispatcher23a(struct rtw_adapter *padapter, struct mlme_handler *ptable,
 614                    struct recv_frame *precv_frame)
 615 {
 616         struct sk_buff *skb = precv_frame->pkt;
 617         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 618
 619         if (ptable->func) {
 620                 /* receive the frames that ra(a1) is my address
 621                    or ra(a1) is bc address. */
 622                 if (!ether_addr_equal(hdr->addr1, myid(&padapter->eeprompriv))&&
 623                     !is_broadcast_ether_addr(hdr->addr1))
 624                         return;
 625
 626                 ptable->func(padapter, precv_frame);
 627         }
 628 }
 629
 630 void mgt_dispatcher23a(struct rtw_adapter *padapter,
 631                     struct recv_frame *precv_frame)
 632 {
 633         struct mlme_handler *ptable;
 634 #ifdef CONFIG_8723AU_AP_MODE
 635         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 636 #endif /* CONFIG_8723AU_AP_MODE */
 637         struct sk_buff *skb = precv_frame->pkt;
 638         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 639         struct sta_info *psta;
 640         u16 stype;
 641         int index;
 642
 643         if (!ieee80211_is_mgmt(mgmt->frame_control))
 644                 return;
 645
 646         /* receive the frames that ra(a1) is my address or ra(a1) is
 647            bc address. */
 648         if (!ether_addr_equal(mgmt->da, myid(&padapter->eeprompriv)) &&
 649             !is_broadcast_ether_addr(mgmt->da))
 650                 return;
 651
 652         ptable = mlme_sta_tbl;
 653
 654         stype = le16_to_cpu(mgmt->frame_control) & IEEE80211_FCTL_STYPE;
 655         index = stype >> 4;
 656
 657         if (index > 13) {
 658                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 659                          "Currently we do not support reserved sub-fr-type =%d\n",
 660                          index);
 661                 return;
 662         }
 663         ptable += index;
 664
 665         psta = rtw_get_stainfo23a(&padapter->stapriv, mgmt->sa);
 666
 667         if (psta) {
 668                 if (ieee80211_has_retry(mgmt->frame_control)) {
 669                         if (precv_frame->attrib.seq_num ==
 670                             psta->RxMgmtFrameSeqNum) {
 671                                 /* drop the duplicate management frame */
 672                                 DBG_8723A("Drop duplicate management frame "
 673                                           "with seq_num = %d.\n",
 674                                           precv_frame->attrib.seq_num);
 675                                 return;
 676                         }
 677                 }
 678                 psta->RxMgmtFrameSeqNum = precv_frame->attrib.seq_num;
 679         }
 680
 681 #ifdef CONFIG_8723AU_AP_MODE
 682         switch (stype) {
 683         case IEEE80211_STYPE_AUTH:
 684                 if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 685                         ptable->func = &OnAuth23a;
 686                 else
 687                         ptable->func = &OnAuth23aClient23a;
 688                 /* pass through */
 689         case IEEE80211_STYPE_ASSOC_REQ:
 690         case IEEE80211_STYPE_REASSOC_REQ:
 691                 _mgt_dispatcher23a(padapter, ptable, precv_frame);
 692                 break;
 693         case IEEE80211_STYPE_PROBE_REQ:
 694                 if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 695                         _mgt_dispatcher23a(padapter, ptable, precv_frame);
 696                 else
 697                         _mgt_dispatcher23a(padapter, ptable, precv_frame);
 698                 break;
 699         case IEEE80211_STYPE_BEACON:
 700                 _mgt_dispatcher23a(padapter, ptable, precv_frame);
 701                 break;
 702         case IEEE80211_STYPE_ACTION:
 703                 /* if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) */
 704                 _mgt_dispatcher23a(padapter, ptable, precv_frame);
 705                 break;
 706         default:
 707                 _mgt_dispatcher23a(padapter, ptable, precv_frame);
 708                 break;
 709         }
 710 #else
 711         _mgt_dispatcher23a(padapter, ptable, precv_frame);
 712 #endif
 713 }
 714
 715 /****************************************************************************
 716
 717 Following are the callback functions for each subtype of the management frames
 718
 719 *****************************************************************************/
 720
 721 static int
 722 OnProbeReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 723 {
 724         const u8 *ie;
 725         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 726         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 727         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 728         struct wlan_bssid_ex *cur = &pmlmeinfo->network;
 729         struct sk_buff *skb = precv_frame->pkt;
 730         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 731         int len = skb->len;
 732
 733         if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 734                 return _SUCCESS;
 735
 736         if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
 737             !check_fwstate(pmlmepriv,
 738                            WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE))
 739                 return _SUCCESS;
 740
 741         if (unlikely(!ieee80211_is_probe_req(mgmt->frame_control))) {
 742                 printk(KERN_WARNING "%s: Received non probe request frame\n",
 743                        __func__);
 744                 return _FAIL;
 745         }
 746
 747         len -= offsetof(struct ieee80211_mgmt, u.probe_req.variable);
 748
 749         ie = cfg80211_find_ie(WLAN_EID_SSID, mgmt->u.probe_req.variable, len);
 750
 751         /* check (wildcard) SSID */
 752         if (!ie)
 753                 goto out;
 754
 755         if ((ie[1] && memcmp(ie + 2, cur->Ssid.ssid, cur->Ssid.ssid_len)) ||
 756             (ie[1] == 0 && pmlmeinfo->hidden_ssid_mode)) {
 757                 return _SUCCESS;
 758         }
 759
 760         if (check_fwstate(pmlmepriv, _FW_LINKED) &&
 761             pmlmepriv->cur_network.join_res)
 762                 issue_probersp(padapter, mgmt->sa);
 763
 764 out:
 765         return _SUCCESS;
 766 }
 767
 768 static int
 769 OnProbeRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 770 {
 771         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 772
 773         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
 774                 report_survey_event23a(padapter, precv_frame);
 775                 return _SUCCESS;
 776         }
 777
 778         return _SUCCESS;
 779 }
 780
 781 static int
 782 OnBeacon23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 783 {
 784         int cam_idx;
 785         struct sta_info *psta;
 786         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 787         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 788         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 789         struct sta_priv *pstapriv = &padapter->stapriv;
 790         struct sk_buff *skb = precv_frame->pkt;
 791         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 792         int pkt_len = skb->len;
 793         struct wlan_bssid_ex *pbss;
 794         int ret = _SUCCESS;
 795         u8 *p, *pie;
 796         int pie_len;
 797         u32 ielen = 0;
 798
 799         pie = mgmt->u.beacon.variable;
 800         pie_len = pkt_len - offsetof(struct ieee80211_mgmt, u.beacon.variable);
 801         p = rtw_get_ie23a(pie, WLAN_EID_EXT_SUPP_RATES, &ielen, pie_len);
 802         if (p && ielen > 0) {
 803                 if (p[1 + ielen] == 0x2D && p[2 + ielen] != 0x2D) {
 804                         /* Invalid value 0x2D is detected in Extended Supported
 805                          * Rates (ESR) IE. Try to fix the IE length to avoid
 806                          * failed Beacon parsing.
 807                          */
 808                         DBG_8723A("[WIFIDBG] Error in ESR IE is detected in "
 809                                   "Beacon of BSSID: %pM. Fix the length of "
 810                                   "ESR IE to avoid failed Beacon parsing.\n",
 811                                   mgmt->bssid);
 812                         p[1] = ielen - 1;
 813                 }
 814         }
 815
 816         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
 817                 report_survey_event23a(padapter, precv_frame);
 818                 return _SUCCESS;
 819         }
 820
 821         if (!ether_addr_equal(mgmt->bssid,
 822                               get_my_bssid23a(&pmlmeinfo->network)))
 823                 goto out;
 824
 825         if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
 826                 /* we should update current network before auth,
 827                    or some IE is wrong */
 828                 pbss = collect_bss_info(padapter, precv_frame);
 829                 if (pbss) {
 830                         update_network23a(&pmlmepriv->cur_network.network, pbss,
 831                                           padapter, true);
 832                         rtw_get_bcn_info23a(&pmlmepriv->cur_network);
 833                         kfree(pbss);
 834                 }
 835
 836                 /* check the vendor of the assoc AP */
 837                 pmlmeinfo->assoc_AP_vendor =
 838                         check_assoc_AP23a((u8 *)&mgmt->u.beacon, pkt_len -
 839                                           offsetof(struct ieee80211_mgmt, u));
 840
 841                 /* update TSF Value */
 842                 rtw_update_TSF(pmlmeext, mgmt);
 843
 844                 /* start auth */
 845                 start_clnt_auth(padapter);
 846
 847                 return _SUCCESS;
 848         }
 849
 850         if (((pmlmeinfo->state & 0x03) == MSR_AP) &&
 851             (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
 852                 psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
 853                 if (psta) {
 854                         ret = rtw_check_bcn_info23a(padapter, mgmt, pkt_len);
 855                         if (ret != _SUCCESS) {
 856                                 DBG_8723A_LEVEL(_drv_always_, "ap has changed, "
 857                                                 "disconnect now\n");
 858                                 receive_disconnect23a(padapter, pmlmeinfo->network.MacAddress, 65535);
 859                                 return _SUCCESS;
 860                         }
 861                         /* update WMM, ERP in the beacon */
 862                         /* todo: the timer is used instead of
 863                            the number of the beacon received */
 864                         if ((sta_rx_pkts(psta) & 0xf) == 0) {
 865                                 /* DBG_8723A("update_bcn_info\n"); */
 866                                 update_beacon23a_info(padapter, mgmt,
 867                                                       pkt_len, psta);
 868                         }
 869                 }
 870         } else if ((pmlmeinfo->state&0x03) == MSR_ADHOC) {
 871                 psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
 872                 if (psta) {
 873                         /* update WMM, ERP in the beacon */
 874                         /* todo: the timer is used instead of the
 875                            number of the beacon received */
 876                         if ((sta_rx_pkts(psta) & 0xf) == 0) {
 877                                 /* DBG_8723A("update_bcn_info\n"); */
 878                                 update_beacon23a_info(padapter, mgmt,
 879                                                       pkt_len, psta);
 880                         }
 881                 } else {
 882                         /* allocate a new CAM entry for IBSS station */
 883                         cam_idx = allocate_fw_sta_entry23a(padapter);
 884                         if (cam_idx == NUM_STA)
 885                                 goto out;
 886
 887                         /* get supported rate */
 888                         if (update_sta_support_rate23a(padapter, pie, pie_len,
 889                                                        cam_idx) == _FAIL) {
 890                                 pmlmeinfo->FW_sta_info[cam_idx].status = 0;
 891                                 goto out;
 892                         }
 893
 894                         /* update TSF Value */
 895                         rtw_update_TSF(pmlmeext, mgmt);
 896
 897                         /* report sta add event */
 898                         report_add_sta_event23a(padapter, mgmt->sa,
 899                                                 cam_idx);
 900                 }
 901         }
 902
 903 out:
 904
 905         return _SUCCESS;
 906 }
 907
 908 #ifdef CONFIG_8723AU_AP_MODE
 909 static int
 910 OnAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 911 {
 912         static struct sta_info stat;
 913         struct sta_info *pstat = NULL;
 914         struct sta_priv *pstapriv = &padapter->stapriv;
 915         struct security_priv *psecuritypriv = &padapter->securitypriv;
 916         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 917         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 918         struct sk_buff *skb = precv_frame->pkt;
 919         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 920         u8 *pframe;
 921         const u8 *p;
 922         unsigned char *sa;
 923         u16 auth_mode, seq, algorithm;
 924         int status, len = skb->len;
 925
 926         if ((pmlmeinfo->state & 0x03) != MSR_AP)
 927                 return _FAIL;
 928
 929         DBG_8723A("+OnAuth23a\n");
 930
 931         sa = mgmt->sa;
 932
 933         auth_mode = psecuritypriv->dot11AuthAlgrthm;
 934
 935         pframe = mgmt->u.auth.variable;
 936         len = skb->len - offsetof(struct ieee80211_mgmt, u.auth.variable);
 937
 938         seq = le16_to_cpu(mgmt->u.auth.auth_transaction);
 939         algorithm = le16_to_cpu(mgmt->u.auth.auth_alg);
 940
 941         DBG_8723A("auth alg =%x, seq =%X\n", algorithm, seq);
 942
 943         if (auth_mode == 2 &&
 944             psecuritypriv->dot11PrivacyAlgrthm != WLAN_CIPHER_SUITE_WEP40 &&
 945             psecuritypriv->dot11PrivacyAlgrthm != WLAN_CIPHER_SUITE_WEP104)
 946                 auth_mode = 0;
 947
 948         /*  rx a shared-key auth but shared not enabled, or */
 949         /*  rx a open-system auth but shared-key is enabled */
 950         if ((algorithm != WLAN_AUTH_OPEN && auth_mode == 0) ||
 951             (algorithm == WLAN_AUTH_OPEN && auth_mode == 1)) {
 952                 DBG_8723A("auth rejected due to bad alg [alg =%d, auth_mib "
 953                           "=%d] %02X%02X%02X%02X%02X%02X\n",
 954                           algorithm, auth_mode,
 955                           sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
 956
 957                 status = WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG;
 958
 959                 goto auth_fail;
 960         }
 961
 962         if (rtw_access_ctrl23a(padapter, sa) == false) {
 963                 status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
 964                 goto auth_fail;
 965         }
 966
 967         pstat = rtw_get_stainfo23a(pstapriv, sa);
 968         if (!pstat) {
 969                 /*  allocate a new one */
 970                 DBG_8723A("going to alloc stainfo for sa =%pM\n", sa);
 971                 pstat = rtw_alloc_stainfo23a(pstapriv, sa, GFP_ATOMIC);
 972                 if (!pstat) {
 973                         DBG_8723A(" Exceed the upper limit of supported "
 974                                   "clients...\n");
 975                         status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
 976                         goto auth_fail;
 977                 }
 978
 979                 pstat->state = WIFI_FW_AUTH_NULL;
 980                 pstat->auth_seq = 0;
 981
 982                 /* pstat->flags = 0; */
 983                 /* pstat->capability = 0; */
 984         } else {
 985                 spin_lock_bh(&pstapriv->asoc_list_lock);
 986                 if (!list_empty(&pstat->asoc_list)) {
 987                         list_del_init(&pstat->asoc_list);
 988                         pstapriv->asoc_list_cnt--;
 989                         if (pstat->expire_to > 0) {
 990                                 /* TODO: STA re_auth within expire_to */
 991                         }
 992                 }
 993                 spin_unlock_bh(&pstapriv->asoc_list_lock);
 994
 995                 if (seq == 1) {
 996                         /* TODO: STA re_auth and auth timeout */
 997                 }
 998         }
 999
1000         spin_lock_bh(&pstapriv->auth_list_lock);
1001         if (list_empty(&pstat->auth_list)) {
1002                 list_add_tail(&pstat->auth_list, &pstapriv->auth_list);
1003                 pstapriv->auth_list_cnt++;
1004         }
1005         spin_unlock_bh(&pstapriv->auth_list_lock);
1006
1007         if (pstat->auth_seq == 0)
1008                 pstat->expire_to = pstapriv->auth_to;
1009
1010         if ((pstat->auth_seq + 1) != seq) {
1011                 DBG_8723A("(1)auth rejected because out of seq [rx_seq =%d, "
1012                           "exp_seq =%d]!\n", seq, pstat->auth_seq+1);
1013                 status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION;
1014                 goto auth_fail;
1015         }
1016
1017         if (algorithm == WLAN_AUTH_OPEN && (auth_mode == 0 || auth_mode == 2)) {
1018                 if (seq == 1) {
1019                         pstat->state &= ~WIFI_FW_AUTH_NULL;
1020                         pstat->state |= WIFI_FW_AUTH_SUCCESS;
1021                         pstat->expire_to = pstapriv->assoc_to;
1022                         pstat->authalg = algorithm;
1023                 } else {
1024                         DBG_8723A("(2)auth rejected because out of seq "
1025                                   "[rx_seq =%d, exp_seq =%d]!\n",
1026                                   seq, pstat->auth_seq+1);
1027                         status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION;
1028                         goto auth_fail;
1029                 }
1030         } else { /*  shared system or auto authentication */
1031                 if (seq == 1) {
1032                         /* prepare for the challenging txt... */
1033                         pstat->state &= ~WIFI_FW_AUTH_NULL;
1034                         pstat->state |= WIFI_FW_AUTH_STATE;
1035                         pstat->authalg = algorithm;
1036                         pstat->auth_seq = 2;
1037                 } else if (seq == 3) {
1038                         /* checking for challenging txt... */
1039                         DBG_8723A("checking for challenging txt...\n");
1040
1041                         p = cfg80211_find_ie(WLAN_EID_CHALLENGE, pframe, len);
1042                         if (!p || p[1] <= 0) {
1043                                 DBG_8723A("auth rejected because challenge "
1044                                           "failure!(1)\n");
1045                                 status = WLAN_STATUS_CHALLENGE_FAIL;
1046                                 goto auth_fail;
1047                         }
1048
1049                         if (!memcmp(p + 2, pstat->chg_txt, 128)) {
1050                                 pstat->state &= ~WIFI_FW_AUTH_STATE;
1051                                 pstat->state |= WIFI_FW_AUTH_SUCCESS;
1052                                 /*  challenging txt is correct... */
1053                                 pstat->expire_to =  pstapriv->assoc_to;
1054                         } else {
1055                                 DBG_8723A("auth rejected because challenge "
1056                                           "failure!\n");
1057                                 status = WLAN_STATUS_CHALLENGE_FAIL;
1058                                 goto auth_fail;
1059                         }
1060                 } else {
1061                         DBG_8723A("(3)auth rejected because out of seq "
1062                                   "[rx_seq =%d, exp_seq =%d]!\n",
1063                                   seq, pstat->auth_seq+1);
1064                         status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION;
1065                         goto auth_fail;
1066                 }
1067         }
1068
1069         /*  Now, we are going to issue_auth... */
1070         pstat->auth_seq = seq + 1;
1071
1072         issue_auth(padapter, pstat, WLAN_STATUS_SUCCESS);
1073
1074         if (pstat->state & WIFI_FW_AUTH_SUCCESS)
1075                 pstat->auth_seq = 0;
1076
1077         return _SUCCESS;
1078
1079 auth_fail:
1080
1081         if (pstat)
1082                 rtw_free_stainfo23a(padapter, pstat);
1083
1084         pstat = &stat;
1085         memset((char *)pstat, '\0', sizeof(stat));
1086         pstat->auth_seq = 2;
1087         ether_addr_copy(pstat->hwaddr, sa);
1088
1089         issue_auth(padapter, pstat, (unsigned short)status);
1090
1091         return _FAIL;
1092 }
1093 #endif
1094
1095 static int
1096 OnAuth23aClient23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
1097 {
1098         unsigned int seq, status, algthm;
1099         unsigned int go2asoc = 0;
1100         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
1101         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
1102         struct sk_buff *skb = precv_frame->pkt;
1103         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
1104         const u8 *p;
1105         u8 *pie;
1106         int plen = skb->len;
1107
1108         DBG_8723A("%s\n", __func__);
1109
1110         /* check A1 matches or not */
1111         if (!ether_addr_equal(myid(&padapter->eeprompriv), mgmt->da))
1112                 return _SUCCESS;
1113
1114         if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
1115                 return _SUCCESS;
1116
1117         pie = mgmt->u.auth.variable;
1118         plen -= offsetof(struct ieee80211_mgmt, u.auth.variable);
1119
1120         algthm = le16_to_cpu(mgmt->u.auth.auth_alg);
1121         seq = le16_to_cpu(mgmt->u.auth.auth_transaction);
1122         status = le16_to_cpu(mgmt->u.auth.status_code);
1123
1124         if (status) {
1125                 DBG_8723A("clnt auth fail, status: %d\n", status);
1126                 /*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
1127                 if (status == WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG) {
1128                         if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1129                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
1130                         else
1131                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
1132                         /* pmlmeinfo->reauth_count = 0; */
1133                 }
1134
1135                 set_link_timer(pmlmeext, 1);
1136                 goto authclnt_fail;
1137         }
1138
1139         if (seq == 2) {
1140                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
1141                         /*  legendary shared system */
1142                         p = cfg80211_find_ie(WLAN_EID_CHALLENGE, pie, plen);
1143
1144                         if (!p) {
1145                                 /* DBG_8723A("marc: no challenge text?\n"); */
1146                                 goto authclnt_fail;
1147                         }
1148
1149                         memcpy((void *)(pmlmeinfo->chg_txt), p + 2, p[1]);
1150                         pmlmeinfo->auth_seq = 3;
1151                         issue_auth(padapter, NULL, 0);
1152                         set_link_timer(pmlmeext, REAUTH_TO);
1153
1154                         return _SUCCESS;
1155                 } else {
1156                         /*  open system */
1157                         go2asoc = 1;
1158                 }
1159         } else if (seq == 4) {
1160                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1161                         go2asoc = 1;
1162                 else
1163                         goto authclnt_fail;
1164         } else {
1165                 /*  this is also illegal */
1166                 /* DBG_8723A("marc: clnt auth failed due to illegal seq =%x\n",
1167                    seq); */
1168                 goto authclnt_fail;
1169         }
1170
1171         if (go2asoc) {
1172                 DBG_8723A_LEVEL(_drv_always_, "auth success, start assoc\n");
1173                 start_clnt_assoc(padapter);
1174                 return _SUCCESS;
1175         }
1176
1177 authclnt_fail:
1178
1179         /* pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE); */
1180
1181         return _FAIL;
1182 }
1183
1184 #ifdef CONFIG_8723AU_AP_MODE
1185 static int rtw_validate_vendor_specific_ies(const u8 *pos, int elen)
1186 {
1187         unsigned int oui;
1188
1189         /* first 3 bytes in vendor specific information element are the IEEE
1190          * OUI of the vendor. The following byte is used a vendor specific
1191          * sub-type. */
1192         if (elen < 4) {
1193                 DBG_8723A("short vendor specific information element "
1194                           "ignored (len =%i)\n", elen);
1195                 return -EINVAL;
1196         }
1197
1198         oui = RTW_GET_BE24(pos);
1199         switch (oui) {
1200         case WLAN_OUI_MICROSOFT:
1201                 /* Microsoft/Wi-Fi information elements are further typed and
1202                  * subtyped */
1203                 switch (pos[3]) {
1204                 case WLAN_OUI_TYPE_MICROSOFT_WPA:
1205                         /* Microsoft OUI (00:50:F2) with OUI Type 1:
1206                          * real WPA information element */
1207                         break;
1208                 case WLAN_OUI_TYPE_MICROSOFT_WMM:
1209                         if (elen < 5) {
1210                                 DBG_8723A("short WME information element "
1211                                           "ignored (len =%i)\n", elen);
1212                                 return -EINVAL;
1213                         }
1214                         switch (pos[4]) {
1215                         case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
1216                         case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
1217                                 break;
1218                         case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
1219                                 break;
1220                         default:
1221                                 DBG_8723A("unknown WME information element "
1222                                           "ignored (subtype =%d len =%i)\n",
1223                                            pos[4], elen);
1224                                 return -EINVAL;
1225                         }
1226                         break;
1227                 case WLAN_OUI_TYPE_MICROSOFT_WPS:
1228                         /* Wi-Fi Protected Setup (WPS) IE */
1229                         break;
1230                 default:
1231                         DBG_8723A("Unknown Microsoft information element "
1232                                   "ignored (type =%d len =%i)\n",
1233                                   pos[3], elen);
1234                         return -EINVAL;
1235                 }
1236                 break;
1237
1238         case OUI_BROADCOM:
1239                 switch (pos[3]) {
1240                 case VENDOR_HT_CAPAB_OUI_TYPE:
1241                         break;
1242                 default:
1243                         DBG_8723A("Unknown Broadcom information element "
1244                                   "ignored (type =%d len =%i)\n", pos[3], elen);
1245                         return -EINVAL;
1246                 }
1247                 break;
1248
1249         default:
1250                 DBG_8723A("unknown vendor specific information element "
1251                           "ignored (vendor OUI %02x:%02x:%02x len =%i)\n",
1252                            pos[0], pos[1], pos[2], elen);
1253                 return -EINVAL;
1254         }
1255
1256         return 0;
1257 }
1258
1259 static int rtw_validate_frame_ies(const u8 *start, uint len)
1260 {
1261         const u8 *pos = start;
1262         int left = len;
1263         int unknown = 0;
1264
1265         while (left >= 2) {
1266                 u8 id, elen;
1267
1268                 id = *pos++;
1269                 elen = *pos++;
1270                 left -= 2;
1271
1272                 if (elen > left) {
1273                         DBG_8723A("%s: IEEE 802.11 failed (id =%d elen =%d "
1274                                   "left =%i)\n", __func__, id, elen, left);
1275                         return -EINVAL;
1276                 }
1277
1278                 switch (id) {
1279                 case WLAN_EID_SSID:
1280                 case WLAN_EID_SUPP_RATES:
1281                 case WLAN_EID_FH_PARAMS:
1282                 case WLAN_EID_DS_PARAMS:
1283                 case WLAN_EID_CF_PARAMS:
1284                 case WLAN_EID_TIM:
1285                 case WLAN_EID_IBSS_PARAMS:
1286                 case WLAN_EID_CHALLENGE:
1287                 case WLAN_EID_ERP_INFO:
1288                 case WLAN_EID_EXT_SUPP_RATES:
1289                         break;
1290                 case WLAN_EID_VENDOR_SPECIFIC:
1291                         if (rtw_validate_vendor_specific_ies(pos, elen))
1292                                 unknown++;
1293                         break;
1294                 case WLAN_EID_RSN:
1295                 case WLAN_EID_PWR_CAPABILITY:
1296                 case WLAN_EID_SUPPORTED_CHANNELS:
1297                 case WLAN_EID_MOBILITY_DOMAIN:
1298                 case WLAN_EID_FAST_BSS_TRANSITION:
1299                 case WLAN_EID_TIMEOUT_INTERVAL:
1300                 case WLAN_EID_HT_CAPABILITY:
1301                 case WLAN_EID_HT_OPERATION:
1302                 default:
1303                         unknown++;
1304                         DBG_8723A("%s IEEE 802.11 ignored unknown element "
1305                                   "(id =%d elen =%d)\n", __func__, id, elen);
1306                         break;
1307                 }
1308
1309                 left -= elen;
1310                 pos += elen;
1311         }
1312
1313         if (left)
1314                 return -EINVAL;
1315
1316         return 0;
1317 }
1318 #endif
1319
1320 static int
1321 OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
1322 {
1323 #ifdef CONFIG_8723AU_AP_MODE
1324         u16 capab_info, listen_interval;
1325         struct sta_info *pstat;
1326         unsigned char reassoc;
1327         int i, wpa_ie_len, left;
1328         unsigned char supportRate[16];
1329         int supportRateNum;
1330         unsigned short status = WLAN_STATUS_SUCCESS;
1331         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1332         struct security_priv *psecuritypriv = &padapter->securitypriv;
1333         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
1334         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
1335         struct wlan_bssid_ex *cur = &pmlmeinfo->network;
1336         struct sta_priv *pstapriv = &padapter->stapriv;
1337         struct sk_buff *skb = precv_frame->pkt;
1338         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
1339         const u8 *pos, *p, *wpa_ie, *wps_ie;
1340         u8 *pframe = skb->data;
1341         uint pkt_len = skb->len;
1342         int r;
1343
1344         if ((pmlmeinfo->state & 0x03) != MSR_AP)
1345                 return _FAIL;
1346
1347         left = pkt_len - sizeof(struct ieee80211_hdr_3addr);
1348         if (ieee80211_is_assoc_req(mgmt->frame_control)) {
1349                 reassoc = 0;
1350                 pos = mgmt->u.assoc_req.variable;
1351                 left -= offsetof(struct ieee80211_mgmt, u.assoc_req.variable);
1352         } else { /*  WIFI_REASSOCREQ */
1353                 reassoc = 1;
1354                 pos = mgmt->u.reassoc_req.variable;
1355                 left -= offsetof(struct ieee80211_mgmt, u.reassoc_req.variable);
1356         }
1357
1358         if (left < 0) {
1359                 DBG_8723A("handle_assoc(reassoc =%d) - too short payload "
1360                           "(len =%lu)\n", reassoc, (unsigned long)pkt_len);
1361                 return _FAIL;
1362         }
1363
1364         pstat = rtw_get_stainfo23a(pstapriv, mgmt->sa);
1365         if (!pstat) {
1366                 status = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA;
1367                 goto asoc_class2_error;
1368         }
1369
1370         /* These two are located at the same offsets whether it's an
1371          * assoc_req or a reassoc_req */
1372         capab_info = get_unaligned_le16(&mgmt->u.assoc_req.capab_info);
1373         listen_interval =
1374                 get_unaligned_le16(&mgmt->u.assoc_req.listen_interval);
1375
1376         DBG_8723A("%s\n", __func__);
1377
1378         /*  check if this stat has been successfully authenticated/assocated */
1379         if (!(pstat->state & WIFI_FW_AUTH_SUCCESS)) {
1380                 if (!(pstat->state & WIFI_FW_ASSOC_SUCCESS)) {
1381                         status = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA;
1382                         goto asoc_class2_error;
1383                 } else {
1384                         pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
1385                         pstat->state |= WIFI_FW_ASSOC_STATE;
1386                 }
1387         } else {
1388                 pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
1389                 pstat->state |= WIFI_FW_ASSOC_STATE;
1390         }
1391
1392         pstat->capability = capab_info;
1393
1394         /* now parse all ieee802_11 ie to point to elems */
1395
1396         if (rtw_validate_frame_ies(pos, left)) {
1397                 DBG_8723A("STA %pM sent invalid association request\n",
1398                           pstat->hwaddr);
1399                 status = WLAN_STATUS_UNSPECIFIED_FAILURE;
1400                 goto OnAssocReq23aFail;
1401         }
1402
1403         /*  now we should check all the fields... */
1404         /*  checking SSID */
1405         p = cfg80211_find_ie(WLAN_EID_SSID, pos, left);
1406         if (!p || p[1] == 0) {
1407                 /*  broadcast ssid, however it is not allowed in assocreq */
1408                 DBG_8723A("STA %pM sent invalid association request lacking an SSID\n",
1409                           pstat->hwaddr);
1410                 status = WLAN_STATUS_UNSPECIFIED_FAILURE;
1411                 goto OnAssocReq23aFail;
1412         } else {
1413                 /*  check if ssid match */
1414                 if (memcmp(p + 2, cur->Ssid.ssid, cur->Ssid.ssid_len))
1415                         status = WLAN_STATUS_UNSPECIFIED_FAILURE;
1416
1417                 if (p[1] != cur->Ssid.ssid_len)
1418                         status = WLAN_STATUS_UNSPECIFIED_FAILURE;
1419         }
1420
1421         if (status != WLAN_STATUS_SUCCESS)
1422                 goto OnAssocReq23aFail;
1423
1424         /*  check if the supported rate is ok */
1425         p = cfg80211_find_ie(WLAN_EID_SUPP_RATES, pos, left);
1426         if (!p) {
1427                 DBG_8723A("Rx a sta assoc-req which supported rate is "
1428                           "empty!\n");
1429                 /*  use our own rate set as statoin used */
1430                 /* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
1431                 /* supportRateNum = AP_BSSRATE_LEN; */
1432
1433                 status = WLAN_STATUS_UNSPECIFIED_FAILURE;
1434                 goto OnAssocReq23aFail;
1435         } else {
1436                 memcpy(supportRate, p + 2, p[1]);
1437                 supportRateNum = p[1];
1438
1439                 p = cfg80211_find_ie(WLAN_EID_EXT_SUPP_RATES, pos, left);
1440                 if (p) {
1441                         if (supportRateNum <= sizeof(supportRate)) {
1442                                 memcpy(supportRate+supportRateNum, p + 2, p[1]);
1443                                 supportRateNum += p[1];
1444                         }
1445                 }
1446         }
1447
1448         /* todo: mask supportRate between AP & STA -> move to update raid */
1449         /* get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0); */
1450
1451         /* update station supportRate */
1452         pstat->bssratelen = supportRateNum;
1453         memcpy(pstat->bssrateset, supportRate, supportRateNum);
1454         Update23aTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
1455
1456         /* check RSN/WPA/WPS */
1457         pstat->dot8021xalg = 0;
1458         pstat->wpa_psk = 0;
1459         pstat->wpa_group_cipher = 0;
1460         pstat->wpa2_group_cipher = 0;
1461         pstat->wpa_pairwise_cipher = 0;
1462         pstat->wpa2_pairwise_cipher = 0;
1463         memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
1464
1465         wpa_ie = cfg80211_find_ie(WLAN_EID_RSN, pos, left);
1466         if (!wpa_ie)
1467                 wpa_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
1468                                                  WLAN_OUI_TYPE_MICROSOFT_WPA,
1469                                                  pos, left);
1470         if (wpa_ie) {
1471                 int group_cipher = 0, pairwise_cipher = 0;
1472
1473                 wpa_ie_len = wpa_ie[1];
1474                 if (psecuritypriv->wpa_psk & BIT(1)) {
1475                         r = rtw_parse_wpa2_ie23a(wpa_ie, wpa_ie_len + 2,
1476                                                  &group_cipher,
1477                                                  &pairwise_cipher, NULL);
1478                         if (r == _SUCCESS) {
1479                                 pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
1480                                 pstat->wpa_psk |= BIT(1);
1481
1482                                 pstat->wpa2_group_cipher = group_cipher &
1483                                         psecuritypriv->wpa2_group_cipher;
1484                                 pstat->wpa2_pairwise_cipher = pairwise_cipher &
1485                                         psecuritypriv->wpa2_pairwise_cipher;
1486                         } else
1487                                 status = WLAN_STATUS_INVALID_IE;
1488                 } else if (psecuritypriv->wpa_psk & BIT(0)) {
1489                         r = rtw_parse_wpa_ie23a(wpa_ie, wpa_ie_len + 2,
1490                                                 &group_cipher, &pairwise_cipher,
1491                                                 NULL);
1492                         if (r == _SUCCESS) {
1493                                 pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
1494                                 pstat->wpa_psk |= BIT(0);
1495
1496                                 pstat->wpa_group_cipher = group_cipher &
1497                                         psecuritypriv->wpa_group_cipher;
1498                                 pstat->wpa_pairwise_cipher = pairwise_cipher &
1499                                         psecuritypriv->wpa_pairwise_cipher;
1500                         } else
1501                                 status = WLAN_STATUS_INVALID_IE;
1502                 } else {
1503                         wpa_ie = NULL;
1504                         wpa_ie_len = 0;
1505                 }
1506                 if (wpa_ie && status == WLAN_STATUS_SUCCESS) {
1507                         if (!pstat->wpa_group_cipher)
1508                                 status = WLAN_STATUS_INVALID_GROUP_CIPHER;
1509
1510                         if (!pstat->wpa_pairwise_cipher)
1511                                 status = WLAN_STATUS_INVALID_PAIRWISE_CIPHER;
1512                 }
1513         }
1514
1515         if (status != WLAN_STATUS_SUCCESS)
1516                 goto OnAssocReq23aFail;
1517
1518         pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
1519
1520         wps_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
1521                                          WLAN_OUI_TYPE_MICROSOFT_WPS,
1522                                          pos, left);
1523
1524         if (!wpa_ie) {
1525                 if (wps_ie) {
1526                         DBG_8723A("STA included WPS IE in (Re)Association "
1527                                   "Request - assume WPS is used\n");
1528                         pstat->flags |= WLAN_STA_WPS;
1529                 } else {
1530                         DBG_8723A("STA did not include WPA/RSN IE in (Re)"
1531                                    "Association Request - possible WPS use\n");
1532                         pstat->flags |= WLAN_STA_MAYBE_WPS;
1533                 }
1534         } else {
1535                 int copy_len;
1536
1537                 if (psecuritypriv->wpa_psk == 0) {
1538                         DBG_8723A("STA %pM: WPA/RSN IE in association request, but AP don't support WPA/RSN\n",
1539                                   pstat->hwaddr);
1540
1541                         status = WLAN_STATUS_INVALID_IE;
1542
1543                         goto OnAssocReq23aFail;
1544                 }
1545
1546                 if (wps_ie) {
1547                         DBG_8723A("STA included WPS IE in (Re)Association "
1548                                   "Request - WPS is used\n");
1549                         pstat->flags |= WLAN_STA_WPS;
1550                         copy_len = 0;
1551                 } else {
1552                         copy_len = ((wpa_ie_len + 2) > sizeof(pstat->wpa_ie)) ?
1553                                 sizeof(pstat->wpa_ie) : (wpa_ie_len + 2);
1554                 }
1555
1556                 if (copy_len > 0)
1557                         memcpy(pstat->wpa_ie, wpa_ie - 2, copy_len);
1558         }
1559
1560         /*  check if there is WMM IE & support WWM-PS */
1561         pstat->flags &= ~WLAN_STA_WME;
1562         pstat->qos_option = 0;
1563         pstat->qos_info = 0;
1564         pstat->has_legacy_ac = true;
1565         pstat->uapsd_vo = 0;
1566         pstat->uapsd_vi = 0;
1567         pstat->uapsd_be = 0;
1568         pstat->uapsd_bk = 0;
1569         if (pmlmepriv->qos_option) {
1570                 const u8 *end = pos + left;
1571
1572                 p = pos;
1573
1574                 for (;;) {
1575                         left = end - p;
1576                         p = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
1577                                                     WLAN_OUI_TYPE_MICROSOFT_WMM,
1578                                                     p, left);
1579                         if (p) {
1580                                 pstat->flags |= WLAN_STA_WME;
1581
1582                                 pstat->qos_option = 1;
1583                                 pstat->qos_info = *(p + 8);
1584
1585                                 pstat->max_sp_len =
1586                                         (pstat->qos_info >> 5) & 0x3;
1587
1588                                 if ((pstat->qos_info & 0xf) != 0xf)
1589                                         pstat->has_legacy_ac = true;
1590                                 else
1591                                         pstat->has_legacy_ac = false;
1592
1593                                 if (pstat->qos_info & 0xf) {
1594                                         if (pstat->qos_info & BIT(0))
1595                                                 pstat->uapsd_vo = BIT(0)|BIT(1);
1596                                         else
1597                                                 pstat->uapsd_vo = 0;
1598
1599                                         if (pstat->qos_info & BIT(1))
1600                                                 pstat->uapsd_vi = BIT(0)|BIT(1);
1601                                         else
1602                                                 pstat->uapsd_vi = 0;
1603
1604                                         if (pstat->qos_info & BIT(2))
1605                                                 pstat->uapsd_bk = BIT(0)|BIT(1);
1606                                         else
1607                                                 pstat->uapsd_bk = 0;
1608
1609                                         if (pstat->qos_info & BIT(3))
1610                                                 pstat->uapsd_be = BIT(0)|BIT(1);
1611                                         else
1612                                                 pstat->uapsd_be = 0;
1613
1614                                         break;
1615                                 }
1616                         } else {
1617                                 break;
1618                         }
1619                         p = p + p[1] + 2;
1620                 }
1621         }
1622
1623         /* save HT capabilities in the sta object */
1624         memset(&pstat->htpriv.ht_cap, 0, sizeof(struct ieee80211_ht_cap));
1625         p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, pos, left);
1626
1627         if (p && p[1] >= sizeof(struct ieee80211_ht_cap)) {
1628                 pstat->flags |= WLAN_STA_HT;
1629
1630                 pstat->flags |= WLAN_STA_WME;
1631
1632                 memcpy(&pstat->htpriv.ht_cap, p + 2,
1633                        sizeof(struct ieee80211_ht_cap));
1634         } else
1635                 pstat->flags &= ~WLAN_STA_HT;
1636
1637         if (!pmlmepriv->htpriv.ht_option && pstat->flags & WLAN_STA_HT){
1638                 status = WLAN_STATUS_UNSPECIFIED_FAILURE;
1639                 goto OnAssocReq23aFail;
1640         }
1641
1642         if (pstat->flags & WLAN_STA_HT &&
1643             (pstat->wpa2_pairwise_cipher & WPA_CIPHER_TKIP ||
1644              pstat->wpa_pairwise_cipher & WPA_CIPHER_TKIP)) {
1645                 DBG_8723A("HT: %pM tried to use TKIP with HT association\n",
1646                           pstat->hwaddr);
1647
1648                 /* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
1649                 /* goto OnAssocReq23aFail; */
1650         }
1651
1652         pstat->flags |= WLAN_STA_NONERP;
1653         for (i = 0; i < pstat->bssratelen; i++) {
1654                 if ((pstat->bssrateset[i] & 0x7f) > 22) {
1655                         pstat->flags &= ~WLAN_STA_NONERP;
1656                         break;
1657                 }
1658         }
1659
1660         if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
1661                 pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
1662         else
1663                 pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
1664
1665         if (status != WLAN_STATUS_SUCCESS)
1666                 goto OnAssocReq23aFail;
1667
1668         /* TODO: identify_proprietary_vendor_ie(); */
1669         /*  Realtek proprietary IE */
1670         /*  identify if this is Broadcom sta */
1671         /*  identify if this is ralink sta */
1672         /*  Customer proprietary IE */
1673
1674         /* get a unique AID */
1675         if (pstat->aid > 0) {
1676                 DBG_8723A("  old AID %d\n", pstat->aid);
1677         } else {
1678                 for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
1679                         if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
1680                                 break;
1681
1682                 if (pstat->aid > NUM_STA)
1683                         pstat->aid = NUM_STA;
1684                 if (pstat->aid > pstapriv->max_num_sta) {
1685
1686                         pstat->aid = 0;
1687
1688                         DBG_8723A("  no room for more AIDs\n");
1689
1690                         status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
1691
1692                         goto OnAssocReq23aFail;
1693                 } else {
1694                         pstapriv->sta_aid[pstat->aid - 1] = pstat;
1695                         DBG_8723A("allocate new AID = (%d)\n", pstat->aid);
1696                 }
1697         }
1698
1699         pstat->state &= ~WIFI_FW_ASSOC_STATE;
1700         pstat->state |= WIFI_FW_ASSOC_SUCCESS;
1701
1702         spin_lock_bh(&pstapriv->auth_list_lock);
1703         if (!list_empty(&pstat->auth_list)) {
1704                 list_del_init(&pstat->auth_list);
1705                 pstapriv->auth_list_cnt--;
1706         }
1707         spin_unlock_bh(&pstapriv->auth_list_lock);
1708
1709         spin_lock_bh(&pstapriv->asoc_list_lock);
1710         if (list_empty(&pstat->asoc_list)) {
1711                 pstat->expire_to = pstapriv->expire_to;
1712                 list_add_tail(&pstat->asoc_list, &pstapriv->asoc_list);
1713                 pstapriv->asoc_list_cnt++;
1714         }
1715         spin_unlock_bh(&pstapriv->asoc_list_lock);
1716
1717         /*  now the station is qualified to join our BSS... */
1718         if (pstat->state & WIFI_FW_ASSOC_SUCCESS &&
1719             status == WLAN_STATUS_SUCCESS) {
1720                 /* 1 bss_cap_update & sta_info_update23a */
1721                 bss_cap_update_on_sta_join23a(padapter, pstat);
1722                 sta_info_update23a(padapter, pstat);
1723
1724                 /* issue assoc rsp before notify station join event. */
1725                 if (ieee80211_is_assoc_req(mgmt->frame_control))
1726                         issue_assocrsp(padapter, status, pstat,
1727                                        IEEE80211_STYPE_ASSOC_RESP);
1728                 else
1729                         issue_assocrsp(padapter, status, pstat,
1730                                        IEEE80211_STYPE_REASSOC_RESP);
1731
1732                 /* 2 - report to upper layer */
1733                 DBG_8723A("indicate_sta_join_event to upper layer - hostapd\n");
1734                 rtw_cfg80211_indicate_sta_assoc(padapter, pframe, pkt_len);
1735
1736                 /* 3-(1) report sta add event */
1737                 report_add_sta_event23a(padapter, pstat->hwaddr, pstat->aid);
1738         }
1739
1740         return _SUCCESS;
1741
1742 asoc_class2_error:
1743
1744         issue_deauth23a(padapter, mgmt->sa, status);
1745         return _FAIL;
1746
1747 OnAssocReq23aFail:
1748
1749         pstat->aid = 0;
1750         if (ieee80211_is_assoc_req(mgmt->frame_control))
1751                 issue_assocrsp(padapter, status, pstat,
1752                                IEEE80211_STYPE_ASSOC_RESP);
1753         else
1754                 issue_assocrsp(padapter, status, pstat,
1755                                IEEE80211_STYPE_REASSOC_RESP);
1756
1757 #endif /* CONFIG_8723AU_AP_MODE */
1758
1759         return _FAIL;
1760 }
1761
1762 static int
1763 OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
1764 {
1765         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1766         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
1767         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
1768         struct sk_buff *skb = precv_frame->pkt;
1769         struct ieee80211_mgmt *pmgmt = (struct ieee80211_mgmt *) skb->data;
1770         int res;
1771         unsigned short status;
1772         const u8 *p, *pie;
1773         u8 *pframe = skb->data;
1774         int pkt_len = skb->len;
1775         int pielen;
1776
1777         DBG_8723A("%s\n", __func__);
1778
1779         /* check A1 matches or not */
1780         if (!ether_addr_equal(myid(&padapter->eeprompriv), pmgmt->da))
1781                 return _SUCCESS;
1782
1783         if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
1784                 return _SUCCESS;
1785
1786         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
1787                 return _SUCCESS;
1788
1789         del_timer_sync(&pmlmeext->link_timer);
1790
1791         /* status */
1792         status = le16_to_cpu(pmgmt->u.assoc_resp.status_code);
1793         if (status > 0) {
1794                 DBG_8723A("assoc reject, status code: %d\n", status);
1795                 pmlmeinfo->state = MSR_NOLINK;
1796                 res = -4;
1797                 goto report_assoc_result;
1798         }
1799
1800         /* get capabilities */
1801         pmlmeinfo->capability = le16_to_cpu(pmgmt->u.assoc_resp.capab_info);
1802
1803         /* set slot time */
1804         pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10))? 9: 20;
1805
1806         /* AID */
1807         res = pmlmeinfo->aid = le16_to_cpu(pmgmt->u.assoc_resp.aid) & 0x3fff;
1808
1809         pie = pframe + offsetof(struct ieee80211_mgmt, u.assoc_resp.variable);
1810         pielen = pkt_len -
1811                 offsetof(struct ieee80211_mgmt, u.assoc_resp.variable);
1812
1813         p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
1814                              pmgmt->u.assoc_resp.variable, pielen);
1815         if (p && p[1])
1816                 HT_caps_handler23a(padapter, p);
1817
1818         p = cfg80211_find_ie(WLAN_EID_HT_OPERATION,
1819                              pmgmt->u.assoc_resp.variable, pielen);
1820         if (p && p[1])
1821                 HT_info_handler23a(padapter, p);
1822
1823         p = cfg80211_find_ie(WLAN_EID_ERP_INFO,
1824                              pmgmt->u.assoc_resp.variable, pielen);
1825         if (p && p[1])
1826                 ERP_IE_handler23a(padapter, p);
1827
1828         pie = pframe + offsetof(struct ieee80211_mgmt, u.assoc_resp.variable);
1829         while (true) {
1830                 p = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
1831                                             WLAN_OUI_TYPE_MICROSOFT_WMM,
1832                                             pie, pframe + pkt_len - pie);
1833                 if (!p)
1834                         break;
1835
1836                 pie = p + p[1] + 2;
1837                 /* if this IE is too short, try the next */
1838                 if (p[1] <= 4)
1839                         continue;
1840                 /* if this IE is WMM params, we found what we wanted */
1841                 if (p[6] == 1)
1842                         break;
1843         }
1844
1845         if (p && p[1])
1846                 WMM_param_handler23a(padapter, p);
1847
1848         pmlmeinfo->state &= ~WIFI_FW_ASSOC_STATE;
1849         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
1850
1851         /* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
1852         UpdateBrateTbl23a(padapter, pmlmeinfo->network.SupportedRates);
1853
1854 report_assoc_result:
1855         pmlmepriv->assoc_rsp_len = 0;
1856         if (res > 0) {
1857                 kfree(pmlmepriv->assoc_rsp);
1858                 pmlmepriv->assoc_rsp = kmalloc(pkt_len, GFP_ATOMIC);
1859                 if (pmlmepriv->assoc_rsp) {
1860                         memcpy(pmlmepriv->assoc_rsp, pframe, pkt_len);
1861                         pmlmepriv->assoc_rsp_len = pkt_len;
1862                 }
1863         } else
1864                 kfree(pmlmepriv->assoc_rsp);
1865
1866         report_join_res23a(padapter, res);
1867
1868         return _SUCCESS;
1869 }
1870
1871 static int
1872 OnDeAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
1873 {
1874         unsigned short reason;
1875         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1876         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
1877         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
1878         struct sk_buff *skb = precv_frame->pkt;
1879         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
1880
1881         if (!ether_addr_equal(mgmt->bssid,
1882                               get_my_bssid23a(&pmlmeinfo->network)))
1883                 return _SUCCESS;
1884
1885         reason = le16_to_cpu(mgmt->u.deauth.reason_code);
1886
1887         DBG_8723A("%s Reason code(%d)\n", __func__, reason);
1888
1889 #ifdef CONFIG_8723AU_AP_MODE
1890         if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
1891                 struct sta_info *psta;
1892                 struct sta_priv *pstapriv = &padapter->stapriv;
1893
1894                 DBG_8723A_LEVEL(_drv_always_, "ap recv deauth reason code(%d) "
1895                                 "sta:%pM\n", reason, mgmt->sa);
1896
1897                 psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
1898                 if (psta) {
1899                         u8 updated = 0;
1900
1901                         spin_lock_bh(&pstapriv->asoc_list_lock);
1902                         if (!list_empty(&psta->asoc_list)) {
1903                                 list_del_init(&psta->asoc_list);
1904                                 pstapriv->asoc_list_cnt--;
1905                                 updated = ap_free_sta23a(padapter, psta,
1906                                                       false, reason);
1907                         }
1908                         spin_unlock_bh(&pstapriv->asoc_list_lock);
1909
1910                         associated_clients_update23a(padapter, updated);
1911                 }
1912
1913                 return _SUCCESS;
1914         } else
1915 #endif
1916         {
1917                 DBG_8723A_LEVEL(_drv_always_, "sta recv deauth reason code(%d) "
1918                                 "sta:%pM\n", reason, mgmt->bssid);
1919
1920                 receive_disconnect23a(padapter, mgmt->bssid, reason);
1921         }
1922         pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
1923
1924         return _SUCCESS;
1925 }
1926
1927 static int
1928 OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
1929 {
1930         unsigned short reason;
1931         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1932         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
1933         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
1934         struct sk_buff *skb = precv_frame->pkt;
1935         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
1936
1937         if (!ether_addr_equal(mgmt->bssid,
1938                               get_my_bssid23a(&pmlmeinfo->network)))
1939                 return _SUCCESS;
1940
1941         reason = le16_to_cpu(mgmt->u.disassoc.reason_code);
1942
1943         DBG_8723A("%s Reason code(%d)\n", __func__, reason);
1944
1945 #ifdef CONFIG_8723AU_AP_MODE
1946         if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
1947                 struct sta_info *psta;
1948                 struct sta_priv *pstapriv = &padapter->stapriv;
1949
1950                 DBG_8723A_LEVEL(_drv_always_, "ap recv disassoc reason code(%d)"
1951                                 " sta:%pM\n", reason, mgmt->sa);
1952
1953                 psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
1954                 if (psta) {
1955                         u8 updated = 0;
1956
1957                         spin_lock_bh(&pstapriv->asoc_list_lock);
1958                         if (!list_empty(&psta->asoc_list)) {
1959                                 list_del_init(&psta->asoc_list);
1960                                 pstapriv->asoc_list_cnt--;
1961                                 updated = ap_free_sta23a(padapter, psta,
1962                                                          false, reason);
1963                         }
1964                         spin_unlock_bh(&pstapriv->asoc_list_lock);
1965
1966                         associated_clients_update23a(padapter, updated);
1967                 }
1968
1969                 return _SUCCESS;
1970         } else
1971 #endif
1972         {
1973                 DBG_8723A_LEVEL(_drv_always_, "ap recv disassoc reason "
1974                                 "code(%d) sta:%pM\n", reason, mgmt->bssid);
1975
1976                 receive_disconnect23a(padapter, mgmt->bssid, reason);
1977         }
1978         pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
1979         return _SUCCESS;
1980 }
1981
1982 static int
1983 OnAtim23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
1984 {
1985         DBG_8723A("%s\n", __func__);
1986         return _SUCCESS;
1987 }
1988
1989 static int
1990 on_action_spct23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
1991 {
1992         return _FAIL;
1993 }
1994
1995 static int
1996 OnAction23a_qos(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
1997 {
1998         return _SUCCESS;
1999 }
2000
2001 static int
2002 OnAction23a_dls(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
2003 {
2004         return _SUCCESS;
2005 }
2006
2007 static int OnAction23a_back23a(struct rtw_adapter *padapter,
2008                                struct recv_frame *precv_frame)
2009 {
2010         u8 *addr;
2011         struct sta_info *psta = NULL;
2012         struct recv_reorder_ctrl *preorder_ctrl;
2013         unsigned char category, action;
2014         unsigned short tid, status, capab, params, reason_code = 0;
2015         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2016         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
2017         struct sk_buff *skb = precv_frame->pkt;
2018         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
2019         struct sta_priv *pstapriv = &padapter->stapriv;
2020
2021         /* check RA matches or not */
2022         if (!ether_addr_equal(myid(&padapter->eeprompriv), mgmt->da))
2023                 return _SUCCESS;
2024
2025         DBG_8723A("%s\n", __func__);
2026
2027         if ((pmlmeinfo->state&0x03) != MSR_AP)
2028                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
2029                         return _SUCCESS;
2030
2031         addr = mgmt->sa;
2032         psta = rtw_get_stainfo23a(pstapriv, addr);
2033
2034         if (!psta)
2035                 return _SUCCESS;
2036
2037         category = mgmt->u.action.category;
2038         if (category == WLAN_CATEGORY_BACK) { /*  representing Block Ack */
2039                 if (!pmlmeinfo->HT_enable)
2040                         return _SUCCESS;
2041                 /* action_code is located in the same place for all
2042                    action events, so pick any */
2043                 action = mgmt->u.action.u.wme_action.action_code;
2044                 DBG_8723A("%s, action =%d\n", __func__, action);
2045                 switch (action) {
2046                 case WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
2047                         memcpy(&pmlmeinfo->ADDBA_req,
2048                                &mgmt->u.action.u.addba_req.dialog_token,
2049                                sizeof(struct ADDBA_request));
2050                         process_addba_req23a(padapter,
2051                                              (u8 *)&pmlmeinfo->ADDBA_req, addr);
2052                         if (pmlmeinfo->bAcceptAddbaReq == true)
2053                                 issue_action_BA23a(padapter, addr,
2054                                                    WLAN_ACTION_ADDBA_RESP, 0);
2055                         else {
2056                                 /* reject ADDBA Req */
2057                                 issue_action_BA23a(padapter, addr,
2058                                                    WLAN_ACTION_ADDBA_RESP, 37);
2059                         }
2060                         break;
2061                 case WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
2062                         status = get_unaligned_le16(
2063                                 &mgmt->u.action.u.addba_resp.status);
2064                         capab = get_unaligned_le16(
2065                                 &mgmt->u.action.u.addba_resp.capab);
2066                         tid = (capab & IEEE80211_ADDBA_PARAM_TID_MASK) >> 2;
2067                         if (status == 0) {      /* successful */
2068                                 DBG_8723A("agg_enable for TID =%d\n", tid);
2069                                 psta->htpriv.agg_enable_bitmap |= BIT(tid);
2070                                 psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
2071                         } else
2072                                 psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
2073                         break;
2074
2075                 case WLAN_ACTION_DELBA: /* DELBA */
2076                         params = get_unaligned_le16(
2077                                 &mgmt->u.action.u.delba.params);
2078                         tid = params >> 12;
2079
2080                         if (params & IEEE80211_DELBA_PARAM_INITIATOR_MASK) {
2081                                 preorder_ctrl = &psta->recvreorder_ctrl[tid];
2082                                 preorder_ctrl->enable = false;
2083                                 preorder_ctrl->indicate_seq = 0xffff;
2084                         } else {
2085                                 psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
2086                                 psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
2087                         }
2088                         reason_code = get_unaligned_le16(
2089                                 &mgmt->u.action.u.delba.reason_code);
2090                         /* todo: how to notify the host while receiving
2091                            DELETE BA */
2092                         break;
2093                 default:
2094                         break;
2095                 }
2096         }
2097         return _SUCCESS;
2098 }
2099
2100 static int on_action_public23a(struct rtw_adapter *padapter,
2101                                struct recv_frame *precv_frame)
2102 {
2103         struct sk_buff *skb = precv_frame->pkt;
2104         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
2105         u8 *pframe = skb->data;
2106         int freq, channel;
2107
2108         /* check RA matches or not */
2109         if (!ether_addr_equal(myid(&padapter->eeprompriv), hdr->addr1))
2110                 return _FAIL;
2111
2112         channel = rtw_get_oper_ch23a(padapter);
2113
2114         if (channel <= RTW_CH_MAX_2G_CHANNEL)
2115                 freq = ieee80211_channel_to_frequency(channel,
2116                                                       NL80211_BAND_2GHZ);
2117         else
2118                 freq = ieee80211_channel_to_frequency(channel,
2119                                                       NL80211_BAND_5GHZ);
2120
2121         if (cfg80211_rx_mgmt(padapter->rtw_wdev, freq, 0, pframe,
2122                              skb->len, 0))
2123                 return _SUCCESS;
2124
2125         return _FAIL;
2126 }
2127
2128 static int
2129 OnAction23a_ht(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
2130 {
2131         return _SUCCESS;
2132 }
2133
2134 static int
2135 OnAction23a_wmm(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
2136 {
2137         return _SUCCESS;
2138 }
2139
2140 static int
2141 OnAction23a_p2p(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
2142 {
2143         return _SUCCESS;
2144 }
2145
2146 static int
2147 OnAction23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
2148 {
2149         int i;
2150         u8 category;
2151         struct action_handler *ptable;
2152         struct sk_buff *skb = precv_frame->pkt;
2153         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
2154
2155         category = mgmt->u.action.category;
2156
2157         for (i = 0; i < ARRAY_SIZE(OnAction23a_tbl); i++) {
2158                 ptable = &OnAction23a_tbl[i];
2159
2160                 if (category == ptable->num)
2161                         ptable->func(padapter, precv_frame);
2162         }
2163
2164         return _SUCCESS;
2165 }
2166
2167 static int DoReserved23a(struct rtw_adapter *padapter,
2168                          struct recv_frame *precv_frame)
2169 {
2170         return _SUCCESS;
2171 }
2172
2173 struct xmit_frame *alloc_mgtxmitframe23a(struct xmit_priv *pxmitpriv)
2174 {
2175         struct xmit_frame *pmgntframe;
2176         struct xmit_buf *pxmitbuf;
2177
2178         pmgntframe = rtw_alloc_xmitframe23a_ext(pxmitpriv);
2179
2180         if (!pmgntframe) {
2181                 DBG_8723A("%s(%s): alloc xmitframe fail\n", __func__,
2182                           pxmitpriv->adapter->pnetdev->name);
2183                 goto exit;
2184         }
2185
2186         pxmitbuf = rtw_alloc_xmitbuf23a_ext(pxmitpriv);
2187         if (!pxmitbuf) {
2188                 DBG_8723A("%s(%s): alloc xmitbuf fail\n", __func__,
2189                           pxmitpriv->adapter->pnetdev->name);
2190                 rtw_free_xmitframe23a(pxmitpriv, pmgntframe);
2191                 pmgntframe = NULL;
2192                 goto exit;
2193         }
2194
2195         pmgntframe->frame_tag = MGNT_FRAMETAG;
2196         pmgntframe->pxmitbuf = pxmitbuf;
2197         pmgntframe->buf_addr = pxmitbuf->pbuf;
2198         pxmitbuf->priv_data = pmgntframe;
2199
2200 exit:
2201         return pmgntframe;
2202 }
2203
2204 /****************************************************************************
2205
2206 Following are some TX functions for WiFi MLME
2207
2208 *****************************************************************************/
2209
2210 void update_mgnt_tx_rate23a(struct rtw_adapter *padapter, u8 rate)
2211 {
2212         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2213
2214         pmlmeext->tx_rate = rate;
2215         DBG_8723A("%s(): rate = %x\n", __func__, rate);
2216 }
2217
2218 void update_mgntframe_attrib23a(struct rtw_adapter *padapter,
2219                                 struct pkt_attrib *pattrib)
2220 {
2221         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2222
2223         memset((u8 *)pattrib, 0, sizeof(struct pkt_attrib));
2224
2225         pattrib->hdrlen = 24;
2226         pattrib->nr_frags = 1;
2227         pattrib->priority = 7;
2228         pattrib->mac_id = 0;
2229         pattrib->qsel = 0x12;
2230
2231         pattrib->pktlen = 0;
2232
2233         if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
2234                 pattrib->raid = 6;/* b mode */
2235         else
2236                 pattrib->raid = 5;/* a/g mode */
2237
2238         pattrib->encrypt = 0;
2239         pattrib->bswenc = false;
2240
2241         pattrib->qos_en = false;
2242         pattrib->ht_en = false;
2243         pattrib->bwmode = HT_CHANNEL_WIDTH_20;
2244         pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
2245         pattrib->sgi = false;
2246
2247         pattrib->seqnum = pmlmeext->mgnt_seq;
2248
2249         pattrib->retry_ctrl = true;
2250 }
2251
2252 void dump_mgntframe23a(struct rtw_adapter *padapter,
2253                        struct xmit_frame *pmgntframe)
2254 {
2255         if (padapter->bSurpriseRemoved == true ||
2256             padapter->bDriverStopped == true)
2257                 return;
2258
2259         rtl8723au_mgnt_xmit(padapter, pmgntframe);
2260 }
2261
2262 int dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
2263                                struct xmit_frame *pmgntframe, int timeout_ms)
2264 {
2265         int ret = _FAIL;
2266         unsigned long irqL;
2267         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2268         struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
2269         struct submit_ctx sctx;
2270
2271         if (padapter->bSurpriseRemoved == true ||
2272             padapter->bDriverStopped == true)
2273                 return ret;
2274
2275         rtw_sctx_init23a(&sctx, timeout_ms);
2276         pxmitbuf->sctx = &sctx;
2277
2278         ret = rtl8723au_mgnt_xmit(padapter, pmgntframe);
2279
2280         if (ret == _SUCCESS)
2281                 ret = rtw_sctx_wait23a(&sctx);
2282
2283         spin_lock_irqsave(&pxmitpriv->lock_sctx, irqL);
2284         pxmitbuf->sctx = NULL;
2285         spin_unlock_irqrestore(&pxmitpriv->lock_sctx, irqL);
2286
2287         return ret;
2288 }
2289
2290 int dump_mgntframe23a_and_wait_ack23a(struct rtw_adapter *padapter,
2291                                       struct xmit_frame *pmgntframe)
2292 {
2293         int ret = _FAIL;
2294         u32 timeout_ms = 500;/*   500ms */
2295         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2296
2297         if (padapter->bSurpriseRemoved == true ||
2298             padapter->bDriverStopped == true)
2299                 return _FAIL;
2300
2301         mutex_lock(&pxmitpriv->ack_tx_mutex);
2302         pxmitpriv->ack_tx = true;
2303
2304         pmgntframe->ack_report = 1;
2305         if (rtl8723au_mgnt_xmit(padapter, pmgntframe) == _SUCCESS)
2306                 ret = rtw_ack_tx_wait23a(pxmitpriv, timeout_ms);
2307
2308         pxmitpriv->ack_tx = false;
2309         mutex_unlock(&pxmitpriv->ack_tx_mutex);
2310
2311         return ret;
2312 }
2313
2314 static int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
2315 {
2316         u8 *ssid_ie;
2317         int ssid_len_ori;
2318         int len_diff = 0;
2319         u8 *next_ie;
2320         u32 remain_len;
2321
2322         ssid_ie = rtw_get_ie23a(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
2323
2324         /* DBG_8723A("%s hidden_ssid_mode:%u, ssid_ie:%p, ssid_len_ori:%d\n",
2325            __func__, hidden_ssid_mode, ssid_ie, ssid_len_ori); */
2326
2327         if (ssid_ie && ssid_len_ori > 0) {
2328                 switch (hidden_ssid_mode) {
2329                 case 1:
2330                         next_ie = ssid_ie + 2 + ssid_len_ori;
2331                         remain_len = ies_len -(next_ie-ies);
2332
2333                         ssid_ie[1] = 0;
2334                         memcpy(ssid_ie+2, next_ie, remain_len);
2335                         len_diff -= ssid_len_ori;
2336
2337                         break;
2338                 case 2:
2339                         memset(&ssid_ie[2], 0, ssid_len_ori);
2340                         break;
2341                 default:
2342                         break;
2343                 }
2344         }
2345
2346         return len_diff;
2347 }
2348
2349 void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
2350 {
2351         struct xmit_frame *pmgntframe;
2352         struct pkt_attrib *pattrib;
2353         unsigned char *pframe;
2354         struct ieee80211_mgmt *mgmt;
2355         unsigned int rate_len;
2356         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2357         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2358         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2359         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
2360         struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
2361         u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
2362         const u8 *wps_ie;
2363         u8 sr = 0;
2364         int len_diff;
2365
2366         /* DBG_8723A("%s\n", __func__); */
2367
2368         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
2369         if (!pmgntframe) {
2370                 DBG_8723A("%s, alloc mgnt frame fail\n", __func__);
2371                 return;
2372         }
2373 #ifdef CONFIG_8723AU_AP_MODE
2374         spin_lock_bh(&pmlmepriv->bcn_update_lock);
2375 #endif
2376
2377         /* update attribute */
2378         pattrib = &pmgntframe->attrib;
2379         update_mgntframe_attrib23a(padapter, pattrib);
2380         pattrib->qsel = 0x10;
2381
2382         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2383
2384         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2385         mgmt = (struct ieee80211_mgmt *)pframe;
2386
2387         mgmt->frame_control =
2388                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_BEACON);
2389         mgmt->seq_ctrl = 0;
2390
2391         ether_addr_copy(mgmt->da, bc_addr);
2392         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
2393         ether_addr_copy(mgmt->bssid, get_my_bssid23a(cur_network));
2394
2395         /* timestamp will be inserted by hardware */
2396
2397         put_unaligned_le16(cur_network->beacon_interval,
2398                            &mgmt->u.beacon.beacon_int);
2399
2400         put_unaligned_le16(cur_network->capability,
2401                            &mgmt->u.beacon.capab_info);
2402
2403         pframe = mgmt->u.beacon.variable;
2404         pattrib->pktlen = offsetof(struct ieee80211_mgmt, u.beacon.variable);
2405
2406         if ((pmlmeinfo->state & 0x03) == MSR_AP) {
2407                 u8 *iebuf;
2408                 int buflen;
2409                 /* DBG_8723A("ie len =%d\n", cur_network->IELength); */
2410                 memcpy(pframe, cur_network->IEs, cur_network->IELength);
2411                 len_diff = update_hidden_ssid(pframe, cur_network->IELength,
2412                                               pmlmeinfo->hidden_ssid_mode);
2413                 pframe += (cur_network->IELength+len_diff);
2414                 pattrib->pktlen += (cur_network->IELength+len_diff);
2415
2416                 iebuf = mgmt->u.beacon.variable;
2417                 buflen = pattrib->pktlen -
2418                         offsetof(struct ieee80211_mgmt, u.beacon.variable);
2419                 wps_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
2420                                                  WLAN_OUI_TYPE_MICROSOFT_WPS,
2421                                                  iebuf, buflen);
2422
2423                 if (wps_ie && wps_ie[1] > 0) {
2424                         rtw_get_wps_attr_content23a(wps_ie, wps_ie[1],
2425                                                     WPS_ATTR_SELECTED_REGISTRAR,
2426                                                     (u8 *)&sr);
2427                 }
2428                 if (sr != 0)
2429                         set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
2430                 else
2431                         _clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
2432
2433                 goto _issue_bcn;
2434         }
2435
2436         /*  SSID */
2437         pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
2438                                cur_network->Ssid.ssid_len,
2439                                cur_network->Ssid.ssid, &pattrib->pktlen);
2440
2441         /*  supported rates... */
2442         rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates);
2443         pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
2444                                ((rate_len > 8)? 8: rate_len),
2445                                cur_network->SupportedRates, &pattrib->pktlen);
2446
2447         /*  DS parameter set */
2448         pframe = rtw_set_ie23a(pframe, WLAN_EID_DS_PARAMS, 1, (unsigned char *)
2449                                &cur_network->DSConfig, &pattrib->pktlen);
2450
2451         /* if ((pmlmeinfo->state&0x03) == MSR_ADHOC) */
2452         {
2453                 u8 erpinfo = 0;
2454                 u32 ATIMWindow;
2455                 /*  IBSS Parameter Set... */
2456                 /* ATIMWindow = cur->ATIMWindow; */
2457                 ATIMWindow = 0;
2458                 pframe = rtw_set_ie23a(pframe, WLAN_EID_IBSS_PARAMS, 2,
2459                                        (unsigned char *)&ATIMWindow,
2460                                        &pattrib->pktlen);
2461
2462                 /* ERP IE */
2463                 pframe = rtw_set_ie23a(pframe, WLAN_EID_ERP_INFO, 1,
2464                                        &erpinfo, &pattrib->pktlen);
2465         }
2466
2467         /*  EXTERNDED SUPPORTED RATE */
2468         if (rate_len > 8)
2469                 pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
2470                                        rate_len - 8,
2471                                        cur_network->SupportedRates + 8,
2472                                        &pattrib->pktlen);
2473
2474         /* todo:HT for adhoc */
2475
2476 _issue_bcn:
2477
2478 #ifdef CONFIG_8723AU_AP_MODE
2479         pmlmepriv->update_bcn = false;
2480
2481         spin_unlock_bh(&pmlmepriv->bcn_update_lock);
2482 #endif
2483
2484         if ((pattrib->pktlen + TXDESC_SIZE) > 512) {
2485                 DBG_8723A("beacon frame too large\n");
2486                 return;
2487         }
2488
2489         pattrib->last_txcmdsz = pattrib->pktlen;
2490
2491         /* DBG_8723A("issue bcn_sz =%d\n", pattrib->last_txcmdsz); */
2492         if (timeout_ms > 0)
2493                 dump_mgntframe23a_and_wait(padapter, pmgntframe, timeout_ms);
2494         else
2495                 dump_mgntframe23a(padapter, pmgntframe);
2496 }
2497
2498 static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da)
2499 {
2500         struct xmit_frame *pmgntframe;
2501         struct pkt_attrib *pattrib;
2502         unsigned char *pframe;
2503         struct ieee80211_mgmt *mgmt;
2504         unsigned char *mac, *bssid;
2505         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2506 #ifdef CONFIG_8723AU_AP_MODE
2507         const u8 *pwps_ie;
2508         u8 *ssid_ie;
2509         int ssid_ielen;
2510         int ssid_ielen_diff;
2511         u8 buf[MAX_IE_SZ];
2512 #endif
2513         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2514         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
2515         struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
2516         unsigned int rate_len;
2517
2518         /* DBG_8723A("%s\n", __func__); */
2519
2520         if (cur_network->IELength > MAX_IE_SZ)
2521                 return;
2522
2523         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
2524         if (!pmgntframe) {
2525                 DBG_8723A("%s, alloc mgnt frame fail\n", __func__);
2526                 return;
2527         }
2528
2529         /* update attribute */
2530         pattrib = &pmgntframe->attrib;
2531         update_mgntframe_attrib23a(padapter, pattrib);
2532
2533         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2534
2535         pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
2536         mgmt = (struct ieee80211_mgmt *)pframe;
2537
2538         mac = myid(&padapter->eeprompriv);
2539         bssid = cur_network->MacAddress;
2540
2541         mgmt->frame_control =
2542                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_RESP);
2543
2544         ether_addr_copy(mgmt->da, da);
2545         ether_addr_copy(mgmt->sa, mac);
2546         ether_addr_copy(mgmt->bssid, bssid);
2547
2548         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
2549         pmlmeext->mgnt_seq++;
2550
2551         pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
2552
2553         /* timestamp will be inserted by hardware */
2554         put_unaligned_le16(cur_network->beacon_interval,
2555                            &mgmt->u.probe_resp.beacon_int);
2556
2557         put_unaligned_le16(cur_network->capability,
2558                            &mgmt->u.probe_resp.capab_info);
2559
2560         pframe = mgmt->u.probe_resp.variable;
2561         pattrib->pktlen =
2562                 offsetof(struct ieee80211_mgmt, u.probe_resp.variable);
2563
2564         /* below for ad-hoc mode */
2565
2566 #ifdef CONFIG_8723AU_AP_MODE
2567         if ((pmlmeinfo->state & 0x03) == MSR_AP) {
2568                 pwps_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
2569                                                   WLAN_OUI_TYPE_MICROSOFT_WPS,
2570                                                   cur_network->IEs,
2571                                                   cur_network->IELength);
2572
2573                 memcpy(pframe, cur_network->IEs, cur_network->IELength);
2574                 pframe += cur_network->IELength;
2575                 pattrib->pktlen += cur_network->IELength;
2576
2577                 /* retrieve SSID IE from cur_network->Ssid */
2578
2579                 ssid_ie = rtw_get_ie23a(mgmt->u.probe_resp.variable,
2580                                         WLAN_EID_SSID, &ssid_ielen,
2581                                         pframe - mgmt->u.probe_resp.variable);
2582
2583                 ssid_ielen_diff = cur_network->Ssid.ssid_len - ssid_ielen;
2584
2585                 if (ssid_ie && cur_network->Ssid.ssid_len) {
2586                         uint remainder_ielen;
2587                         u8 *remainder_ie;
2588
2589                         remainder_ie = ssid_ie + 2;
2590
2591                         remainder_ielen = pframe - remainder_ie;
2592
2593                         DBG_8723A_LEVEL(_drv_warning_, "%s(%s): "
2594                                         "remainder_ielen > MAX_IE_SZ\n",
2595                                         __func__, padapter->pnetdev->name);
2596                         if (remainder_ielen > MAX_IE_SZ)
2597                                 remainder_ielen = MAX_IE_SZ;
2598
2599                         memcpy(buf, remainder_ie, remainder_ielen);
2600                         memcpy(remainder_ie + ssid_ielen_diff, buf,
2601                                remainder_ielen);
2602                         *(ssid_ie + 1) = cur_network->Ssid.ssid_len;
2603                         memcpy(ssid_ie + 2, cur_network->Ssid.ssid,
2604                                cur_network->Ssid.ssid_len);
2605
2606                         pframe += ssid_ielen_diff;
2607                         pattrib->pktlen += ssid_ielen_diff;
2608                 }
2609         } else
2610 #endif
2611         {
2612                 /*  SSID */
2613                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
2614                                        cur_network->Ssid.ssid_len,
2615                                        cur_network->Ssid.ssid,
2616                                        &pattrib->pktlen);
2617
2618                 /*  supported rates... */
2619                 rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates);
2620                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
2621                                        ((rate_len > 8)? 8: rate_len),
2622                                        cur_network->SupportedRates,
2623                                        &pattrib->pktlen);
2624
2625                 /*  DS parameter set */
2626                 pframe = rtw_set_ie23a(pframe, WLAN_EID_DS_PARAMS, 1,
2627                                        (unsigned char *)&cur_network->DSConfig,
2628                                        &pattrib->pktlen);
2629
2630                 if ((pmlmeinfo->state & 0x03) == MSR_ADHOC) {
2631                         u8 erpinfo = 0;
2632                         u32 ATIMWindow;
2633                         /*  IBSS Parameter Set... */
2634                         /* ATIMWindow = cur->ATIMWindow; */
2635                         ATIMWindow = 0;
2636                         pframe = rtw_set_ie23a(pframe, WLAN_EID_IBSS_PARAMS, 2,
2637                                                (unsigned char *)&ATIMWindow,
2638                                                &pattrib->pktlen);
2639
2640                         /* ERP IE */
2641                         pframe = rtw_set_ie23a(pframe, WLAN_EID_ERP_INFO, 1,
2642                                                &erpinfo, &pattrib->pktlen);
2643                 }
2644
2645                 /*  EXTERNDED SUPPORTED RATE */
2646                 if (rate_len > 8)
2647                         pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
2648                                                rate_len - 8,
2649                                                cur_network->SupportedRates + 8,
2650                                                &pattrib->pktlen);
2651
2652                 /* todo:HT for adhoc */
2653         }
2654
2655         pattrib->last_txcmdsz = pattrib->pktlen;
2656
2657         dump_mgntframe23a(padapter, pmgntframe);
2658 }
2659
2660 static int _issue_probereq(struct rtw_adapter *padapter,
2661                            struct cfg80211_ssid *pssid, u8 *da, int wait_ack)
2662 {
2663         int ret = _FAIL;
2664         struct xmit_frame *pmgntframe;
2665         struct pkt_attrib *pattrib;
2666         unsigned char *pframe;
2667         struct ieee80211_hdr *pwlanhdr;
2668         unsigned char *mac;
2669         unsigned char bssrate[NumRates];
2670         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2671         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2672         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2673         int bssrate_len = 0;
2674         u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
2675
2676         RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
2677                  "+%s\n", __func__);
2678
2679         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
2680         if (!pmgntframe)
2681                 goto exit;
2682
2683         /* update attribute */
2684         pattrib = &pmgntframe->attrib;
2685         update_mgntframe_attrib23a(padapter, pattrib);
2686
2687         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2688
2689         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2690         pwlanhdr = (struct ieee80211_hdr *)pframe;
2691
2692         mac = myid(&padapter->eeprompriv);
2693
2694         pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
2695                                               IEEE80211_STYPE_PROBE_REQ);
2696
2697         if (da) {
2698                 /*      unicast probe request frame */
2699                 ether_addr_copy(pwlanhdr->addr1, da);
2700                 ether_addr_copy(pwlanhdr->addr3, da);
2701         } else {
2702                 /*      broadcast probe request frame */
2703                 ether_addr_copy(pwlanhdr->addr1, bc_addr);
2704                 ether_addr_copy(pwlanhdr->addr3, bc_addr);
2705         }
2706
2707         ether_addr_copy(pwlanhdr->addr2, mac);
2708
2709         pwlanhdr->seq_ctrl =
2710                 cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
2711
2712         pmlmeext->mgnt_seq++;
2713
2714         pframe += sizeof (struct ieee80211_hdr_3addr);
2715         pattrib->pktlen = sizeof (struct ieee80211_hdr_3addr);
2716
2717         if (pssid)
2718                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID, pssid->ssid_len,
2719                                        pssid->ssid, &pattrib->pktlen);
2720         else
2721                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID, 0, NULL,
2722                                        &pattrib->pktlen);
2723
2724         get_rate_set23a(padapter, bssrate, &bssrate_len);
2725
2726         if (bssrate_len > 8) {
2727                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
2728                                        bssrate, &pattrib->pktlen);
2729                 pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
2730                                        (bssrate_len - 8), (bssrate + 8),
2731                                        &pattrib->pktlen);
2732         } else {
2733                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
2734                                        bssrate_len, bssrate, &pattrib->pktlen);
2735         }
2736
2737         /* add wps_ie for wps2.0 */
2738         if (pmlmepriv->wps_probe_req_ie_len>0 && pmlmepriv->wps_probe_req_ie) {
2739                 memcpy(pframe, pmlmepriv->wps_probe_req_ie,
2740                        pmlmepriv->wps_probe_req_ie_len);
2741                 pframe += pmlmepriv->wps_probe_req_ie_len;
2742                 pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
2743         }
2744
2745         pattrib->last_txcmdsz = pattrib->pktlen;
2746
2747         RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
2748                  "issuing probe_req, tx_len =%d\n", pattrib->last_txcmdsz);
2749
2750         if (wait_ack) {
2751                 ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
2752         } else {
2753                 dump_mgntframe23a(padapter, pmgntframe);
2754                 ret = _SUCCESS;
2755         }
2756
2757 exit:
2758         return ret;
2759 }
2760
2761 static inline void issue_probereq(struct rtw_adapter *padapter,
2762                                   struct cfg80211_ssid *pssid, u8 *da)
2763 {
2764         _issue_probereq(padapter, pssid, da, false);
2765 }
2766
2767 static int issue_probereq_ex(struct rtw_adapter *padapter,
2768                              struct cfg80211_ssid *pssid, u8 *da,
2769                              int try_cnt, int wait_ms)
2770 {
2771         int ret;
2772         int i = 0;
2773         unsigned long start = jiffies;
2774
2775         do {
2776                 ret = _issue_probereq(padapter, pssid, da,
2777                                       wait_ms > 0 ? true : false);
2778
2779                 i++;
2780
2781                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
2782                         break;
2783
2784                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
2785                         msleep(wait_ms);
2786
2787         } while((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
2788
2789         if (ret != _FAIL) {
2790                 ret = _SUCCESS;
2791                 goto exit;
2792         }
2793
2794         if (try_cnt && wait_ms) {
2795                 if (da)
2796                         DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
2797                                   __func__, padapter->pnetdev->name,
2798                                   da, rtw_get_oper_ch23a(padapter),
2799                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
2800                                   jiffies_to_msecs(jiffies - start));
2801                 else
2802                         DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
2803                                   __func__, padapter->pnetdev->name,
2804                                   rtw_get_oper_ch23a(padapter),
2805                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
2806                                   jiffies_to_msecs(jiffies - start));
2807         }
2808 exit:
2809         return ret;
2810 }
2811
2812 /*  if psta == NULL, indiate we are station(client) now... */
2813 static void issue_auth(struct rtw_adapter *padapter, struct sta_info *psta,
2814                        unsigned short status)
2815 {
2816         struct xmit_frame *pmgntframe;
2817         struct pkt_attrib *pattrib;
2818         unsigned char *pframe;
2819         struct ieee80211_mgmt *mgmt;
2820         unsigned int val32;
2821         u16 auth_algo;
2822         int use_shared_key = 0;
2823         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2824         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2825         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
2826
2827         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
2828         if (!pmgntframe)
2829                 return;
2830
2831         /* update attribute */
2832         pattrib = &pmgntframe->attrib;
2833         update_mgntframe_attrib23a(padapter, pattrib);
2834
2835         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2836
2837         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2838         mgmt = (struct ieee80211_mgmt *)pframe;
2839
2840         mgmt->frame_control =
2841                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_AUTH);
2842         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
2843         pmlmeext->mgnt_seq++;
2844
2845         pattrib->pktlen = offsetof(struct ieee80211_mgmt, u.auth.variable);
2846
2847         if (psta) { /*  for AP mode */
2848 #ifdef CONFIG_8723AU_AP_MODE
2849                 unsigned short val16;
2850
2851                 ether_addr_copy(mgmt->da, psta->hwaddr);
2852                 ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
2853                 ether_addr_copy(mgmt->bssid, myid(&padapter->eeprompriv));
2854
2855                 /*  setting auth algo number */
2856                 val16 = (u16)psta->authalg;
2857
2858                 if (status != WLAN_STATUS_SUCCESS)
2859                         val16 = 0;
2860
2861                 if (val16)
2862                         use_shared_key = 1;
2863
2864                 mgmt->u.auth.auth_alg = cpu_to_le16(val16);
2865
2866                 /*  setting auth seq number */
2867                 mgmt->u.auth.auth_transaction =
2868                         cpu_to_le16((u16)psta->auth_seq);
2869
2870                 /*  setting status code... */
2871                 mgmt->u.auth.status_code = cpu_to_le16(status);
2872
2873                 pframe = mgmt->u.auth.variable;
2874                 /*  added challenging text... */
2875                 if ((psta->auth_seq == 2) &&
2876                     (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1))
2877                         pframe = rtw_set_ie23a(pframe, WLAN_EID_CHALLENGE, 128,
2878                                                psta->chg_txt, &pattrib->pktlen);
2879 #endif
2880         } else {
2881                 struct ieee80211_mgmt *iv_mgmt;
2882
2883                 ether_addr_copy(mgmt->da, get_my_bssid23a(&pmlmeinfo->network));
2884                 ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
2885                 ether_addr_copy(mgmt->bssid,
2886                                 get_my_bssid23a(&pmlmeinfo->network));
2887
2888                 /*  setting auth algo number */
2889                 /*  0:OPEN System, 1:Shared key */
2890                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
2891                         use_shared_key = 1;
2892                         auth_algo = WLAN_AUTH_SHARED_KEY;
2893                 } else
2894                         auth_algo = WLAN_AUTH_OPEN;
2895
2896                 /* DBG_8723A("%s auth_algo = %s auth_seq =%d\n", __func__,
2897                    (pmlmeinfo->auth_algo == 0)?"OPEN":"SHARED",
2898                    pmlmeinfo->auth_seq); */
2899
2900                 /* setting IV for auth seq #3 */
2901                 if ((pmlmeinfo->auth_seq == 3) &&
2902                     (pmlmeinfo->state & WIFI_FW_AUTH_STATE) &&
2903                     (use_shared_key == 1)) {
2904                         u32 *piv = (u32 *)&mgmt->u.auth;
2905
2906                         iv_mgmt = (struct ieee80211_mgmt *)(pframe + 4);
2907                         /* DBG_8723A("==> iv(%d), key_index(%d)\n",
2908                            pmlmeinfo->iv, pmlmeinfo->key_index); */
2909                         val32 = (pmlmeinfo->iv & 0x3fffffff) |
2910                                 (pmlmeinfo->key_index << 30);
2911                         pmlmeinfo->iv++;
2912                         put_unaligned_le32(val32, piv);
2913
2914                         pattrib->pktlen += 4;
2915
2916                         pattrib->iv_len = IEEE80211_WEP_IV_LEN;
2917                 } else
2918                         iv_mgmt = mgmt;
2919
2920                 iv_mgmt->u.auth.auth_alg = cpu_to_le16(auth_algo);
2921
2922                 /*  setting auth seq number */
2923                 iv_mgmt->u.auth.auth_transaction =
2924                         cpu_to_le16(pmlmeinfo->auth_seq);
2925
2926                 /*  setting status code... */
2927                 iv_mgmt->u.auth.status_code = cpu_to_le16(status);
2928
2929                 pframe = iv_mgmt->u.auth.variable;
2930
2931                 /*  then checking to see if sending challenging text... */
2932                 if ((pmlmeinfo->auth_seq == 3) &&
2933                     (pmlmeinfo->state & WIFI_FW_AUTH_STATE) &&
2934                     (use_shared_key == 1)) {
2935                         pframe = rtw_set_ie23a(pframe, WLAN_EID_CHALLENGE, 128,
2936                                                pmlmeinfo->chg_txt,
2937                                                &pattrib->pktlen);
2938
2939                         mgmt->frame_control |=
2940                                 cpu_to_le16(IEEE80211_FCTL_PROTECTED);
2941
2942                         pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
2943
2944                         pattrib->encrypt = WLAN_CIPHER_SUITE_WEP40;
2945
2946                         pattrib->icv_len = IEEE80211_WEP_ICV_LEN;
2947
2948                         pattrib->pktlen += pattrib->icv_len;
2949                 }
2950         }
2951
2952         pattrib->last_txcmdsz = pattrib->pktlen;
2953
2954         rtw_wep_encrypt23a(padapter, pmgntframe);
2955         DBG_8723A("%s\n", __func__);
2956         dump_mgntframe23a(padapter, pmgntframe);
2957 }
2958
2959 #ifdef CONFIG_8723AU_AP_MODE
2960 static void issue_assocrsp(struct rtw_adapter *padapter, unsigned short status,
2961                            struct sta_info *pstat, u16 pkt_type)
2962 {
2963         struct xmit_frame *pmgntframe;
2964         struct ieee80211_mgmt *mgmt;
2965         struct pkt_attrib *pattrib;
2966         unsigned char *pframe;
2967         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2968         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2969         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2970         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
2971         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
2972         const u8 *p;
2973         u8 *ie = pnetwork->IEs;
2974
2975         DBG_8723A("%s\n", __func__);
2976
2977         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
2978         if (!pmgntframe)
2979                 return;
2980
2981         /* update attribute */
2982         pattrib = &pmgntframe->attrib;
2983         update_mgntframe_attrib23a(padapter, pattrib);
2984
2985         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2986
2987         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2988         mgmt = (struct ieee80211_mgmt *)pframe;
2989
2990         mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT | pkt_type);
2991
2992         ether_addr_copy(mgmt->da, pstat->hwaddr);
2993         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
2994         ether_addr_copy(mgmt->bssid, get_my_bssid23a(&pmlmeinfo->network));
2995
2996         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
2997
2998         pmlmeext->mgnt_seq++;
2999
3000         pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
3001         pattrib->pktlen =
3002                 offsetof(struct ieee80211_mgmt, u.assoc_resp.variable);
3003
3004         mgmt->u.assoc_resp.capab_info = cpu_to_le16(pnetwork->capability);
3005         mgmt->u.assoc_resp.status_code = cpu_to_le16(status);
3006         mgmt->u.assoc_resp.aid = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
3007
3008         pframe = mgmt->u.assoc_resp.variable;
3009
3010         if (pstat->bssratelen <= 8) {
3011                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
3012                                        pstat->bssratelen, pstat->bssrateset,
3013                                        &pattrib->pktlen);
3014         } else {
3015                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
3016                                        pstat->bssrateset, &pattrib->pktlen);
3017                 pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
3018                                        pstat->bssratelen - 8,
3019                                        pstat->bssrateset + 8, &pattrib->pktlen);
3020         }
3021
3022         if (pstat->flags & WLAN_STA_HT && pmlmepriv->htpriv.ht_option) {
3023                 /* FILL HT CAP INFO IE */
3024                 /* p = hostapd_eid_ht_capabilities_info(hapd, p); */
3025                 p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, ie,
3026                                      pnetwork->IELength);
3027                 if (p && p[1]) {
3028                         memcpy(pframe, p, p[1] + 2);
3029                         pframe += (p[1] + 2);
3030                         pattrib->pktlen += (p[1] + 2);
3031                 }
3032
3033                 /* FILL HT ADD INFO IE */
3034                 /* p = hostapd_eid_ht_operation(hapd, p); */
3035                 p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, ie,
3036                                      pnetwork->IELength);
3037                 if (p && p[1] > 0) {
3038                         memcpy(pframe, p, p[1] + 2);
3039                         pframe += (p[1] + 2);
3040                         pattrib->pktlen += (p[1] + 2);
3041                 }
3042         }
3043
3044         /* FILL WMM IE */
3045         if (pstat->flags & WLAN_STA_WME && pmlmepriv->qos_option) {
3046                 unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02,
3047                                                0x01, 0x01};
3048                 int ie_len = 0;
3049
3050                 for (p = ie; ; p += (ie_len + 2)) {
3051                         p = cfg80211_find_ie(WLAN_EID_VENDOR_SPECIFIC, p,
3052                                              pnetwork->IELength - (ie_len + 2));
3053                         if (p)
3054                                 ie_len = p[1];
3055                         else
3056                                 ie_len = 0;
3057                         if (p && !memcmp(p + 2, WMM_PARA_IE, 6)) {
3058                                 memcpy(pframe, p, ie_len + 2);
3059                                 pframe += (ie_len + 2);
3060                                 pattrib->pktlen += (ie_len + 2);
3061
3062                                 break;
3063                         }
3064
3065                         if (!p || ie_len == 0)
3066                                 break;
3067                 }
3068         }
3069
3070         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK) {
3071                 pframe = rtw_set_ie23a(pframe, WLAN_EID_VENDOR_SPECIFIC, 6,
3072                                        REALTEK_96B_IE, &pattrib->pktlen);
3073         }
3074
3075         pattrib->last_txcmdsz = pattrib->pktlen;
3076
3077         dump_mgntframe23a(padapter, pmgntframe);
3078 }
3079 #endif
3080
3081 static void issue_assocreq(struct rtw_adapter *padapter)
3082 {
3083         int ret = _FAIL;
3084         struct xmit_frame *pmgntframe;
3085         struct pkt_attrib *pattrib;
3086         unsigned char *pframe;
3087         const u8 *p;
3088         struct ieee80211_mgmt *mgmt;
3089         unsigned int i, j, index = 0;
3090         unsigned char rf_type, bssrate[NumRates], sta_bssrate[NumRates];
3091         struct registry_priv *pregpriv = &padapter->registrypriv;
3092         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3093         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
3094         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3095         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3096         int bssrate_len = 0, sta_bssrate_len = 0, pie_len;
3097         u8 *pie;
3098
3099         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3100         if (!pmgntframe)
3101                 goto exit;
3102
3103         /* update attribute */
3104         pattrib = &pmgntframe->attrib;
3105         update_mgntframe_attrib23a(padapter, pattrib);
3106
3107         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3108
3109         pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
3110         mgmt = (struct ieee80211_mgmt *)pframe;
3111
3112         mgmt->frame_control =
3113                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_ASSOC_REQ);
3114
3115         ether_addr_copy(mgmt->da, get_my_bssid23a(&pmlmeinfo->network));
3116         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
3117         ether_addr_copy(mgmt->bssid, get_my_bssid23a(&pmlmeinfo->network));
3118
3119         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3120         pmlmeext->mgnt_seq++;
3121
3122         /* caps */
3123         put_unaligned_le16(pmlmeinfo->network.capability,
3124                            &mgmt->u.assoc_req.capab_info);
3125         /* todo: listen interval for power saving */
3126         put_unaligned_le16(3, &mgmt->u.assoc_req.listen_interval);
3127
3128         pframe = mgmt->u.assoc_req.variable;
3129         pattrib->pktlen = offsetof(struct ieee80211_mgmt, u.assoc_req.variable);
3130
3131         /* SSID */
3132         pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
3133                                pmlmeinfo->network.Ssid.ssid_len,
3134                                pmlmeinfo->network.Ssid.ssid, &pattrib->pktlen);
3135
3136         /* supported rate & extended supported rate */
3137
3138         get_rate_set23a(padapter, sta_bssrate, &sta_bssrate_len);
3139         /* DBG_8723A("sta_bssrate_len =%d\n", sta_bssrate_len); */
3140
3141         /*  for JAPAN, channel 14 can only uses B Mode(CCK) */
3142         if (pmlmeext->cur_channel == 14)
3143                 sta_bssrate_len = 4;
3144
3145         /* for (i = 0; i < sta_bssrate_len; i++) { */
3146         /*      DBG_8723A("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]); */
3147         /*  */
3148
3149         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
3150                 if (pmlmeinfo->network.SupportedRates[i] == 0)
3151                         break;
3152                 DBG_8723A("network.SupportedRates[%d]=%02X\n", i,
3153                           pmlmeinfo->network.SupportedRates[i]);
3154         }
3155
3156         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
3157                 if (pmlmeinfo->network.SupportedRates[i] == 0)
3158                         break;
3159
3160                 /*  Check if the AP's supported rates are also
3161                     supported by STA. */
3162                 for (j = 0; j < sta_bssrate_len; j++) {
3163                          /*  Avoid the proprietary data rate (22Mbps) of
3164                              Handlink WSG-4000 AP */
3165                         if ((pmlmeinfo->network.SupportedRates[i] |
3166                              IEEE80211_BASIC_RATE_MASK) ==
3167                             (sta_bssrate[j] | IEEE80211_BASIC_RATE_MASK)) {
3168                                 /* DBG_8723A("match i = %d, j =%d\n", i, j); */
3169                                 break;
3170                         }
3171                 }
3172
3173                 if (j == sta_bssrate_len) {
3174                         /*  the rate is not supported by STA */
3175                         DBG_8723A("%s(): the rate[%d]=%02X is not supported by "
3176                                   "STA!\n", __func__, i,
3177                                   pmlmeinfo->network.SupportedRates[i]);
3178                 } else {
3179                         /*  the rate is supported by STA */
3180                         bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
3181                 }
3182         }
3183
3184         bssrate_len = index;
3185         DBG_8723A("bssrate_len = %d\n", bssrate_len);
3186
3187         if (bssrate_len == 0) {
3188                 rtw_free_xmitbuf23a(pxmitpriv, pmgntframe->pxmitbuf);
3189                 rtw_free_xmitframe23a(pxmitpriv, pmgntframe);
3190                 goto exit; /* don't connect to AP if no joint supported rate */
3191         }
3192
3193         if (bssrate_len > 8) {
3194                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
3195                                        bssrate, &pattrib->pktlen);
3196                 pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
3197                                        (bssrate_len - 8), (bssrate + 8),
3198                                        &pattrib->pktlen);
3199         } else
3200                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
3201                                        bssrate_len, bssrate, &pattrib->pktlen);
3202
3203         /* RSN */
3204
3205         pie = pmlmeinfo->network.IEs;
3206         pie_len = pmlmeinfo->network.IELength;
3207
3208         p = cfg80211_find_ie(WLAN_EID_RSN, pie, pie_len);
3209         if (p)
3210                 pframe = rtw_set_ie23a(pframe, WLAN_EID_RSN, p[1], p + 2,
3211                                        &pattrib->pktlen);
3212
3213         /* HT caps */
3214         if (padapter->mlmepriv.htpriv.ht_option) {
3215                 p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, pie, pie_len);
3216
3217                 if (p && !is_ap_in_tkip23a(padapter)) {
3218                         struct ieee80211_ht_cap *cap = &pmlmeinfo->ht_cap;
3219
3220                         memcpy(cap, p + 2, sizeof(struct ieee80211_ht_cap));
3221
3222                         /* to disable 40M Hz support while gd_bw_40MHz_en = 0 */
3223                         if (pregpriv->cbw40_enable == 0) {
3224                                 cap->cap_info &= ~cpu_to_le16(
3225                                         IEEE80211_HT_CAP_SGI_40 |
3226                                         IEEE80211_HT_CAP_SUP_WIDTH_20_40);
3227                         } else {
3228                                 cap->cap_info |= cpu_to_le16(
3229                                         IEEE80211_HT_CAP_SUP_WIDTH_20_40);
3230                         }
3231
3232                         /* todo: disable SM power save mode */
3233                         cap->cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SM_PS);
3234
3235                         rf_type = rtl8723a_get_rf_type(padapter);
3236                         /* switch (pregpriv->rf_config) */
3237                         switch (rf_type) {
3238                         case RF_1T1R:
3239                                 /* RX STBC One spatial stream */
3240                                 if (pregpriv->rx_stbc)
3241                                         cap->cap_info |= cpu_to_le16(1 << IEEE80211_HT_CAP_RX_STBC_SHIFT);
3242
3243                                 memcpy(&cap->mcs, MCS_rate_1R23A, 16);
3244                                 break;
3245
3246                         case RF_2T2R:
3247                         case RF_1T2R:
3248                         default:
3249                                 /* enable for 2.4/5 GHz */
3250                                 if (pregpriv->rx_stbc == 0x3 ||
3251                                     (pmlmeext->cur_wireless_mode &
3252                                      WIRELESS_11_24N &&
3253                                      /* enable for 2.4GHz */
3254                                      pregpriv->rx_stbc == 0x1) ||
3255                                     (pmlmeext->cur_wireless_mode &
3256                                      WIRELESS_11_5N &&
3257                                      pregpriv->rx_stbc == 0x2) ||
3258                                     /* enable for 5GHz */
3259                                     pregpriv->wifi_spec == 1) {
3260                                         DBG_8723A("declare supporting RX "
3261                                                   "STBC\n");
3262                                         /* RX STBC two spatial stream */
3263                                         cap->cap_info |= cpu_to_le16(2 << IEEE80211_HT_CAP_RX_STBC_SHIFT);
3264                                 }
3265                                 memcpy(&cap->mcs, MCS_rate_2R23A, 16);
3266                                 break;
3267                         }
3268
3269                         if (rtl8723a_BT_coexist(padapter) &&
3270                             rtl8723a_BT_using_antenna_1(padapter)) {
3271                                 /*  set to 8K */
3272                                 cap->ampdu_params_info &=
3273                                         ~IEEE80211_HT_AMPDU_PARM_FACTOR;
3274 /*                              cap->ampdu_params_info |= MAX_AMPDU_FACTOR_8K */
3275                         }
3276
3277                         pframe = rtw_set_ie23a(pframe, WLAN_EID_HT_CAPABILITY,
3278                                                p[1], (u8 *)&pmlmeinfo->ht_cap,
3279                                                &pattrib->pktlen);
3280                 }
3281         }
3282
3283         /* vendor specific IE, such as WPA, WMM, WPS */
3284         for (i = 0;  i < pmlmeinfo->network.IELength;) {
3285                 p = pmlmeinfo->network.IEs + i;
3286
3287                 switch (p[0]) {
3288                 case WLAN_EID_VENDOR_SPECIFIC:
3289                         if (!memcmp(p + 2, RTW_WPA_OUI23A_TYPE, 4) ||
3290                             !memcmp(p + 2, WMM_OUI23A, 4) ||
3291                             !memcmp(p + 2, WPS_OUI23A, 4)) {
3292                                 u8 plen = p[1];
3293
3294                                 if (!padapter->registrypriv.wifi_spec) {
3295                                         /* Commented by Kurt 20110629 */
3296                                         /* In some older APs, WPS handshake */
3297                                         /* would be fail if we append vender
3298                                            extensions informations to AP */
3299                                         if (!memcmp(p + 2, WPS_OUI23A, 4))
3300                                                 plen = 14;
3301                                 }
3302                                 pframe = rtw_set_ie23a(pframe,
3303                                                        WLAN_EID_VENDOR_SPECIFIC,
3304                                                        plen, p + 2,
3305                                                        &pattrib->pktlen);
3306                         }
3307                         break;
3308
3309                 default:
3310                         break;
3311                 }
3312
3313                 i += p[1] + 2;
3314         }
3315
3316         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
3317                 pframe = rtw_set_ie23a(pframe, WLAN_EID_VENDOR_SPECIFIC, 6,
3318                                        REALTEK_96B_IE, &pattrib->pktlen);
3319
3320         pattrib->last_txcmdsz = pattrib->pktlen;
3321         dump_mgntframe23a(padapter, pmgntframe);
3322
3323         ret = _SUCCESS;
3324
3325 exit:
3326         pmlmepriv->assoc_req_len = 0;
3327         if (ret == _SUCCESS) {
3328                 kfree(pmlmepriv->assoc_req);
3329                 pmlmepriv->assoc_req = kmalloc(pattrib->pktlen, GFP_ATOMIC);
3330                 if (pmlmepriv->assoc_req) {
3331                         memcpy(pmlmepriv->assoc_req, mgmt, pattrib->pktlen);
3332                         pmlmepriv->assoc_req_len = pattrib->pktlen;
3333                 }
3334         } else
3335                 kfree(pmlmepriv->assoc_req);
3336 }
3337
3338 /* when wait_ack is true, this function should be called at process context */
3339 static int _issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
3340                               unsigned int power_mode, int wait_ack)
3341 {
3342         int ret = _FAIL;
3343         struct xmit_frame *pmgntframe;
3344         struct pkt_attrib *pattrib;
3345         unsigned char *pframe;
3346         struct ieee80211_hdr *pwlanhdr;
3347         struct xmit_priv *pxmitpriv;
3348         struct mlme_ext_priv *pmlmeext;
3349         struct mlme_ext_info *pmlmeinfo;
3350
3351         /* DBG_8723A("%s:%d\n", __func__, power_mode); */
3352
3353         if (!padapter)
3354                 goto exit;
3355
3356         pxmitpriv = &padapter->xmitpriv;
3357         pmlmeext = &padapter->mlmeextpriv;
3358         pmlmeinfo = &pmlmeext->mlmext_info;
3359
3360         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3361         if (!pmgntframe)
3362                 goto exit;
3363
3364         /* update attribute */
3365         pattrib = &pmgntframe->attrib;
3366         update_mgntframe_attrib23a(padapter, pattrib);
3367         pattrib->retry_ctrl = false;
3368
3369         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3370
3371         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3372         pwlanhdr = (struct ieee80211_hdr *)pframe;
3373
3374         pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
3375                                               IEEE80211_STYPE_NULLFUNC);
3376
3377         if ((pmlmeinfo->state&0x03) == MSR_AP)
3378                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
3379         else if ((pmlmeinfo->state&0x03) == MSR_INFRA)
3380                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_TODS);
3381
3382         if (power_mode)
3383                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
3384
3385         ether_addr_copy(pwlanhdr->addr1, da);
3386         ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
3387         ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
3388
3389         pwlanhdr->seq_ctrl =
3390                 cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3391         pmlmeext->mgnt_seq++;
3392
3393         pframe += sizeof(struct ieee80211_hdr_3addr);
3394         pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
3395
3396         pattrib->last_txcmdsz = pattrib->pktlen;
3397
3398         if (wait_ack)
3399                 ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
3400         else {
3401                 dump_mgntframe23a(padapter, pmgntframe);
3402                 ret = _SUCCESS;
3403         }
3404
3405 exit:
3406         return ret;
3407 }
3408
3409 /* when wait_ms >0 , this function should be called at process context */
3410 /* da == NULL for station mode */
3411 int issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
3412                       unsigned int power_mode, int try_cnt, int wait_ms)
3413 {
3414         int ret;
3415         int i = 0;
3416         unsigned long start = jiffies;
3417         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3418         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3419
3420         /* da == NULL, assume it's null data for sta to ap*/
3421         if (da == NULL)
3422                 da = get_my_bssid23a(&pmlmeinfo->network);
3423
3424         do {
3425                 ret = _issue_nulldata23a(padapter, da, power_mode,
3426                                          wait_ms > 0 ? true : false);
3427
3428                 i++;
3429
3430                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
3431                         break;
3432
3433                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
3434                         msleep(wait_ms);
3435
3436         } while((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
3437
3438         if (ret != _FAIL) {
3439                 ret = _SUCCESS;
3440                 goto exit;
3441         }
3442
3443         if (try_cnt && wait_ms) {
3444                 if (da)
3445                         DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
3446                                   __func__, padapter->pnetdev->name,
3447                                   da, rtw_get_oper_ch23a(padapter),
3448                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3449                                   jiffies_to_msecs(jiffies - start));
3450                 else
3451                         DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
3452                                   __func__, padapter->pnetdev->name,
3453                                   rtw_get_oper_ch23a(padapter),
3454                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3455                                   jiffies_to_msecs(jiffies - start));
3456         }
3457 exit:
3458         return ret;
3459 }
3460
3461 /* when wait_ack is true, this function should be called at process context */
3462 static int _issue_qos_nulldata23a(struct rtw_adapter *padapter,
3463                                   unsigned char *da, u16 tid, int wait_ack)
3464 {
3465         int ret = _FAIL;
3466         struct xmit_frame *pmgntframe;
3467         struct pkt_attrib *pattrib;
3468         unsigned char *pframe;
3469         struct ieee80211_qos_hdr *pwlanhdr;
3470         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3471         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3472         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3473
3474         DBG_8723A("%s\n", __func__);
3475
3476         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3477         if (!pmgntframe)
3478                 goto exit;
3479
3480         /* update attribute */
3481         pattrib = &pmgntframe->attrib;
3482         update_mgntframe_attrib23a(padapter, pattrib);
3483
3484         pattrib->hdrlen += 2;
3485         pattrib->qos_en = true;
3486         pattrib->eosp = 1;
3487         pattrib->ack_policy = 0;
3488         pattrib->mdata = 0;
3489
3490         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3491
3492         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3493         pwlanhdr = (struct ieee80211_qos_hdr *)pframe;
3494
3495         pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
3496                                               IEEE80211_STYPE_QOS_NULLFUNC);
3497
3498         if ((pmlmeinfo->state&0x03) == MSR_AP)
3499                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
3500         else if ((pmlmeinfo->state&0x03) == MSR_INFRA)
3501                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_TODS);
3502
3503         if (pattrib->mdata)
3504                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
3505
3506         pwlanhdr->qos_ctrl = cpu_to_le16(tid & IEEE80211_QOS_CTL_TID_MASK);
3507         pwlanhdr->qos_ctrl |= cpu_to_le16((pattrib->ack_policy << 5) &
3508                                           IEEE80211_QOS_CTL_ACK_POLICY_MASK);
3509         if (pattrib->eosp)
3510                 pwlanhdr->qos_ctrl |= cpu_to_le16(IEEE80211_QOS_CTL_EOSP);
3511
3512         ether_addr_copy(pwlanhdr->addr1, da);
3513         ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
3514         ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
3515
3516         pwlanhdr->seq_ctrl =
3517                 cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3518         pmlmeext->mgnt_seq++;
3519
3520         pframe += sizeof(struct ieee80211_qos_hdr);
3521         pattrib->pktlen = sizeof(struct ieee80211_qos_hdr);
3522
3523         pattrib->last_txcmdsz = pattrib->pktlen;
3524
3525         if (wait_ack)
3526                 ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
3527         else {
3528                 dump_mgntframe23a(padapter, pmgntframe);
3529                 ret = _SUCCESS;
3530         }
3531
3532 exit:
3533         return ret;
3534 }
3535
3536 /* when wait_ms >0 , this function should be called at process context */
3537 /* da == NULL for station mode */
3538 int issue_qos_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
3539                           u16 tid, int try_cnt, int wait_ms)
3540 {
3541         int ret;
3542         int i = 0;
3543         unsigned long start = jiffies;
3544         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3545         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3546
3547         /* da == NULL, assume it's null data for sta to ap*/
3548         if (da == NULL)
3549                 da = get_my_bssid23a(&pmlmeinfo->network);
3550
3551         do {
3552                 ret = _issue_qos_nulldata23a(padapter, da, tid,
3553                                              wait_ms > 0 ? true : false);
3554
3555                 i++;
3556
3557                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
3558                         break;
3559
3560                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
3561                         msleep(wait_ms);
3562         } while((i < try_cnt) && ((ret == _FAIL)||(wait_ms == 0)));
3563
3564         if (ret != _FAIL) {
3565                 ret = _SUCCESS;
3566                 goto exit;
3567         }
3568
3569         if (try_cnt && wait_ms) {
3570                 if (da)
3571                         DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
3572                                   __func__, padapter->pnetdev->name,
3573                                   da, rtw_get_oper_ch23a(padapter),
3574                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3575                                   jiffies_to_msecs(jiffies - start));
3576                 else
3577                         DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
3578                                   __func__, padapter->pnetdev->name,
3579                                   rtw_get_oper_ch23a(padapter),
3580                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3581                                   jiffies_to_msecs(jiffies - start));
3582         }
3583 exit:
3584         return ret;
3585 }
3586
3587 static int _issue_deauth(struct rtw_adapter *padapter, unsigned char *da,
3588                          unsigned short reason, u8 wait_ack)
3589 {
3590         struct xmit_frame *pmgntframe;
3591         struct pkt_attrib *pattrib;
3592         struct ieee80211_mgmt *mgmt;
3593         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3594         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3595         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3596         int ret = _FAIL;
3597
3598         /* DBG_8723A("%s to %pM\n", __func__, da); */
3599
3600         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3601         if (!pmgntframe)
3602                 goto exit;
3603
3604         /* update attribute */
3605         pattrib = &pmgntframe->attrib;
3606         update_mgntframe_attrib23a(padapter, pattrib);
3607         pattrib->retry_ctrl = false;
3608
3609         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3610
3611         mgmt = (struct ieee80211_mgmt *)(pmgntframe->buf_addr + TXDESC_OFFSET);
3612
3613         mgmt->frame_control =
3614                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_DEAUTH);
3615
3616         ether_addr_copy(mgmt->da, da);
3617         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
3618         ether_addr_copy(mgmt->bssid, get_my_bssid23a(&pmlmeinfo->network));
3619
3620         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3621         pmlmeext->mgnt_seq++;
3622
3623         pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr) + 2;
3624
3625         mgmt->u.deauth.reason_code = cpu_to_le16(reason);
3626
3627         pattrib->last_txcmdsz = pattrib->pktlen;
3628
3629         if (wait_ack)
3630                 ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
3631         else {
3632                 dump_mgntframe23a(padapter, pmgntframe);
3633                 ret = _SUCCESS;
3634         }
3635
3636 exit:
3637         return ret;
3638 }
3639
3640 int issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da,
3641                     unsigned short reason)
3642 {
3643         DBG_8723A("%s to %pM\n", __func__, da);
3644         return _issue_deauth(padapter, da, reason, false);
3645 }
3646
3647 static int issue_deauth_ex(struct rtw_adapter *padapter, u8 *da,
3648                            unsigned short reason, int try_cnt, int wait_ms)
3649 {
3650         int ret;
3651         int i = 0;
3652         unsigned long start = jiffies;
3653
3654         do {
3655                 ret = _issue_deauth(padapter, da, reason,
3656                                     wait_ms >0 ? true : false);
3657
3658                 i++;
3659
3660                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
3661                         break;
3662
3663                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
3664                         msleep(wait_ms);
3665
3666         } while((i < try_cnt) && ((ret == _FAIL)||(wait_ms == 0)));
3667
3668         if (ret != _FAIL) {
3669                 ret = _SUCCESS;
3670                 goto exit;
3671         }
3672
3673         if (try_cnt && wait_ms) {
3674                 if (da)
3675                         DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
3676                                   __func__, padapter->pnetdev->name,
3677                                   da, rtw_get_oper_ch23a(padapter),
3678                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3679                                   jiffies_to_msecs(jiffies - start));
3680                 else
3681                         DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
3682                                   __func__, padapter->pnetdev->name,
3683                                   rtw_get_oper_ch23a(padapter),
3684                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3685                                   jiffies_to_msecs(jiffies - start));
3686         }
3687 exit:
3688         return ret;
3689 }
3690
3691 void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter,
3692                                     u8 *ra, u8 new_ch, u8 ch_offset)
3693 {
3694         struct xmit_frame *pmgntframe;
3695         struct pkt_attrib *pattrib;
3696         unsigned char *pframe;
3697         struct ieee80211_mgmt *mgmt;
3698         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3699         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3700
3701         DBG_8723A("%s(%s): ra=%pM, ch:%u, offset:%u\n",
3702                   __func__, padapter->pnetdev->name, ra, new_ch, ch_offset);
3703
3704         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3705         if (!pmgntframe)
3706                 return;
3707
3708         /* update attribute */
3709         pattrib = &pmgntframe->attrib;
3710         update_mgntframe_attrib23a(padapter, pattrib);
3711
3712         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3713
3714         mgmt = (struct ieee80211_mgmt *)(pmgntframe->buf_addr + TXDESC_OFFSET);
3715
3716         mgmt->frame_control =
3717                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_ACTION);
3718
3719         ether_addr_copy(mgmt->da, ra); /* RA */
3720         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv)); /* TA */
3721         ether_addr_copy(mgmt->bssid, ra); /* DA = RA */
3722
3723         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3724         pmlmeext->mgnt_seq++;
3725
3726         mgmt->u.action.category = WLAN_CATEGORY_SPECTRUM_MGMT;
3727         mgmt->u.action.u.chan_switch.action_code = WLAN_ACTION_SPCT_CHL_SWITCH;
3728
3729         pframe = mgmt->u.action.u.chan_switch.variable;
3730         pattrib->pktlen = offsetof(struct ieee80211_mgmt,
3731                                    u.action.u.chan_switch.variable);
3732
3733         pframe = rtw_set_ie23a_ch_switch (pframe, &pattrib->pktlen, 0,
3734                                           new_ch, 0);
3735         pframe = rtw_set_ie23a_secondary_ch_offset(pframe, &pattrib->pktlen,
3736                 hal_ch_offset_to_secondary_ch_offset23a(ch_offset));
3737
3738         pattrib->last_txcmdsz = pattrib->pktlen;
3739
3740         dump_mgntframe23a(padapter, pmgntframe);
3741 }
3742
3743 void issue_action_BA23a(struct rtw_adapter *padapter,
3744                         const unsigned char *raddr,
3745                         unsigned char action, unsigned short status)
3746 {
3747         u16 start_seq;
3748         u16 BA_para_set;
3749         u16 BA_starting_seqctrl;
3750         u16 BA_para;
3751         int max_rx_ampdu_factor;
3752         struct xmit_frame *pmgntframe;
3753         struct pkt_attrib *pattrib;
3754         struct ieee80211_mgmt *mgmt;
3755         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3756         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3757         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3758         struct sta_info *psta;
3759         struct sta_priv *pstapriv = &padapter->stapriv;
3760         struct registry_priv *pregpriv = &padapter->registrypriv;
3761         u8 tendaAPMac[] = {0xC8, 0x3A, 0x35};
3762
3763         DBG_8723A("%s, action =%d, status =%d\n", __func__, action, status);
3764
3765         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3766         if (!pmgntframe)
3767                 return;
3768
3769         /* update attribute */
3770         pattrib = &pmgntframe->attrib;
3771         update_mgntframe_attrib23a(padapter, pattrib);
3772
3773         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3774
3775         mgmt = (struct ieee80211_mgmt *)(pmgntframe->buf_addr + TXDESC_OFFSET);
3776
3777         mgmt->frame_control =
3778                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_ACTION);
3779
3780         ether_addr_copy(mgmt->da, raddr);
3781         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
3782         ether_addr_copy(mgmt->bssid, get_my_bssid23a(&pmlmeinfo->network));
3783
3784         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3785         pmlmeext->mgnt_seq++;
3786
3787         mgmt->u.action.category = WLAN_CATEGORY_BACK;
3788
3789         pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr) + 1;
3790
3791         switch (action) {
3792         case WLAN_ACTION_ADDBA_REQ:
3793                 pattrib->pktlen += sizeof(mgmt->u.action.u.addba_req);
3794
3795                 mgmt->u.action.u.addba_req.action_code = action;
3796
3797                 do {
3798                         pmlmeinfo->dialogToken++;
3799                 } while (pmlmeinfo->dialogToken == 0);
3800
3801                 mgmt->u.action.u.addba_req.dialog_token =
3802                         pmlmeinfo->dialogToken;
3803
3804                 if (rtl8723a_BT_coexist(padapter) &&
3805                     rtl8723a_BT_using_antenna_1(padapter) &&
3806                     (pmlmeinfo->assoc_AP_vendor != broadcomAP ||
3807                      memcmp(raddr, tendaAPMac, 3))) {
3808                         /*  A-MSDU NOT Supported */
3809                         BA_para_set = 0;
3810                         /*  immediate Block Ack */
3811                         BA_para_set |= (1 << 1) &
3812                                 IEEE80211_ADDBA_PARAM_POLICY_MASK;
3813                         /*  TID */
3814                         BA_para_set |= (status << 2) &
3815                                 IEEE80211_ADDBA_PARAM_TID_MASK;
3816                         /*  max buffer size is 8 MSDU */
3817                         BA_para_set |= (8 << 6) &
3818                                 IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
3819                 } else {
3820                         /* immediate ack & 64 buffer size */
3821                         BA_para_set = 0x1002 | ((status & 0xf) << 2);
3822                 }
3823
3824                 put_unaligned_le16(BA_para_set,
3825                                    &mgmt->u.action.u.addba_req.capab);
3826
3827                 /*  5ms */
3828                 put_unaligned_le16(5000, &mgmt->u.action.u.addba_req.timeout);
3829
3830                 psta = rtw_get_stainfo23a(pstapriv, raddr);
3831                 if (psta) {
3832                         int idx;
3833
3834                         idx = status & 0x07;
3835                         start_seq =
3836                                 (psta->sta_xmitpriv.txseq_tid[idx] & 0xfff) + 1;
3837
3838                         DBG_8723A("BA_starting_seqctrl = %d for TID =%d\n",
3839                                   start_seq, idx);
3840
3841                         psta->BA_starting_seqctrl[idx] = start_seq;
3842
3843                         BA_starting_seqctrl = start_seq << 4;
3844                 } else
3845                         BA_starting_seqctrl = 0;
3846
3847                 put_unaligned_le16(BA_starting_seqctrl,
3848                                    &mgmt->u.action.u.addba_req.start_seq_num);
3849
3850                 break;
3851
3852         case WLAN_ACTION_ADDBA_RESP:
3853                 pattrib->pktlen += sizeof(mgmt->u.action.u.addba_resp);
3854
3855                 mgmt->u.action.u.addba_resp.action_code = action;
3856                 mgmt->u.action.u.addba_resp.dialog_token =
3857                         pmlmeinfo->ADDBA_req.dialog_token;
3858                 put_unaligned_le16(status,
3859                                    &mgmt->u.action.u.addba_resp.status);
3860
3861                 GetHalDefVar8192CUsb(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR,
3862                                      &max_rx_ampdu_factor);
3863
3864                 BA_para = le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f;
3865                 if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_64K)
3866                         BA_para_set = BA_para | 0x1000; /* 64 buffer size */
3867                 else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_32K)
3868                         BA_para_set = BA_para | 0x0800; /* 32 buffer size */
3869                 else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_16K)
3870                         BA_para_set = BA_para | 0x0400; /* 16 buffer size */
3871                 else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_8K)
3872                         BA_para_set = BA_para | 0x0200; /* 8 buffer size */
3873                 else
3874                         BA_para_set = BA_para | 0x1000; /* 64 buffer size */
3875
3876                 if (rtl8723a_BT_coexist(padapter) &&
3877                     rtl8723a_BT_using_antenna_1(padapter) &&
3878                     (pmlmeinfo->assoc_AP_vendor != broadcomAP ||
3879                      memcmp(raddr, tendaAPMac, 3))) {
3880                         /*  max buffer size is 8 MSDU */
3881                         BA_para_set &= ~IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
3882                         BA_para_set |= (8 << 6) &
3883                                 IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
3884                 }
3885
3886                 if (pregpriv->ampdu_amsdu == 0)/* disabled */
3887                         BA_para_set &= ~BIT(0);
3888                 else if (pregpriv->ampdu_amsdu == 1)/* enabled */
3889                         BA_para_set |= BIT(0);
3890
3891                 put_unaligned_le16(BA_para_set,
3892                                    &mgmt->u.action.u.addba_resp.capab);
3893
3894                 mgmt->u.action.u.addba_resp.timeout
3895                         = pmlmeinfo->ADDBA_req.BA_timeout_value;
3896
3897                 pattrib->pktlen += 8;
3898                 break;
3899         case WLAN_ACTION_DELBA:
3900                 pattrib->pktlen += sizeof(mgmt->u.action.u.delba);
3901
3902                 mgmt->u.action.u.delba.action_code = action;
3903                 BA_para_set = (status & 0x1F) << 3;
3904                 mgmt->u.action.u.delba.params = cpu_to_le16(BA_para_set);
3905                 mgmt->u.action.u.delba.reason_code =
3906                         cpu_to_le16(WLAN_REASON_QSTA_NOT_USE);
3907
3908                 pattrib->pktlen += 5;
3909                 break;
3910         default:
3911                 break;
3912         }
3913
3914         pattrib->last_txcmdsz = pattrib->pktlen;
3915
3916         dump_mgntframe23a(padapter, pmgntframe);
3917 }
3918
3919 int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr)
3920 {
3921         struct sta_priv *pstapriv = &padapter->stapriv;
3922         struct sta_info *psta = NULL;
3923         /* struct recv_reorder_ctrl *preorder_ctrl; */
3924         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3925         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3926         u16 tid;
3927
3928         if ((pmlmeinfo->state&0x03) != MSR_AP)
3929                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
3930                         return _SUCCESS;
3931
3932         psta = rtw_get_stainfo23a(pstapriv, addr);
3933         if (psta == NULL)
3934                 return _SUCCESS;
3935
3936         if (initiator == 0) {  /*  recipient */
3937                 for (tid = 0; tid < MAXTID; tid++) {
3938                         if (psta->recvreorder_ctrl[tid].enable == true) {
3939                                 DBG_8723A("rx agg disable tid(%d)\n", tid);
3940                                 issue_action_BA23a(padapter, addr, WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F));
3941                                 psta->recvreorder_ctrl[tid].enable = false;
3942                                 psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
3943                         }
3944                 }
3945         } else if (initiator == 1) { /*  originator */
3946                 for (tid = 0; tid < MAXTID; tid++) {
3947                         if (psta->htpriv.agg_enable_bitmap & BIT(tid)) {
3948                                 DBG_8723A("tx agg disable tid(%d)\n", tid);
3949                                 issue_action_BA23a(padapter, addr, WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F));
3950                                 psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
3951                                 psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
3952
3953                         }
3954                 }
3955         }
3956         return _SUCCESS;
3957 }
3958
3959 int send_beacon23a(struct rtw_adapter *padapter)
3960 {
3961         bool bxmitok;
3962         int issue = 0;
3963         int poll = 0;
3964         unsigned long start = jiffies;
3965         unsigned int passing_time;
3966
3967         rtl8723a_bcn_valid(padapter);
3968         do {
3969                 issue_beacon23a(padapter, 100);
3970                 issue++;
3971                 do {
3972                         yield();
3973                         bxmitok = rtl8723a_get_bcn_valid(padapter);
3974                         poll++;
3975                 } while ((poll % 10) != 0 && !bxmitok &&
3976                          !padapter->bSurpriseRemoved &&
3977                          !padapter->bDriverStopped);
3978
3979         } while (!bxmitok && issue<100 && !padapter->bSurpriseRemoved &&
3980                  !padapter->bDriverStopped);
3981
3982         if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
3983                 return _FAIL;
3984
3985         passing_time = jiffies_to_msecs(jiffies - start);
3986
3987         if (!bxmitok) {
3988                 DBG_8723A("%s fail! %u ms\n", __func__, passing_time);
3989                 return _FAIL;
3990         } else {
3991
3992                 if (passing_time > 100 || issue > 3)
3993                         DBG_8723A("%s success, issue:%d, poll:%d, %u ms\n",
3994                                   __func__, issue, poll, passing_time);
3995                 return _SUCCESS;
3996         }
3997 }
3998
3999 /****************************************************************************
4000
4001 Following are some utitity functions for WiFi MLME
4002
4003 *****************************************************************************/
4004
4005 bool IsLegal5GChannel(struct rtw_adapter *Adapter, u8 channel)
4006 {
4007
4008         int i = 0;
4009         u8 Channel_5G[45] = {36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58,
4010                              60, 62, 64, 100, 102, 104, 106, 108, 110, 112,
4011                              114, 116, 118, 120, 122, 124, 126, 128, 130, 132,
4012                              134, 136, 138, 140, 149, 151, 153, 155, 157, 159,
4013                              161, 163, 165};
4014         for (i = 0; i < sizeof(Channel_5G); i++)
4015                 if (channel == Channel_5G[i])
4016                         return true;
4017         return false;
4018 }
4019
4020 static void rtw_site_survey(struct rtw_adapter *padapter)
4021 {
4022         unsigned char survey_channel = 0;
4023         enum rt_scan_type ScanType = SCAN_PASSIVE;
4024         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4025         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4026         struct rtw_ieee80211_channel *ch;
4027
4028         if (pmlmeext->sitesurvey_res.channel_idx <
4029             pmlmeext->sitesurvey_res.ch_num) {
4030                 ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
4031                 survey_channel = ch->hw_value;
4032                 ScanType = (ch->flags & IEEE80211_CHAN_NO_IR) ?
4033                         SCAN_PASSIVE : SCAN_ACTIVE;
4034         }
4035
4036         if (survey_channel != 0) {
4037                 /* PAUSE 4-AC Queue when site_survey */
4038                 if (pmlmeext->sitesurvey_res.channel_idx == 0)
4039                         set_channel_bwmode23a(padapter, survey_channel,
4040                                               HAL_PRIME_CHNL_OFFSET_DONT_CARE,
4041                                               HT_CHANNEL_WIDTH_20);
4042                 else
4043                         SelectChannel23a(padapter, survey_channel);
4044
4045                 if (ScanType == SCAN_ACTIVE) /* obey the channel plan setting... */
4046                 {
4047                         int i;
4048
4049                         for (i = 0;i<RTW_SSID_SCAN_AMOUNT;i++) {
4050                                 if (pmlmeext->sitesurvey_res.ssid[i].ssid_len) {
4051                                         /* todo: to issue two probe req??? */
4052                                         issue_probereq(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
4053                                         /* msleep(SURVEY_TO>>1); */
4054                                         issue_probereq(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
4055                                 }
4056                         }
4057
4058                         if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
4059                                 /* todo: to issue two probe req??? */
4060                                 issue_probereq(padapter, NULL, NULL);
4061                                 /* msleep(SURVEY_TO>>1); */
4062                                 issue_probereq(padapter, NULL, NULL);
4063                         }
4064                 }
4065
4066                 set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
4067         } else {
4068                 /*      channel number is 0 or this channel is not valid. */
4069                 pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
4070
4071                 /* switch back to the original channel */
4072
4073                 set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
4074                                       pmlmeext->cur_ch_offset,
4075                                       pmlmeext->cur_bwmode);
4076
4077                 /* flush 4-AC Queue after rtw_site_survey */
4078                 /* val8 = 0; */
4079
4080                 /* config MSR */
4081                 rtl8723a_set_media_status(padapter, pmlmeinfo->state & 0x3);
4082
4083                 /* restore RX GAIN */
4084                 rtl8723a_set_initial_gain(padapter, 0xff);
4085                 /* turn on dynamic functions */
4086                 rtl8723a_odm_support_ability_restore(padapter);
4087
4088                 if (is_client_associated_to_ap23a(padapter) == true)
4089                         issue_nulldata23a(padapter, NULL, 0, 3, 500);
4090
4091                 rtl8723a_mlme_sitesurvey(padapter, 0);
4092
4093                 report_surveydone_event23a(padapter);
4094
4095                 pmlmeext->chan_scan_time = SURVEY_TO;
4096                 pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
4097         }
4098 }
4099
4100 /* collect bss info from Beacon and Probe request/response frames. */
4101 static struct wlan_bssid_ex *collect_bss_info(struct rtw_adapter *padapter,
4102                                               struct recv_frame *precv_frame)
4103 {
4104         struct sk_buff *skb = precv_frame->pkt;
4105         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
4106         struct registry_priv *pregistrypriv = &padapter->registrypriv;
4107         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4108         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4109         struct wlan_bssid_ex *bssid;
4110         const u8 *p;
4111         u8 *pie;
4112         unsigned int length;
4113         int i;
4114
4115         length = skb->len;
4116
4117         bssid = kzalloc(sizeof(struct wlan_bssid_ex), GFP_ATOMIC);
4118         if (!bssid)
4119                 return NULL;
4120
4121         if (ieee80211_is_beacon(mgmt->frame_control)) {
4122                 length -= offsetof(struct ieee80211_mgmt, u.beacon.variable);
4123                 pie = mgmt->u.beacon.variable;
4124                 bssid->reserved = 1;
4125                 bssid->capability =
4126                         get_unaligned_le16(&mgmt->u.beacon.capab_info);
4127                 bssid->beacon_interval =
4128                         get_unaligned_le16(&mgmt->u.beacon.beacon_int);
4129                 bssid->tsf = get_unaligned_le64(&mgmt->u.beacon.timestamp);
4130         } else if (ieee80211_is_probe_req(mgmt->frame_control)) {
4131                 length -= offsetof(struct ieee80211_mgmt, u.probe_req.variable);
4132                 pie = mgmt->u.probe_req.variable;
4133                 bssid->reserved = 2;
4134                 bssid->capability = 0;
4135                 bssid->beacon_interval =
4136                         padapter->registrypriv.dev_network.beacon_interval;
4137                 bssid->tsf = 0;
4138         } else if (ieee80211_is_probe_resp(mgmt->frame_control)) {
4139                 length -=
4140                         offsetof(struct ieee80211_mgmt, u.probe_resp.variable);
4141                 pie = mgmt->u.probe_resp.variable;
4142                 bssid->reserved = 3;
4143                 bssid->capability =
4144                         get_unaligned_le16(&mgmt->u.probe_resp.capab_info);
4145                 bssid->beacon_interval =
4146                         get_unaligned_le16(&mgmt->u.probe_resp.beacon_int);
4147                 bssid->tsf = get_unaligned_le64(&mgmt->u.probe_resp.timestamp);
4148         } else {
4149                 length -= offsetof(struct ieee80211_mgmt, u.beacon.variable);
4150                 pie = mgmt->u.beacon.variable;
4151                 bssid->reserved = 0;
4152                 bssid->capability =
4153                         get_unaligned_le16(&mgmt->u.beacon.capab_info);
4154                 bssid->beacon_interval =
4155                         padapter->registrypriv.dev_network.beacon_interval;
4156                 bssid->tsf = 0;
4157         }
4158
4159         if (length > MAX_IE_SZ) {
4160                 /* DBG_8723A("IE too long for survey event\n"); */
4161                 kfree(bssid);
4162                 return NULL;
4163         }
4164
4165         bssid->Length = offsetof(struct wlan_bssid_ex, IEs) + length;
4166
4167         /* below is to copy the information element */
4168         bssid->IELength = length;
4169         memcpy(bssid->IEs, pie, bssid->IELength);
4170
4171         /* get the signal strength */
4172         /*  in dBM.raw data */
4173         bssid->Rssi = precv_frame->attrib.phy_info.RecvSignalPower;
4174         bssid->SignalQuality =
4175                 precv_frame->attrib.phy_info.SignalQuality;/* in percentage */
4176         bssid->SignalStrength =
4177                 precv_frame->attrib.phy_info.SignalStrength;/* in percentage */
4178
4179         /*  checking SSID */
4180         p = cfg80211_find_ie(WLAN_EID_SSID, bssid->IEs, bssid->IELength);
4181
4182         if (!p) {
4183                 DBG_8723A("marc: cannot find SSID for survey event\n");
4184                 goto fail;
4185         }
4186
4187         if (p[1] > IEEE80211_MAX_SSID_LEN) {
4188                 DBG_8723A("%s()-%d: IE too long (%d) for survey "
4189                           "event\n", __func__, __LINE__, p[1]);
4190                 goto fail;
4191         }
4192         memcpy(bssid->Ssid.ssid, p + 2, p[1]);
4193         bssid->Ssid.ssid_len = p[1];
4194
4195         /* checking rate info... */
4196         i = 0;
4197         p = cfg80211_find_ie(WLAN_EID_SUPP_RATES, bssid->IEs, bssid->IELength);
4198         if (p) {
4199                 if (p[1] > NDIS_802_11_LENGTH_RATES_EX) {
4200                         DBG_8723A("%s()-%d: IE too long (%d) for survey "
4201                                   "event\n", __func__, __LINE__, p[1]);
4202                         goto fail;
4203                 }
4204                 memcpy(bssid->SupportedRates, p + 2, p[1]);
4205                 i = p[1];
4206         }
4207
4208         p = cfg80211_find_ie(WLAN_EID_EXT_SUPP_RATES, bssid->IEs,
4209                              bssid->IELength);
4210         if (p) {
4211                 if (p[1] > (NDIS_802_11_LENGTH_RATES_EX-i)) {
4212                         DBG_8723A("%s()-%d: IE too long (%d) for survey "
4213                                   "event\n", __func__, __LINE__, p[1]);
4214                         goto fail;
4215                 }
4216                 memcpy(bssid->SupportedRates + i, p + 2, p[1]);
4217         }
4218
4219         /*  Checking for DSConfig */
4220         p = cfg80211_find_ie(WLAN_EID_DS_PARAMS, bssid->IEs, bssid->IELength);
4221
4222         bssid->DSConfig = 0;
4223
4224         if (p) {
4225                 bssid->DSConfig = p[2];
4226         } else {/*  In 5G, some ap do not have DSSET IE */
4227                 /*  checking HT info for channel */
4228                 p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, bssid->IEs,
4229                                      bssid->IELength);
4230                 if (p) {
4231                         struct ieee80211_ht_operation *HT_info =
4232                                 (struct ieee80211_ht_operation *)(p + 2);
4233                         bssid->DSConfig = HT_info->primary_chan;
4234                 } else /*  use current channel */
4235                         bssid->DSConfig = rtw_get_oper_ch23a(padapter);
4236         }
4237
4238         if (ieee80211_is_probe_req(mgmt->frame_control)) {
4239                 /*  FIXME */
4240                 bssid->ifmode = NL80211_IFTYPE_STATION;
4241                 ether_addr_copy(bssid->MacAddress, mgmt->sa);
4242                 bssid->Privacy = 1;
4243                 return bssid;
4244         }
4245
4246         if (bssid->capability & WLAN_CAPABILITY_ESS) {
4247                 bssid->ifmode = NL80211_IFTYPE_STATION;
4248                 ether_addr_copy(bssid->MacAddress, mgmt->sa);
4249         } else {
4250                 bssid->ifmode = NL80211_IFTYPE_ADHOC;
4251                 ether_addr_copy(bssid->MacAddress, mgmt->bssid);
4252         }
4253
4254         if (bssid->capability & WLAN_CAPABILITY_PRIVACY)
4255                 bssid->Privacy = 1;
4256         else
4257                 bssid->Privacy = 0;
4258
4259         bssid->ATIMWindow = 0;
4260
4261         /* 20/40 BSS Coexistence check */
4262         if (pregistrypriv->wifi_spec == 1 &&
4263             pmlmeinfo->bwmode_updated == false) {
4264                 struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
4265
4266                 p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, bssid->IEs,
4267                                      bssid->IELength);
4268                 if (p && p[1] > 0) {
4269                         struct ieee80211_ht_cap *pHT_caps;
4270
4271                         pHT_caps = (struct ieee80211_ht_cap *)(p + 2);
4272
4273                         if (pHT_caps->cap_info &
4274                             cpu_to_le16(IEEE80211_HT_CAP_40MHZ_INTOLERANT))
4275                                 pmlmepriv->num_FortyMHzIntolerant++;
4276                 } else
4277                         pmlmepriv->num_sta_no_ht++;
4278         }
4279
4280
4281         /*  mark bss info receiving from nearby channel as SignalQuality 101 */
4282         if (bssid->DSConfig != rtw_get_oper_ch23a(padapter))
4283                 bssid->SignalQuality = 101;
4284
4285         return bssid;
4286 fail:
4287         kfree (bssid);
4288         return NULL;
4289 }
4290
4291 static void start_create_ibss(struct rtw_adapter *padapter)
4292 {
4293         unsigned short caps;
4294         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4295         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4296         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
4297
4298         pmlmeext->cur_channel = (u8)pnetwork->DSConfig;
4299         pmlmeinfo->bcn_interval = pnetwork->beacon_interval;
4300
4301         /* update wireless mode */
4302         update_wireless_mode23a(padapter);
4303
4304         /* update capability */
4305         caps = pnetwork->capability;
4306         update_capinfo23a(padapter, caps);
4307         if (caps & WLAN_CAPABILITY_IBSS) {      /* adhoc master */
4308                 rtl8723a_set_sec_cfg(padapter, 0xcf);
4309
4310                 /* switch channel */
4311                 /* SelectChannel23a(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */
4312                 set_channel_bwmode23a(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
4313
4314                 rtl8723a_SetBeaconRelatedRegisters(padapter);
4315
4316                 /* set msr to MSR_ADHOC */
4317                 pmlmeinfo->state = MSR_ADHOC;
4318                 rtl8723a_set_media_status(padapter, pmlmeinfo->state & 0x3);
4319
4320                 /* issue beacon */
4321                 if (send_beacon23a(padapter) == _FAIL) {
4322                         RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
4323                                  "issuing beacon frame fail....\n");
4324
4325                         report_join_res23a(padapter, -1);
4326                         pmlmeinfo->state = MSR_NOLINK;
4327                 } else {
4328                         hw_var_set_bssid(padapter, padapter->registrypriv.dev_network.MacAddress);
4329                         hw_var_set_mlme_join(padapter, 0);
4330
4331                         report_join_res23a(padapter, 1);
4332                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
4333                 }
4334         } else {
4335                 DBG_8723A("%s: invalid cap:%x\n", __func__, caps);
4336                 return;
4337         }
4338 }
4339
4340 static void start_clnt_join(struct rtw_adapter *padapter)
4341 {
4342         unsigned short caps;
4343         u8 val8;
4344         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4345         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4346         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
4347         int beacon_timeout;
4348
4349         pmlmeext->cur_channel = (u8)pnetwork->DSConfig;
4350         pmlmeinfo->bcn_interval = pnetwork->beacon_interval;
4351
4352         /* update wireless mode */
4353         update_wireless_mode23a(padapter);
4354
4355         /* update capability */
4356         caps = pnetwork->capability;
4357         update_capinfo23a(padapter, caps);
4358         if (caps & WLAN_CAPABILITY_ESS) {
4359                 /* switch channel */
4360                 set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
4361
4362                 rtl8723a_set_media_status(padapter, MSR_INFRA);
4363
4364                 val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ?
4365                         0xcc: 0xcf;
4366
4367                 rtl8723a_set_sec_cfg(padapter, val8);
4368
4369                 /* switch channel */
4370                 /* set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
4371
4372                 /* here wait for receiving the beacon to start auth */
4373                 /* and enable a timer */
4374                 beacon_timeout = decide_wait_for_beacon_timeout23a(pmlmeinfo->bcn_interval);
4375                 set_link_timer(pmlmeext, beacon_timeout);
4376                 mod_timer(&padapter->mlmepriv.assoc_timer, jiffies +
4377                           msecs_to_jiffies((REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) + beacon_timeout));
4378                 pmlmeinfo->state = WIFI_FW_AUTH_NULL | MSR_INFRA;
4379         } else if (caps & WLAN_CAPABILITY_IBSS) {       /* adhoc client */
4380                 rtl8723a_set_media_status(padapter, MSR_ADHOC);
4381
4382                 rtl8723a_set_sec_cfg(padapter, 0xcf);
4383
4384                 /* switch channel */
4385                 set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
4386
4387                 rtl8723a_SetBeaconRelatedRegisters(padapter);
4388
4389                 pmlmeinfo->state = MSR_ADHOC;
4390
4391                 report_join_res23a(padapter, 1);
4392         } else {
4393                 /* DBG_8723A("marc: invalid cap:%x\n", caps); */
4394                 return;
4395         }
4396 }
4397
4398 static void start_clnt_auth(struct rtw_adapter *padapter)
4399 {
4400         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4401         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4402
4403         del_timer_sync(&pmlmeext->link_timer);
4404
4405         pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
4406         pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
4407
4408         pmlmeinfo->auth_seq = 1;
4409         pmlmeinfo->reauth_count = 0;
4410         pmlmeinfo->reassoc_count = 0;
4411         pmlmeinfo->link_count = 0;
4412         pmlmeext->retry = 0;
4413
4414         /*  Because of AP's not receiving deauth before */
4415         /*  AP may: 1)not response auth or 2)deauth us after link is complete */
4416         /*  issue deauth before issuing auth to deal with the situation */
4417         /*      Commented by Albert 2012/07/21 */
4418         /*      For the Win8 P2P connection, it will be hard to have a
4419                 successful connection if this Wi-Fi doesn't connect to it. */
4420         issue_deauth23a(padapter, (&pmlmeinfo->network)->MacAddress,
4421                         WLAN_REASON_DEAUTH_LEAVING);
4422
4423         DBG_8723A_LEVEL(_drv_always_, "start auth\n");
4424         issue_auth(padapter, NULL, 0);
4425
4426         set_link_timer(pmlmeext, REAUTH_TO);
4427 }
4428
4429 static void start_clnt_assoc(struct rtw_adapter *padapter)
4430 {
4431         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4432         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4433
4434         del_timer_sync(&pmlmeext->link_timer);
4435
4436         pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
4437         pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
4438
4439         issue_assocreq(padapter);
4440
4441         set_link_timer(pmlmeext, REASSOC_TO);
4442 }
4443
4444 int receive_disconnect23a(struct rtw_adapter *padapter,
4445                           unsigned char *MacAddr, unsigned short reason)
4446 {
4447         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4448         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4449
4450         /* check A3 */
4451         if (!ether_addr_equal(MacAddr, get_my_bssid23a(&pmlmeinfo->network)))
4452                 return _SUCCESS;
4453
4454         DBG_8723A("%s\n", __func__);
4455
4456         if ((pmlmeinfo->state&0x03) == MSR_INFRA) {
4457                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
4458                         pmlmeinfo->state = MSR_NOLINK;
4459                         report_del_sta_event23a(padapter, MacAddr, reason);
4460
4461                 } else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) {
4462                         pmlmeinfo->state = MSR_NOLINK;
4463                         report_join_res23a(padapter, -2);
4464                 }
4465         }
4466
4467         return _SUCCESS;
4468 }
4469
4470 static void process_80211d(struct rtw_adapter *padapter,
4471                            struct wlan_bssid_ex *bssid)
4472 {
4473         struct registry_priv *pregistrypriv;
4474         struct mlme_ext_priv *pmlmeext;
4475         struct rt_channel_info *chplan_new;
4476         u8 channel;
4477         u8 i;
4478
4479         pregistrypriv = &padapter->registrypriv;
4480         pmlmeext = &padapter->mlmeextpriv;
4481
4482         /*  Adjust channel plan by AP Country IE */
4483         if (pregistrypriv->enable80211d &&
4484             !pmlmeext->update_channel_plan_by_ap_done) {
4485                 const u8 *ie, *p;
4486                 struct rt_channel_plan chplan_ap;
4487                 struct rt_channel_info chplan_sta[MAX_CHANNEL_NUM];
4488                 u8 country[4];
4489                 u8 fcn; /*  first channel number */
4490                 u8 noc; /*  number of channel */
4491                 u8 j, k;
4492
4493                 ie = cfg80211_find_ie(WLAN_EID_COUNTRY, bssid->IEs,
4494                                       bssid->IELength);
4495                 if (!ie || ie[1] < IEEE80211_COUNTRY_IE_MIN_LEN)
4496                         return;
4497
4498                 p = ie + 2;
4499                 ie += ie[1];
4500                 ie += 2;
4501
4502                 memcpy(country, p, 3);
4503                 country[3] = '\0';
4504
4505                 p += 3;
4506                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
4507                          "%s: 802.11d country =%s\n", __func__, country);
4508
4509                 i = 0;
4510                 while ((ie - p) >= 3) {
4511                         fcn = *(p++);
4512                         noc = *(p++);
4513                         p++;
4514
4515                         for (j = 0; j < noc; j++) {
4516                                 if (fcn <= 14)
4517                                         channel = fcn + j; /*  2.4 GHz */
4518                                 else
4519                                         channel = fcn + j * 4; /*  5 GHz */
4520
4521                                 chplan_ap.Channel[i++] = channel;
4522                         }
4523                 }
4524                 chplan_ap.Len = i;
4525
4526                 memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
4527                 memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
4528                 chplan_new = pmlmeext->channel_set;
4529
4530                 i = j = k = 0;
4531                 if (pregistrypriv->wireless_mode & WIRELESS_11G) {
4532                         do {
4533                                 if (i == MAX_CHANNEL_NUM ||
4534                                     chplan_sta[i].ChannelNum == 0 ||
4535                                     chplan_sta[i].ChannelNum > 14)
4536                                         break;
4537
4538                                 if (j == chplan_ap.Len ||
4539                                     chplan_ap.Channel[j] > 14)
4540                                         break;
4541
4542                                 if (chplan_sta[i].ChannelNum ==
4543                                     chplan_ap.Channel[j]) {
4544                                         chplan_new[k].ChannelNum =
4545                                                 chplan_ap.Channel[j];
4546                                         chplan_new[k].ScanType = SCAN_ACTIVE;
4547                                         i++;
4548                                         j++;
4549                                         k++;
4550                                 } else if (chplan_sta[i].ChannelNum <
4551                                            chplan_ap.Channel[j]) {
4552                                         chplan_new[k].ChannelNum =
4553                                                 chplan_sta[i].ChannelNum;
4554                                         chplan_new[k].ScanType =
4555                                                 SCAN_PASSIVE;
4556                                         i++;
4557                                         k++;
4558                                 } else if (chplan_sta[i].ChannelNum >
4559                                            chplan_ap.Channel[j]) {
4560                                         chplan_new[k].ChannelNum =
4561                                                 chplan_ap.Channel[j];
4562                                         chplan_new[k].ScanType =
4563                                                 SCAN_ACTIVE;
4564                                         j++;
4565                                         k++;
4566                                 }
4567                         } while (1);
4568
4569                         /*  change AP not support channel to Passive scan */
4570                         while (i < MAX_CHANNEL_NUM &&
4571                                chplan_sta[i].ChannelNum != 0 &&
4572                                chplan_sta[i].ChannelNum <= 14) {
4573                                 chplan_new[k].ChannelNum =
4574                                         chplan_sta[i].ChannelNum;
4575                                 chplan_new[k].ScanType = SCAN_PASSIVE;
4576                                 i++;
4577                                 k++;
4578                         }
4579
4580                         /*  add channel AP supported */
4581                         while (j < chplan_ap.Len && chplan_ap.Channel[j] <= 14){
4582                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
4583                                 chplan_new[k].ScanType = SCAN_ACTIVE;
4584                                 j++;
4585                                 k++;
4586                         }
4587                 } else {
4588                         /*  keep original STA 2.4G channel plan */
4589                         while (i < MAX_CHANNEL_NUM &&
4590                                chplan_sta[i].ChannelNum != 0 &&
4591                                chplan_sta[i].ChannelNum <= 14) {
4592                                 chplan_new[k].ChannelNum =
4593                                         chplan_sta[i].ChannelNum;
4594                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
4595                                 i++;
4596                                 k++;
4597                         }
4598
4599                         /*  skip AP 2.4G channel plan */
4600                         while (j < chplan_ap.Len && chplan_ap.Channel[j] <= 14)
4601                                 j++;
4602                 }
4603
4604                 if (pregistrypriv->wireless_mode & WIRELESS_11A) {
4605                         do {
4606                                 if (i == MAX_CHANNEL_NUM ||
4607                                     chplan_sta[i].ChannelNum == 0)
4608                                         break;
4609
4610                                 if (j == chplan_ap.Len ||
4611                                     chplan_ap.Channel[j] == 0)
4612                                         break;
4613
4614                                 if (chplan_sta[i].ChannelNum ==
4615                                     chplan_ap.Channel[j]) {
4616                                         chplan_new[k].ChannelNum =
4617                                                 chplan_ap.Channel[j];
4618                                         chplan_new[k].ScanType = SCAN_ACTIVE;
4619                                         i++;
4620                                         j++;
4621                                         k++;
4622                                 } else if (chplan_sta[i].ChannelNum <
4623                                            chplan_ap.Channel[j]) {
4624                                         chplan_new[k].ChannelNum =
4625                                                 chplan_sta[i].ChannelNum;
4626                                         chplan_new[k].ScanType = SCAN_PASSIVE;
4627                                         i++;
4628                                         k++;
4629                                 } else if (chplan_sta[i].ChannelNum >
4630                                            chplan_ap.Channel[j]) {
4631                                         chplan_new[k].ChannelNum =
4632                                                 chplan_ap.Channel[j];
4633                                         chplan_new[k].ScanType = SCAN_ACTIVE;
4634                                         j++;
4635                                         k++;
4636                                 }
4637                         } while (1);
4638
4639                         /*  change AP not support channel to Passive scan */
4640                         while (i < MAX_CHANNEL_NUM &&
4641                                chplan_sta[i].ChannelNum != 0) {
4642                                 chplan_new[k].ChannelNum =
4643                                         chplan_sta[i].ChannelNum;
4644                                 chplan_new[k].ScanType = SCAN_PASSIVE;
4645                                 i++;
4646                                 k++;
4647                         }
4648
4649                         /*  add channel AP supported */
4650                         while (j < chplan_ap.Len && chplan_ap.Channel[j] != 0) {
4651                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
4652                                 chplan_new[k].ScanType = SCAN_ACTIVE;
4653                                 j++;
4654                                 k++;
4655                         }
4656                 } else {
4657                         /*  keep original STA 5G channel plan */
4658                         while (i < MAX_CHANNEL_NUM &&
4659                                chplan_sta[i].ChannelNum != 0) {
4660                                 chplan_new[k].ChannelNum =
4661                                         chplan_sta[i].ChannelNum;
4662                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
4663                                 i++;
4664                                 k++;
4665                         }
4666                 }
4667                 pmlmeext->update_channel_plan_by_ap_done = 1;
4668         }
4669
4670         /*  If channel is used by AP, set channel scan type to active */
4671         channel = bssid->DSConfig;
4672         chplan_new = pmlmeext->channel_set;
4673         i = 0;
4674         while (i < MAX_CHANNEL_NUM && chplan_new[i].ChannelNum != 0) {
4675                 if (chplan_new[i].ChannelNum == channel) {
4676                         if (chplan_new[i].ScanType == SCAN_PASSIVE) {
4677                                 /* 5G Bnad 2, 3 (DFS) doesn't change
4678                                    to active scan */
4679                                 if (channel >= 52 && channel <= 144)
4680                                         break;
4681
4682                                 chplan_new[i].ScanType = SCAN_ACTIVE;
4683                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
4684                                          "%s: change channel %d scan type from passive to active\n",
4685                                          __func__, channel);
4686                         }
4687                         break;
4688                 }
4689                 i++;
4690         }
4691 }
4692
4693 /****************************************************************************
4694
4695 Following are the functions to report events
4696
4697 *****************************************************************************/
4698
4699 void report_survey_event23a(struct rtw_adapter *padapter,
4700                             struct recv_frame *precv_frame)
4701 {
4702         struct cmd_obj *pcmd_obj;
4703         u8 *pevtcmd;
4704         u32 cmdsz;
4705         struct survey_event *psurvey_evt;
4706         struct C2HEvent_Header *pc2h_evt_hdr;
4707         struct mlme_ext_priv *pmlmeext;
4708         struct cmd_priv *pcmdpriv;
4709
4710         if (!padapter)
4711                 return;
4712
4713         pmlmeext = &padapter->mlmeextpriv;
4714         pcmdpriv = &padapter->cmdpriv;
4715
4716         pcmd_obj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
4717         if (!pcmd_obj)
4718                 return;
4719
4720         cmdsz = sizeof(struct survey_event) + sizeof(struct C2HEvent_Header);
4721         pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
4722         if (!pevtcmd) {
4723                 kfree(pcmd_obj);
4724                 return;
4725         }
4726
4727         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
4728         pcmd_obj->cmdsz = cmdsz;
4729         pcmd_obj->parmbuf = pevtcmd;
4730
4731         pcmd_obj->rsp = NULL;
4732         pcmd_obj->rspsz  = 0;
4733
4734         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
4735         pc2h_evt_hdr->len = sizeof(struct survey_event);
4736         pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
4737         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
4738
4739         psurvey_evt = (struct survey_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
4740
4741         psurvey_evt->bss = collect_bss_info(padapter, precv_frame);
4742         if (!psurvey_evt->bss) {
4743                 kfree(pcmd_obj);
4744                 kfree(pevtcmd);
4745                 return;
4746         }
4747
4748         process_80211d(padapter, psurvey_evt->bss);
4749
4750         rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj);
4751
4752         pmlmeext->sitesurvey_res.bss_cnt++;
4753 }
4754
4755 void report_surveydone_event23a(struct rtw_adapter *padapter)
4756 {
4757         struct cmd_obj *pcmd_obj;
4758         u8 *pevtcmd;
4759         u32 cmdsz;
4760         struct surveydone_event *psurveydone_evt;
4761         struct C2HEvent_Header *pc2h_evt_hdr;
4762         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4763         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
4764
4765         pcmd_obj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
4766         if (!pcmd_obj)
4767                 return;
4768
4769         cmdsz = sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header);
4770         pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
4771         if (!pevtcmd) {
4772                 kfree(pcmd_obj);
4773                 return;
4774         }
4775
4776         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
4777         pcmd_obj->cmdsz = cmdsz;
4778         pcmd_obj->parmbuf = pevtcmd;
4779
4780         pcmd_obj->rsp = NULL;
4781         pcmd_obj->rspsz  = 0;
4782
4783         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
4784         pc2h_evt_hdr->len = sizeof(struct surveydone_event);
4785         pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
4786         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
4787
4788         psurveydone_evt = (struct surveydone_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
4789         psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
4790
4791         DBG_8723A("survey done event(%x)\n", psurveydone_evt->bss_cnt);
4792
4793         rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj);
4794 }
4795
4796 void report_join_res23a(struct rtw_adapter *padapter, int res)
4797 {
4798         struct cmd_obj *pcmd_obj;
4799         u8 *pevtcmd;
4800         u32 cmdsz;
4801         struct joinbss_event            *pjoinbss_evt;
4802         struct C2HEvent_Header  *pc2h_evt_hdr;
4803         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
4804         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4805         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
4806
4807         pcmd_obj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
4808         if (!pcmd_obj)
4809                 return;
4810
4811         cmdsz = sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header);
4812         pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
4813         if (!pevtcmd) {
4814                 kfree(pcmd_obj);
4815                 return;
4816         }
4817
4818         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
4819         pcmd_obj->cmdsz = cmdsz;
4820         pcmd_obj->parmbuf = pevtcmd;
4821
4822         pcmd_obj->rsp = NULL;
4823         pcmd_obj->rspsz  = 0;
4824
4825         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
4826         pc2h_evt_hdr->len = sizeof(struct joinbss_event);
4827         pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
4828         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
4829
4830         pjoinbss_evt = (struct joinbss_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
4831         memcpy((unsigned char *)&pjoinbss_evt->network.network,
4832                &pmlmeinfo->network, sizeof(struct wlan_bssid_ex));
4833         pjoinbss_evt->network.join_res = res;
4834
4835         DBG_8723A("report_join_res23a(%d)\n", res);
4836
4837         rtw_joinbss_event_prehandle23a(padapter, (u8 *)&pjoinbss_evt->network);
4838
4839         rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj);
4840 }
4841
4842 void report_del_sta_event23a(struct rtw_adapter *padapter,
4843                              unsigned char *MacAddr, unsigned short reason)
4844 {
4845         struct cmd_obj *pcmd_obj;
4846         u8 *pevtcmd;
4847         u32 cmdsz;
4848         struct sta_info *psta;
4849         int mac_id;
4850         struct stadel_event *pdel_sta_evt;
4851         struct C2HEvent_Header *pc2h_evt_hdr;
4852         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4853         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
4854
4855         pcmd_obj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
4856         if (!pcmd_obj)
4857                 return;
4858
4859         cmdsz = sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header);
4860         pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
4861         if (!pevtcmd) {
4862                 kfree(pcmd_obj);
4863                 return;
4864         }
4865
4866         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
4867         pcmd_obj->cmdsz = cmdsz;
4868         pcmd_obj->parmbuf = pevtcmd;
4869
4870         pcmd_obj->rsp = NULL;
4871         pcmd_obj->rspsz  = 0;
4872
4873         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
4874         pc2h_evt_hdr->len = sizeof(struct stadel_event);
4875         pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
4876         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
4877
4878         pdel_sta_evt = (struct stadel_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
4879         ether_addr_copy((unsigned char *)&pdel_sta_evt->macaddr, MacAddr);
4880         memcpy((unsigned char *)pdel_sta_evt->rsvd, (unsigned char *)&reason,
4881                2);
4882
4883         psta = rtw_get_stainfo23a(&padapter->stapriv, MacAddr);
4884         if (psta)
4885                 mac_id = (int)psta->mac_id;
4886         else
4887                 mac_id = -1;
4888
4889         pdel_sta_evt->mac_id = mac_id;
4890
4891         DBG_8723A("report_del_sta_event23a: delete STA, mac_id =%d\n", mac_id);
4892
4893         rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj);
4894 }
4895
4896 void report_add_sta_event23a(struct rtw_adapter *padapter,
4897                              unsigned char *MacAddr, int cam_idx)
4898 {
4899         struct cmd_obj *pcmd_obj;
4900         u8 *pevtcmd;
4901         u32 cmdsz;
4902         struct stassoc_event *padd_sta_evt;
4903         struct C2HEvent_Header *pc2h_evt_hdr;
4904         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4905         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
4906
4907         pcmd_obj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
4908         if (!pcmd_obj)
4909                 return;
4910
4911         cmdsz = sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header);
4912         pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
4913         if (!pevtcmd) {
4914                 kfree(pcmd_obj);
4915                 return;
4916         }
4917
4918         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
4919         pcmd_obj->cmdsz = cmdsz;
4920         pcmd_obj->parmbuf = pevtcmd;
4921
4922         pcmd_obj->rsp = NULL;
4923         pcmd_obj->rspsz  = 0;
4924
4925         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
4926         pc2h_evt_hdr->len = sizeof(struct stassoc_event);
4927         pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
4928         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
4929
4930         padd_sta_evt = (struct stassoc_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
4931         ether_addr_copy((unsigned char *)&padd_sta_evt->macaddr, MacAddr);
4932         padd_sta_evt->cam_id = cam_idx;
4933
4934         DBG_8723A("report_add_sta_event23a: add STA\n");
4935
4936         rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj);
4937 }
4938
4939 /****************************************************************************
4940
4941 Following are the event callback functions
4942
4943 *****************************************************************************/
4944
4945 /* for sta/adhoc mode */
4946 void update_sta_info23a(struct rtw_adapter *padapter, struct sta_info *psta)
4947 {
4948         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
4949         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4950         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4951
4952         /* ERP */
4953         VCS_update23a(padapter, psta);
4954
4955         /* HT */
4956         if (pmlmepriv->htpriv.ht_option) {
4957                 psta->htpriv.ht_option = true;
4958
4959                 psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
4960
4961                 if (support_short_GI23a(padapter, &pmlmeinfo->ht_cap))
4962                         psta->htpriv.sgi = true;
4963
4964                 psta->qos_option = true;
4965
4966         } else {
4967                 psta->htpriv.ht_option = false;
4968
4969                 psta->htpriv.ampdu_enable = false;
4970
4971                 psta->htpriv.sgi = false;
4972                 psta->qos_option = false;
4973
4974         }
4975         psta->htpriv.bwmode = pmlmeext->cur_bwmode;
4976         psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
4977
4978         psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
4979         psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
4980
4981         /* QoS */
4982         if (pmlmepriv->qos_option)
4983                 psta->qos_option = true;
4984
4985         psta->state = _FW_LINKED;
4986 }
4987
4988 void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter,
4989                                        int join_res)
4990 {
4991         struct sta_info *psta, *psta_bmc;
4992         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4993         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4994         struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
4995         struct sta_priv *pstapriv = &padapter->stapriv;
4996
4997         if (join_res < 0) {
4998                 hw_var_set_mlme_join(padapter, 1);
4999                 hw_var_set_bssid(padapter, null_addr);
5000
5001                 /* restore to initial setting. */
5002                 update_tx_basic_rate23a(padapter,
5003                                         padapter->registrypriv.wireless_mode);
5004
5005                 goto exit_mlmeext_joinbss_event_callback23a;
5006         }
5007
5008         if ((pmlmeinfo->state&0x03) == MSR_ADHOC) {
5009                 /* for bc/mc */
5010                 psta_bmc = rtw_get_bcmc_stainfo23a(padapter);
5011                 if (psta_bmc) {
5012                         pmlmeinfo->FW_sta_info[psta_bmc->mac_id].psta = psta_bmc;
5013                         update_bmc_sta_support_rate23a(padapter, psta_bmc->mac_id);
5014                         Update_RA_Entry23a(padapter, psta_bmc);
5015                 }
5016         }
5017
5018         /* turn on dynamic functions */
5019         rtl8723a_odm_support_ability_set(padapter, DYNAMIC_ALL_FUNC_ENABLE);
5020
5021         /*  update IOT-releated issue */
5022         update_IOT_info23a(padapter);
5023
5024         HalSetBrateCfg23a(padapter, cur_network->SupportedRates);
5025
5026         /* BCN interval */
5027         rtl8723a_set_beacon_interval(padapter, pmlmeinfo->bcn_interval);
5028
5029         /* update capability */
5030         update_capinfo23a(padapter, pmlmeinfo->capability);
5031
5032         /* WMM, Update EDCA param */
5033         WMMOnAssocRsp23a(padapter);
5034
5035         /* HT */
5036         HTOnAssocRsp23a(padapter);
5037
5038         /* Set cur_channel&cur_bwmode&cur_ch_offset */
5039         set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
5040
5041         psta = rtw_get_stainfo23a(pstapriv, cur_network->MacAddress);
5042         if (psta) { /* only for infra. mode */
5043                 pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
5044
5045                 /* DBG_8723A("set_sta_rate23a\n"); */
5046
5047                 psta->wireless_mode = pmlmeext->cur_wireless_mode;
5048
5049                 /* set per sta rate after updating HT cap. */
5050                 set_sta_rate23a(padapter, psta);
5051         }
5052
5053         hw_var_set_mlme_join(padapter, 2);
5054
5055         if ((pmlmeinfo->state&0x03) == MSR_INFRA) {
5056                 /*  correcting TSF */
5057                 rtw_correct_TSF(padapter);
5058
5059                 /* set_link_timer(pmlmeext, DISCONNECT_TO); */
5060         }
5061
5062         rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_CONNECT, 0);
5063
5064 exit_mlmeext_joinbss_event_callback23a:
5065         DBG_8723A("=>%s\n", __func__);
5066 }
5067
5068 void mlmeext_sta_add_event_callback23a(struct rtw_adapter *padapter,
5069                                        struct sta_info *psta)
5070 {
5071         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5072         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5073
5074         DBG_8723A("%s\n", __func__);
5075
5076         if ((pmlmeinfo->state & 0x03) == MSR_ADHOC) {
5077         /* adhoc master or sta_count>1 */
5078                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
5079                         /* nothing to do */
5080                 } else { /* adhoc client */
5081                         /*  correcting TSF */
5082                         rtw_correct_TSF(padapter);
5083
5084                         /* start beacon */
5085                         if (send_beacon23a(padapter) != _SUCCESS) {
5086                                 pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
5087
5088                                 pmlmeinfo->state ^= MSR_ADHOC;
5089
5090                                 return;
5091                         }
5092
5093                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
5094                 }
5095                 hw_var_set_mlme_join(padapter, 2);
5096         }
5097
5098         pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
5099
5100         /* rate radaptive */
5101         Update_RA_Entry23a(padapter, psta);
5102
5103         /* update adhoc sta_info */
5104         update_sta_info23a(padapter, psta);
5105 }
5106
5107 void mlmeext_sta_del_event_callback23a(struct rtw_adapter *padapter)
5108 {
5109         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5110         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5111
5112         if (is_client_associated_to_ap23a(padapter) ||
5113             is_IBSS_empty23a(padapter)) {
5114                 /* set_opmode_cmd(padapter, infra_client_with_mlme); */
5115
5116                 hw_var_set_mlme_disconnect(padapter);
5117                 hw_var_set_bssid(padapter, null_addr);
5118
5119                 /* restore to initial setting. */
5120                 update_tx_basic_rate23a(padapter,
5121                                         padapter->registrypriv.wireless_mode);
5122
5123                 /* switch to the 20M Hz mode after disconnect */
5124                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
5125                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
5126
5127                 set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
5128                                       pmlmeext->cur_ch_offset,
5129                                       pmlmeext->cur_bwmode);
5130
5131                 flush_all_cam_entry23a(padapter);
5132
5133                 pmlmeinfo->state = MSR_NOLINK;
5134
5135                 /* set MSR to no link state -> infra. mode */
5136                 rtl8723a_set_media_status(padapter, MSR_INFRA);
5137
5138                 del_timer_sync(&pmlmeext->link_timer);
5139         }
5140 }
5141
5142 static u8 chk_ap_is_alive(struct rtw_adapter *padapter, struct sta_info *psta)
5143 {
5144         u8 ret = false;
5145
5146         if (sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta) &&
5147             sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta) &&
5148             sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta))
5149                 ret = false;
5150         else
5151                 ret = true;
5152
5153         sta_update_last_rx_pkts(psta);
5154         return ret;
5155 }
5156
5157 void linked_status_chk23a(struct rtw_adapter *padapter)
5158 {
5159         u32 i;
5160         struct sta_info *psta;
5161         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
5162         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5163         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5164         struct sta_priv *pstapriv = &padapter->stapriv;
5165
5166         if (is_client_associated_to_ap23a(padapter)) {
5167                 /* linked infrastructure client mode */
5168
5169                 int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
5170                 int rx_chk_limit;
5171
5172                 rx_chk_limit = 4;
5173
5174                 psta = rtw_get_stainfo23a(pstapriv,
5175                                           pmlmeinfo->network.MacAddress);
5176                 if (psta) {
5177                         bool is_p2p_enable = false;
5178
5179                         if (chk_ap_is_alive(padapter, psta) == false)
5180                                 rx_chk = _FAIL;
5181
5182                         if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
5183                                 tx_chk = _FAIL;
5184
5185                         if (pmlmeext->active_keep_alive_check &&
5186                             (rx_chk == _FAIL || tx_chk == _FAIL)) {
5187                                 u8 backup_oper_channel = 0;
5188
5189                                 /* switch to correct channel of current
5190                                    network  before issue keep-alive frames */
5191                                 if (rtw_get_oper_ch23a(padapter) !=
5192                                     pmlmeext->cur_channel) {
5193                                         backup_oper_channel =
5194                                                 rtw_get_oper_ch23a(padapter);
5195                                         SelectChannel23a(padapter,
5196                                                          pmlmeext->cur_channel);
5197                                 }
5198
5199                                 if (rx_chk != _SUCCESS)
5200                                         issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
5201
5202                                 if ((tx_chk != _SUCCESS &&
5203                                      pmlmeinfo->link_count++ == 0xf) ||
5204                                     rx_chk != _SUCCESS) {
5205                                         tx_chk = issue_nulldata23a(padapter,
5206                                                                    psta->hwaddr,
5207                                                                    0, 3, 1);
5208                                         /* if tx acked and p2p disabled,
5209                                            set rx_chk _SUCCESS to reset retry
5210                                            count */
5211                                         if (tx_chk == _SUCCESS &&
5212                                             !is_p2p_enable)
5213                                                 rx_chk = _SUCCESS;
5214                                 }
5215
5216                                 /* back to the original operation channel */
5217                                 if (backup_oper_channel>0)
5218                                         SelectChannel23a(padapter,
5219                                                          backup_oper_channel);
5220                         } else {
5221                                 if (rx_chk != _SUCCESS) {
5222                                         if (pmlmeext->retry == 0) {
5223                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
5224                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
5225                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
5226                                         }
5227                                 }
5228
5229                                 if (tx_chk != _SUCCESS &&
5230                                     pmlmeinfo->link_count++ == 0xf)
5231                                         tx_chk = issue_nulldata23a(padapter,
5232                                                                    NULL, 0, 1,
5233                                                                    0);
5234                         }
5235
5236                         if (rx_chk == _FAIL) {
5237                                 pmlmeext->retry++;
5238                                 if (pmlmeext->retry > rx_chk_limit) {
5239                                         DBG_8723A_LEVEL(_drv_always_,
5240                                                         "%s(%s): disconnect or "
5241                                                         "roaming\n", __func__,
5242                                                         padapter->pnetdev->name);
5243                                         receive_disconnect23a(padapter, pmlmeinfo->network.MacAddress,
5244                                                 WLAN_REASON_EXPIRATION_CHK);
5245                                         return;
5246                                 }
5247                         } else
5248                                 pmlmeext->retry = 0;
5249
5250                         if (tx_chk == _FAIL)
5251                                 pmlmeinfo->link_count &= 0xf;
5252                         else {
5253                                 pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
5254                                 pmlmeinfo->link_count = 0;
5255                         }
5256
5257                 }
5258         } else if (is_client_associated_to_ibss23a(padapter)) {
5259                 /* linked IBSS mode */
5260                 /* for each assoc list entry to check the rx pkt counter */
5261                 for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
5262                         if (pmlmeinfo->FW_sta_info[i].status == 1) {
5263                                 psta = pmlmeinfo->FW_sta_info[i].psta;
5264
5265                                 if (!psta)
5266                                         continue;
5267
5268                                 if (pmlmeinfo->FW_sta_info[i].rx_pkt ==
5269                                     sta_rx_pkts(psta)) {
5270
5271                                         if (pmlmeinfo->FW_sta_info[i].retry<3) {
5272                                                 pmlmeinfo->FW_sta_info[i].retry++;
5273                                         } else {
5274                                                 pmlmeinfo->FW_sta_info[i].retry = 0;
5275                                                 pmlmeinfo->FW_sta_info[i].status = 0;
5276                                                 report_del_sta_event23a(padapter, psta->hwaddr,
5277                                                         65535/*  indicate disconnect caused by no rx */
5278                                                 );
5279                                         }
5280                                 } else {
5281                                         pmlmeinfo->FW_sta_info[i].retry = 0;
5282                                         pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
5283                                 }
5284                         }
5285                 }
5286                 /* set_link_timer(pmlmeext, DISCONNECT_TO); */
5287         }
5288 }
5289
5290 static void survey_timer_hdl(unsigned long data)
5291 {
5292         struct rtw_adapter *padapter = (struct rtw_adapter *)data;
5293         struct cmd_obj *ph2c;
5294         struct sitesurvey_parm *psurveyPara;
5295         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
5296         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5297
5298         /* issue rtw_sitesurvey_cmd23a */
5299         if (pmlmeext->sitesurvey_res.state > SCAN_START) {
5300                 if (pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
5301                         pmlmeext->sitesurvey_res.channel_idx++;
5302
5303                 if (pmlmeext->scan_abort == true) {
5304                         pmlmeext->sitesurvey_res.channel_idx =
5305                                 pmlmeext->sitesurvey_res.ch_num;
5306                         DBG_8723A("%s idx:%d\n", __func__,
5307                                   pmlmeext->sitesurvey_res.channel_idx);
5308
5309                         pmlmeext->scan_abort = false;/* reset */
5310                 }
5311
5312                 ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
5313                 if (!ph2c)
5314                         goto exit_survey_timer_hdl;
5315
5316                 psurveyPara = kzalloc(sizeof(struct sitesurvey_parm),
5317                                         GFP_ATOMIC);
5318                 if (!psurveyPara) {
5319                         kfree(ph2c);
5320                         goto exit_survey_timer_hdl;
5321                 }
5322
5323                 init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara,
5324                                            GEN_CMD_CODE(_SiteSurvey));
5325                 rtw_enqueue_cmd23a(pcmdpriv, ph2c);
5326         }
5327
5328 exit_survey_timer_hdl:
5329         return;
5330 }
5331
5332 static void link_timer_hdl(unsigned long data)
5333 {
5334         struct rtw_adapter *padapter = (struct rtw_adapter *)data;
5335         /* static unsigned int          rx_pkt = 0; */
5336         /* static u64                           tx_cnt = 0; */
5337         /* struct xmit_priv *pxmitpriv = &padapter->xmitpriv; */
5338         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5339         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5340         /* struct sta_priv              *pstapriv = &padapter->stapriv; */
5341
5342         if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
5343                 DBG_8723A("link_timer_hdl:no beacon while connecting\n");
5344                 pmlmeinfo->state = MSR_NOLINK;
5345                 report_join_res23a(padapter, -3);
5346         } else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) {
5347                 /* re-auth timer */
5348                 if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) {
5349                         /* if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto) */
5350                         /*  */
5351                                 pmlmeinfo->state = 0;
5352                                 report_join_res23a(padapter, -1);
5353                                 return;
5354                         /*  */
5355                         /* else */
5356                         /*  */
5357                         /* pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */
5358                         /* pmlmeinfo->reauth_count = 0; */
5359                         /*  */
5360                 }
5361
5362                 DBG_8723A("link_timer_hdl: auth timeout and try again\n");
5363                 pmlmeinfo->auth_seq = 1;
5364                 issue_auth(padapter, NULL, 0);
5365                 set_link_timer(pmlmeext, REAUTH_TO);
5366         } else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) {
5367                 /* re-assoc timer */
5368                 if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) {
5369                         pmlmeinfo->state = MSR_NOLINK;
5370                         report_join_res23a(padapter, -2);
5371                         return;
5372                 }
5373
5374                 DBG_8723A("link_timer_hdl: assoc timeout and try again\n");
5375                 issue_assocreq(padapter);
5376                 set_link_timer(pmlmeext, REASSOC_TO);
5377         }
5378 }
5379
5380 static void addba_timer_hdl(unsigned long data)
5381 {
5382         struct sta_info *psta = (struct sta_info *)data;
5383         struct ht_priv *phtpriv;
5384
5385         if (!psta)
5386                 return;
5387
5388         phtpriv = &psta->htpriv;
5389
5390         if (phtpriv->ht_option && phtpriv->ampdu_enable) {
5391                 if (phtpriv->candidate_tid_bitmap)
5392                         phtpriv->candidate_tid_bitmap = 0x0;
5393         }
5394 }
5395
5396 void init_addba_retry_timer23a(struct sta_info *psta)
5397 {
5398         setup_timer(&psta->addba_retry_timer, addba_timer_hdl,
5399                     (unsigned long)psta);
5400 }
5401
5402 void init_mlme_ext_timer23a(struct rtw_adapter *padapter)
5403 {
5404         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5405
5406         setup_timer(&pmlmeext->survey_timer, survey_timer_hdl,
5407                     (unsigned long)padapter);
5408
5409         setup_timer(&pmlmeext->link_timer, link_timer_hdl,
5410                     (unsigned long)padapter);
5411 }
5412
5413 int NULL_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5414 {
5415         return H2C_SUCCESS;
5416 }
5417
5418 int setopmode_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5419 {
5420         enum nl80211_iftype type;
5421         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5422         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5423         const struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
5424
5425         switch (psetop->mode) {
5426         case NL80211_IFTYPE_P2P_GO:
5427         case NL80211_IFTYPE_AP:
5428                 pmlmeinfo->state = MSR_AP;
5429                 type = MSR_AP;
5430                 break;
5431         case NL80211_IFTYPE_P2P_CLIENT:
5432         case NL80211_IFTYPE_STATION:
5433                 /*  clear state */
5434                 pmlmeinfo->state &= ~(BIT(0)|BIT(1));
5435                 /* set to STATION_STATE */
5436                 pmlmeinfo->state |= MSR_INFRA;
5437                 type = MSR_INFRA;
5438                 break;
5439         case NL80211_IFTYPE_ADHOC:
5440                 type = MSR_ADHOC;
5441                 break;
5442         default:
5443                 type = MSR_NOLINK;
5444                 break;
5445         }
5446
5447         hw_var_set_opmode(padapter, type);
5448         /* Set_NETYPE0_MSR(padapter, type); */
5449
5450         return H2C_SUCCESS;
5451 }
5452
5453 int createbss_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5454 {
5455         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5456         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5457         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
5458         const struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
5459         /* u32  initialgain; */
5460
5461         if (pparm->ifmode == NL80211_IFTYPE_AP ||
5462             pparm->ifmode == NL80211_IFTYPE_P2P_GO) {
5463 #ifdef CONFIG_8723AU_AP_MODE
5464                 if (pmlmeinfo->state == MSR_AP) {
5465                         /* todo: */
5466                         return H2C_SUCCESS;
5467                 }
5468 #endif
5469         }
5470
5471         /* below is for ad-hoc master */
5472         if (pparm->ifmode == NL80211_IFTYPE_ADHOC) {
5473                 rtw_joinbss_reset23a(padapter);
5474
5475                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
5476                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
5477                 pmlmeinfo->ERP_enable = 0;
5478                 pmlmeinfo->WMM_enable = 0;
5479                 pmlmeinfo->HT_enable = 0;
5480                 pmlmeinfo->HT_caps_enable = 0;
5481                 pmlmeinfo->HT_info_enable = 0;
5482
5483                 /* disable dynamic functions, such as high power, DIG */
5484                 rtl8723a_odm_support_ability_backup(padapter);
5485
5486                 rtl8723a_odm_support_ability_clr(padapter,
5487                                                  DYNAMIC_FUNC_DISABLE);
5488
5489                 /* cancel link timer */
5490                 del_timer_sync(&pmlmeext->link_timer);
5491
5492                 /* clear CAM */
5493                 flush_all_cam_entry23a(padapter);
5494
5495                 if (pparm->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
5496                         return H2C_PARAMETERS_ERROR;
5497
5498                 memcpy(pnetwork, pparm, sizeof(struct wlan_bssid_ex));
5499
5500                 start_create_ibss(padapter);
5501         }
5502
5503         return H2C_SUCCESS;
5504 }
5505
5506 int join_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5507 {
5508         struct registry_priv *pregpriv = &padapter->registrypriv;
5509         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5510         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5511         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
5512         const struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
5513         struct ieee80211_ht_operation *pht_info;
5514         u32 i;
5515         u8 *p;
5516         /* u32  initialgain; */
5517         /* u32  acparm; */
5518
5519         /* check already connecting to AP or not */
5520         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
5521                 if (pmlmeinfo->state & MSR_INFRA)
5522                         issue_deauth_ex(padapter, pnetwork->MacAddress,
5523                                         WLAN_REASON_DEAUTH_LEAVING, 5, 100);
5524
5525                 pmlmeinfo->state = MSR_NOLINK;
5526
5527                 /* clear CAM */
5528                 flush_all_cam_entry23a(padapter);
5529
5530                 del_timer_sync(&pmlmeext->link_timer);
5531
5532                 /* set MSR to nolink -> infra. mode */
5533                 rtl8723a_set_media_status(padapter, MSR_INFRA);
5534
5535                 hw_var_set_mlme_disconnect(padapter);
5536         }
5537
5538         rtw_joinbss_reset23a(padapter);
5539
5540         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
5541         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
5542         pmlmeinfo->ERP_enable = 0;
5543         pmlmeinfo->WMM_enable = 0;
5544         pmlmeinfo->HT_enable = 0;
5545         pmlmeinfo->HT_caps_enable = 0;
5546         pmlmeinfo->HT_info_enable = 0;
5547         pmlmeinfo->bwmode_updated = false;
5548         /* pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX; */
5549
5550         if (pparm->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
5551                 return H2C_PARAMETERS_ERROR;
5552
5553         memcpy(pnetwork, pbuf, sizeof(struct wlan_bssid_ex));
5554
5555         /* Check AP vendor to move rtw_joinbss_cmd23a() */
5556         /* pmlmeinfo->assoc_AP_vendor = check_assoc_AP23a(pnetwork->IEs,
5557            pnetwork->IELength); */
5558
5559         for (i = 0; i < pnetwork->IELength;) {
5560                 p = pnetwork->IEs + i;
5561
5562                 switch (p[0]) {
5563                 case WLAN_EID_VENDOR_SPECIFIC:/* Get WMM IE. */
5564                         if (!memcmp(p + 2, WMM_OUI23A, 4))
5565                                 pmlmeinfo->WMM_enable = 1;
5566                         break;
5567
5568                 case WLAN_EID_HT_CAPABILITY:    /* Get HT Cap IE. */
5569                         pmlmeinfo->HT_caps_enable = 1;
5570                         break;
5571
5572                 case WLAN_EID_HT_OPERATION:     /* Get HT Info IE. */
5573                         pmlmeinfo->HT_info_enable = 1;
5574
5575                         /* spec case only for cisco's ap because cisco's ap
5576                          * issue assoc rsp using mcs rate @40MHz or @20MHz */
5577                         pht_info = (struct ieee80211_ht_operation *)(p + 2);
5578
5579                         if (pregpriv->cbw40_enable &&
5580                             (pht_info->ht_param &
5581                              IEEE80211_HT_PARAM_CHAN_WIDTH_ANY)) {
5582                                 /* switch to the 40M Hz mode according to AP */
5583                                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
5584                                 switch (pht_info->ht_param &
5585                                         IEEE80211_HT_PARAM_CHA_SEC_OFFSET) {
5586                                 case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
5587                                         pmlmeext->cur_ch_offset =
5588                                                 HAL_PRIME_CHNL_OFFSET_LOWER;
5589                                         break;
5590
5591                                 case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
5592                                         pmlmeext->cur_ch_offset =
5593                                                 HAL_PRIME_CHNL_OFFSET_UPPER;
5594                                         break;
5595
5596                                 default:
5597                                         pmlmeext->cur_ch_offset =
5598                                                 HAL_PRIME_CHNL_OFFSET_DONT_CARE;
5599                                         break;
5600                                 }
5601
5602                                 DBG_8723A("set ch/bw before connected\n");
5603                         }
5604                         break;
5605
5606                 default:
5607                         break;
5608                 }
5609
5610                 i += (p[1] + 2);
5611         }
5612
5613         hw_var_set_bssid(padapter, pmlmeinfo->network.MacAddress);
5614         hw_var_set_mlme_join(padapter, 0);
5615
5616         /* cancel link timer */
5617         del_timer_sync(&pmlmeext->link_timer);
5618
5619         start_clnt_join(padapter);
5620
5621         return H2C_SUCCESS;
5622 }
5623
5624 int disconnect_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5625 {
5626         const struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
5627         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5628         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5629         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
5630
5631         if (is_client_associated_to_ap23a(padapter)) {
5632                 issue_deauth_ex(padapter, pnetwork->MacAddress,
5633                                 WLAN_REASON_DEAUTH_LEAVING,
5634                                 param->deauth_timeout_ms/100, 100);
5635         }
5636
5637         /* set_opmode_cmd(padapter, infra_client_with_mlme); */
5638
5639         /* pmlmeinfo->state = MSR_NOLINK; */
5640
5641         hw_var_set_mlme_disconnect(padapter);
5642         hw_var_set_bssid(padapter, null_addr);
5643
5644         /* restore to initial setting. */
5645         update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode);
5646
5647         if ((pmlmeinfo->state & 0x03) == MSR_ADHOC ||
5648             (pmlmeinfo->state & 0x03) == MSR_AP)
5649                 rtl8723a_set_bcn_func(padapter, 0);     /* Stop BCN */
5650
5651         /* set MSR to no link state -> infra. mode */
5652         rtl8723a_set_media_status(padapter, MSR_INFRA);
5653
5654         pmlmeinfo->state = MSR_NOLINK;
5655
5656         /* switch to the 20M Hz mode after disconnect */
5657         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
5658         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
5659
5660         set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
5661                               pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
5662
5663         flush_all_cam_entry23a(padapter);
5664
5665         del_timer_sync(&pmlmeext->link_timer);
5666
5667         rtw_free_uc_swdec_pending_queue23a(padapter);
5668
5669         return H2C_SUCCESS;
5670 }
5671
5672 static int
5673 rtw_scan_ch_decision(struct rtw_adapter *padapter,
5674                      struct rtw_ieee80211_channel *out, u32 out_num,
5675                      const struct rtw_ieee80211_channel *in, u32 in_num)
5676 {
5677         int i, j;
5678         int scan_ch_num = 0;
5679         int set_idx;
5680         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5681
5682         /* clear out first */
5683         memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num);
5684
5685         /* acquire channels from in */
5686         j = 0;
5687         for (i = 0;i<in_num;i++) {
5688                 if (in[i].hw_value &&
5689                     !(in[i].flags & IEEE80211_CHAN_DISABLED) &&
5690                     (set_idx = rtw_ch_set_search_ch23a(pmlmeext->channel_set,
5691                                                        in[i].hw_value)) >= 0) {
5692                         memcpy(&out[j], &in[i],
5693                                sizeof(struct rtw_ieee80211_channel));
5694
5695                         if (pmlmeext->channel_set[set_idx].ScanType ==
5696                             SCAN_PASSIVE)
5697                                 out[j].flags &= IEEE80211_CHAN_NO_IR;
5698
5699                         j++;
5700                 }
5701                 if (j>= out_num)
5702                         break;
5703         }
5704
5705         /* if out is empty, use channel_set as default */
5706         if (j == 0) {
5707                 for (i = 0;i<pmlmeext->max_chan_nums;i++) {
5708                         out[i].hw_value = pmlmeext->channel_set[i].ChannelNum;
5709
5710                         if (pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
5711                                 out[i].flags &= IEEE80211_CHAN_NO_IR;
5712
5713                         j++;
5714                 }
5715         }
5716
5717         if (padapter->setband == GHZ_24) {                      /*  2.4G */
5718                 for (i = 0; i < j ; i++) {
5719                         if (out[i].hw_value > 35)
5720                                 memset(&out[i], 0,
5721                                        sizeof(struct rtw_ieee80211_channel));
5722                         else
5723                                 scan_ch_num++;
5724                 }
5725                 j = scan_ch_num;
5726         } else if  (padapter->setband == GHZ_50) {              /*  5G */
5727                 for (i = 0; i < j ; i++) {
5728                         if (out[i].hw_value > 35) {
5729                                 memcpy(&out[scan_ch_num++], &out[i],
5730                                        sizeof(struct rtw_ieee80211_channel));
5731                         }
5732                 }
5733                 j = scan_ch_num;
5734         } else
5735                 {}
5736
5737         return j;
5738 }
5739
5740 int sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5741 {
5742         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5743         const struct sitesurvey_parm *pparm = (struct sitesurvey_parm *)pbuf;
5744         u8 bdelayscan = false;
5745         u32 initialgain;
5746         u32 i;
5747
5748         if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE) {
5749                 pmlmeext->sitesurvey_res.state = SCAN_START;
5750                 pmlmeext->sitesurvey_res.bss_cnt = 0;
5751                 pmlmeext->sitesurvey_res.channel_idx = 0;
5752
5753                 for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
5754                         if (pparm->ssid[i].ssid_len) {
5755                                 memcpy(pmlmeext->sitesurvey_res.ssid[i].ssid,
5756                                        pparm->ssid[i].ssid,
5757                                        IEEE80211_MAX_SSID_LEN);
5758                                 pmlmeext->sitesurvey_res.ssid[i].ssid_len =
5759                                         pparm->ssid[i].ssid_len;
5760                         } else {
5761                                 pmlmeext->sitesurvey_res.ssid[i].ssid_len = 0;
5762                         }
5763                 }
5764
5765                 pmlmeext->sitesurvey_res.ch_num =
5766                         rtw_scan_ch_decision(padapter,
5767                                              pmlmeext->sitesurvey_res.ch,
5768                                              RTW_CHANNEL_SCAN_AMOUNT,
5769                                              pparm->ch, pparm->ch_num);
5770
5771                 pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
5772
5773                 /* issue null data if associating to the AP */
5774                 if (is_client_associated_to_ap23a(padapter)) {
5775                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
5776
5777                         /* switch to correct channel of current network
5778                            before issue keep-alive frames */
5779                         if (rtw_get_oper_ch23a(padapter) !=
5780                             pmlmeext->cur_channel)
5781                                 SelectChannel23a(padapter,
5782                                                  pmlmeext->cur_channel);
5783
5784                         issue_nulldata23a(padapter, NULL, 1, 3, 500);
5785
5786                         bdelayscan = true;
5787                 }
5788
5789                 if (bdelayscan) {
5790                         /* delay 50ms to protect nulldata(1). */
5791                         set_survey_timer(pmlmeext, 50);
5792                         return H2C_SUCCESS;
5793                 }
5794         }
5795
5796         if (pmlmeext->sitesurvey_res.state == SCAN_START ||
5797             pmlmeext->sitesurvey_res.state == SCAN_TXNULL) {
5798                 /* disable dynamic functions, such as high power, DIG */
5799                 rtl8723a_odm_support_ability_backup(padapter);
5800                 rtl8723a_odm_support_ability_clr(padapter,
5801                                                  DYNAMIC_FUNC_DISABLE);
5802
5803                 /* config the initial gain under scanning, need to
5804                    write the BB registers */
5805                 if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled == true)
5806                         initialgain = 0x30;
5807                 else
5808                         initialgain = 0x1E;
5809
5810                 rtl8723a_set_initial_gain(padapter, initialgain);
5811
5812                 /* set MSR to no link state */
5813                 rtl8723a_set_media_status(padapter, MSR_NOLINK);
5814
5815                 rtl8723a_mlme_sitesurvey(padapter, 1);
5816
5817                 pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
5818         }
5819
5820         rtw_site_survey(padapter);
5821
5822         return H2C_SUCCESS;
5823 }
5824
5825 int setauth_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5826 {
5827         const struct setauth_parm *pparm = (struct setauth_parm *)pbuf;
5828         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5829         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5830
5831         if (pparm->mode < 4)
5832                 pmlmeinfo->auth_algo = pparm->mode;
5833
5834         return H2C_SUCCESS;
5835 }
5836
5837 int setkey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5838 {
5839         unsigned short ctrl;
5840         const struct setkey_parm *pparm = (struct setkey_parm *)pbuf;
5841         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5842         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5843         unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
5844
5845         /* main tx key for wep. */
5846         if (pparm->set_tx)
5847                 pmlmeinfo->key_index = pparm->keyid;
5848
5849         /* write cam */
5850         ctrl = BIT(15) | (pparm->algorithm) << 2 | pparm->keyid;
5851
5852         DBG_8723A_LEVEL(_drv_always_, "set group key to hw: alg:%d(WEP40-1 "
5853                         "WEP104-5 TKIP-2 AES-4) keyid:%d\n",
5854                         pparm->algorithm, pparm->keyid);
5855         rtl8723a_cam_write(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
5856
5857         /* allow multicast packets to driver */
5858         rtl8723a_on_rcr_am(padapter);
5859
5860         return H2C_SUCCESS;
5861 }
5862
5863 int set_stakey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5864 {
5865         u16 ctrl = 0;
5866         u8 cam_id;/* cam_entry */
5867         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5868         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5869         const struct set_stakey_parm *pparm = (struct set_stakey_parm *)pbuf;
5870
5871         /* cam_entry: */
5872         /* 0~3 for default key */
5873
5874         /* for concurrent mode (ap+sta): */
5875         /* default key is disable, using sw encrypt/decrypt */
5876         /* cam_entry = 4  for sta mode (macid = 0) */
5877         /* cam_entry(macid+3) = 5 ~ N for ap mode (aid = 1~N, macid = 2 ~N) */
5878
5879         /* for concurrent mode (sta+sta): */
5880         /* default key is disable, using sw encrypt/decrypt */
5881         /* cam_entry = 4 mapping to macid = 0 */
5882         /* cam_entry = 5 mapping to macid = 2 */
5883
5884         cam_id = 4;
5885
5886         DBG_8723A_LEVEL(_drv_always_, "set pairwise key to hw: alg:%d(WEP40-1 "
5887                         "WEP104-5 TKIP-2 AES-4) camid:%d\n",
5888                         pparm->algorithm, cam_id);
5889         if ((pmlmeinfo->state & 0x03) == MSR_AP) {
5890                 struct sta_info *psta;
5891                 struct sta_priv *pstapriv = &padapter->stapriv;
5892
5893                 if (pparm->algorithm == 0) {    /*  clear cam entry */
5894                         clear_cam_entry23a(padapter, pparm->id);
5895                         return H2C_SUCCESS_RSP;
5896                 }
5897
5898                 psta = rtw_get_stainfo23a(pstapriv, pparm->addr);
5899                 if (psta) {
5900                         ctrl = BIT(15) | (pparm->algorithm << 2);
5901
5902                         DBG_8723A("r871x_set_stakey_hdl23a(): enc_algorithm "
5903                                   "=%d\n", pparm->algorithm);
5904
5905                         if (psta->mac_id < 1 || psta->mac_id > (NUM_STA - 4)) {
5906                                 DBG_8723A("r871x_set_stakey_hdl23a():set_stakey"
5907                                           " failed, mac_id(aid) =%d\n",
5908                                           psta->mac_id);
5909                                 return H2C_REJECTED;
5910                         }
5911
5912                         /* 0~3 for default key, cmd_id = macid + 3,
5913                            macid = aid+1; */
5914                         cam_id = psta->mac_id + 3;
5915
5916                         DBG_8723A("Write CAM, mac_addr =%pM, "
5917                                   "cam_entry =%d\n", pparm->addr, cam_id);
5918
5919                         rtl8723a_cam_write(padapter, cam_id, ctrl,
5920                                            pparm->addr, pparm->key);
5921
5922                         return H2C_SUCCESS_RSP;
5923                 } else {
5924                         DBG_8723A("r871x_set_stakey_hdl23a(): sta has been "
5925                                   "free\n");
5926                         return H2C_REJECTED;
5927                 }
5928         }
5929
5930         /* below for sta mode */
5931
5932         if (pparm->algorithm == 0) {    /*  clear cam entry */
5933                 clear_cam_entry23a(padapter, pparm->id);
5934                 return H2C_SUCCESS;
5935         }
5936
5937         ctrl = BIT(15) | (pparm->algorithm << 2);
5938
5939         rtl8723a_cam_write(padapter, cam_id, ctrl, pparm->addr, pparm->key);
5940
5941         pmlmeinfo->enc_algo = pparm->algorithm;
5942
5943         return H2C_SUCCESS;
5944 }
5945
5946 int add_ba_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5947 {
5948         const struct addBaReq_parm *pparm = (struct addBaReq_parm *)pbuf;
5949         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5950         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5951         struct sta_info *psta;
5952
5953         psta = rtw_get_stainfo23a(&padapter->stapriv, pparm->addr);
5954
5955         if (!psta)
5956                 return H2C_SUCCESS;
5957
5958         if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) &&
5959              pmlmeinfo->HT_enable) ||
5960             (pmlmeinfo->state & 0x03) == MSR_AP) {
5961                 issue_action_BA23a(padapter, pparm->addr,
5962                                    WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
5963                 mod_timer(&psta->addba_retry_timer,
5964                           jiffies + msecs_to_jiffies(ADDBA_TO));
5965         } else
5966                 psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
5967
5968         return H2C_SUCCESS;
5969 }
5970
5971 int set_tx_beacon_cmd23a(struct rtw_adapter *padapter)
5972 {
5973         struct cmd_obj *ph2c;
5974         struct Tx_Beacon_param *ptxBeacon_parm;
5975         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
5976         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5977         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5978         u8 res = _SUCCESS;
5979         int len_diff = 0;
5980
5981         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
5982         if (!ph2c) {
5983                 res = _FAIL;
5984                 goto exit;
5985         }
5986
5987         ptxBeacon_parm = kzalloc(sizeof(struct Tx_Beacon_param), GFP_ATOMIC);
5988         if (!ptxBeacon_parm) {
5989                 kfree(ph2c);
5990                 res = _FAIL;
5991                 goto exit;
5992         }
5993
5994         memcpy(&ptxBeacon_parm->network, &pmlmeinfo->network,
5995                sizeof(struct wlan_bssid_ex));
5996
5997         len_diff = update_hidden_ssid(ptxBeacon_parm->network.IEs,
5998                                       ptxBeacon_parm->network.IELength,
5999                                       pmlmeinfo->hidden_ssid_mode);
6000         ptxBeacon_parm->network.IELength += len_diff;
6001
6002         init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm,
6003                                    GEN_CMD_CODE(_TX_Beacon));
6004
6005         res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
6006
6007 exit:
6008         return res;
6009 }
6010
6011 int mlme_evt_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6012 {
6013         u8 evt_code, evt_seq;
6014         u16 evt_sz;
6015         const struct C2HEvent_Header *c2h;
6016         void (*event_callback)(struct rtw_adapter *dev, const u8 *pbuf);
6017
6018         c2h = (struct C2HEvent_Header *)pbuf;
6019         evt_sz = c2h->len;
6020         evt_seq = c2h->seq;
6021         evt_code = c2h->ID;
6022
6023         /*  checking if event code is valid */
6024         if (evt_code >= MAX_C2HEVT) {
6025                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
6026                          "Event Code(%d) mismatch!\n", evt_code);
6027                 goto _abort_event_;
6028         }
6029
6030         /*  checking if event size match the event parm size */
6031         if (wlanevents[evt_code].parmsize != 0 &&
6032             wlanevents[evt_code].parmsize != evt_sz) {
6033                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
6034                          "Event(%d) Parm Size mismatch (%d vs %d)!\n",
6035                          evt_code, wlanevents[evt_code].parmsize, evt_sz);
6036                 goto _abort_event_;
6037         }
6038
6039         event_callback = wlanevents[evt_code].event_callback;
6040         event_callback(padapter, pbuf + sizeof(struct C2HEvent_Header));
6041
6042 _abort_event_:
6043
6044         return H2C_SUCCESS;
6045 }
6046
6047 int h2c_msg_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6048 {
6049         if (!pbuf)
6050                 return H2C_PARAMETERS_ERROR;
6051
6052         return H2C_SUCCESS;
6053 }
6054
6055 int tx_beacon_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6056 {
6057         if (send_beacon23a(padapter) == _FAIL) {
6058                 DBG_8723A("issue_beacon23a, fail!\n");
6059                 return H2C_PARAMETERS_ERROR;
6060         }
6061 #ifdef CONFIG_8723AU_AP_MODE
6062         else { /* tx bc/mc frames after update TIM */
6063                 struct sta_info *psta_bmc;
6064                 struct list_head *phead;
6065                 struct xmit_frame *pxmitframe, *ptmp;
6066                 struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
6067                 struct sta_priv *pstapriv = &padapter->stapriv;
6068
6069                 /* for BC/MC Frames */
6070                 psta_bmc = rtw_get_bcmc_stainfo23a(padapter);
6071                 if (!psta_bmc)
6072                         return H2C_SUCCESS;
6073
6074                 if (pstapriv->tim_bitmap & BIT(0) && psta_bmc->sleepq_len > 0) {
6075                         msleep(10);/*  10ms, ATIM(HIQ) Windows */
6076                         /* spin_lock_bh(&psta_bmc->sleep_q.lock); */
6077                         spin_lock_bh(&pxmitpriv->lock);
6078
6079                         phead = get_list_head(&psta_bmc->sleep_q);
6080
6081                         list_for_each_entry_safe(pxmitframe, ptmp,
6082                                                  phead, list) {
6083
6084                                 list_del_init(&pxmitframe->list);
6085
6086                                 psta_bmc->sleepq_len--;
6087                                 if (psta_bmc->sleepq_len>0)
6088                                         pxmitframe->attrib.mdata = 1;
6089                                 else
6090                                         pxmitframe->attrib.mdata = 0;
6091
6092                                 pxmitframe->attrib.triggered = 1;
6093
6094                                 pxmitframe->attrib.qsel = 0x11;/* HIQ */
6095
6096                                 rtl8723au_hal_xmitframe_enqueue(padapter,
6097                                                                 pxmitframe);
6098                         }
6099                         /* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
6100                         spin_unlock_bh(&pxmitpriv->lock);
6101                 }
6102         }
6103 #endif
6104
6105         return H2C_SUCCESS;
6106 }
6107
6108 int set_ch_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6109 {
6110         const struct set_ch_parm *set_ch_parm;
6111         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
6112
6113         if (!pbuf)
6114                 return H2C_PARAMETERS_ERROR;
6115
6116         set_ch_parm = (struct set_ch_parm *)pbuf;
6117
6118         DBG_8723A("%s(%s): ch:%u, bw:%u, ch_offset:%u\n", __func__,
6119                   padapter->pnetdev->name, set_ch_parm->ch,
6120                   set_ch_parm->bw, set_ch_parm->ch_offset);
6121
6122         pmlmeext->cur_channel = set_ch_parm->ch;
6123         pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
6124         pmlmeext->cur_bwmode = set_ch_parm->bw;
6125
6126         set_channel_bwmode23a(padapter, set_ch_parm->ch,
6127                               set_ch_parm->ch_offset, set_ch_parm->bw);
6128
6129         return H2C_SUCCESS;
6130 }
6131
6132 int set_chplan_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6133 {
6134         const struct SetChannelPlan_param *setChannelPlan_param;
6135         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
6136
6137         if (!pbuf)
6138                 return H2C_PARAMETERS_ERROR;
6139
6140         setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
6141
6142         pmlmeext->max_chan_nums =
6143                 init_channel_set(padapter, setChannelPlan_param->channel_plan,
6144                                  pmlmeext->channel_set);
6145         init_channel_list(padapter, pmlmeext->channel_set,
6146                           pmlmeext->max_chan_nums, &pmlmeext->channel_list);
6147
6148         return H2C_SUCCESS;
6149 }
6150
6151 int led_blink_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6152 {
6153         struct LedBlink_param *ledBlink_param;
6154
6155         if (!pbuf)
6156                 return H2C_PARAMETERS_ERROR;
6157
6158         ledBlink_param = (struct LedBlink_param *)pbuf;
6159
6160         return H2C_SUCCESS;
6161 }
6162
6163 int set_csa_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6164 {
6165         return H2C_REJECTED;
6166 }
6167
6168 /*  TDLS_WRCR           : write RCR DATA BIT */
6169 /*  TDLS_SD_PTI         : issue peer traffic indication */
6170 /*  TDLS_CS_OFF         : go back to the channel linked with AP,
6171                           terminating channel switch procedure */
6172 /*  TDLS_INIT_CH_SEN    : init channel sensing, receive all data and
6173                           mgnt frame */
6174 /*  TDLS_DONE_CH_SEN    : channel sensing and report candidate channel */
6175 /*  TDLS_OFF_CH         : first time set channel to off channel */
6176 /*  TDLS_BASE_CH        : go back tp the channel linked with AP when set
6177                           base channel as target channel */
6178 /*  TDLS_P_OFF_CH       : periodically go to off channel */
6179 /*  TDLS_P_BASE_CH      : periodically go back to base channel */
6180 /*  TDLS_RS_RCR         : restore RCR */
6181 /*  TDLS_CKALV_PH1      : check alive timer phase1 */
6182 /*  TDLS_CKALV_PH2      : check alive timer phase2 */
6183 /*  TDLS_FREE_STA       : free tdls sta */
6184 int tdls_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6185 {
6186         return H2C_REJECTED;
6187 }