kernel/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                                                       IEEE80211_BAND_2GHZ);
2117         else
2118                 freq = ieee80211_channel_to_frequency(channel,
2119                                                       IEEE80211_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;
2158              i < sizeof(OnAction23a_tbl) / sizeof(struct action_handler); i++) {
2159                 ptable = &OnAction23a_tbl[i];
2160
2161                 if (category == ptable->num)
2162                         ptable->func(padapter, precv_frame);
2163         }
2164
2165         return _SUCCESS;
2166 }
2167
2168 static int DoReserved23a(struct rtw_adapter *padapter,
2169                          struct recv_frame *precv_frame)
2170 {
2171         return _SUCCESS;
2172 }
2173
2174 struct xmit_frame *alloc_mgtxmitframe23a(struct xmit_priv *pxmitpriv)
2175 {
2176         struct xmit_frame *pmgntframe;
2177         struct xmit_buf *pxmitbuf;
2178
2179         pmgntframe = rtw_alloc_xmitframe23a_ext(pxmitpriv);
2180
2181         if (!pmgntframe) {
2182                 DBG_8723A("%s(%s): alloc xmitframe fail\n", __func__,
2183                           pxmitpriv->adapter->pnetdev->name);
2184                 goto exit;
2185         }
2186
2187         pxmitbuf = rtw_alloc_xmitbuf23a_ext(pxmitpriv);
2188         if (!pxmitbuf) {
2189                 DBG_8723A("%s(%s): alloc xmitbuf fail\n", __func__,
2190                           pxmitpriv->adapter->pnetdev->name);
2191                 rtw_free_xmitframe23a(pxmitpriv, pmgntframe);
2192                 pmgntframe = NULL;
2193                 goto exit;
2194         }
2195
2196         pmgntframe->frame_tag = MGNT_FRAMETAG;
2197         pmgntframe->pxmitbuf = pxmitbuf;
2198         pmgntframe->buf_addr = pxmitbuf->pbuf;
2199         pxmitbuf->priv_data = pmgntframe;
2200
2201 exit:
2202         return pmgntframe;
2203 }
2204
2205 /****************************************************************************
2206
2207 Following are some TX functions for WiFi MLME
2208
2209 *****************************************************************************/
2210
2211 void update_mgnt_tx_rate23a(struct rtw_adapter *padapter, u8 rate)
2212 {
2213         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2214
2215         pmlmeext->tx_rate = rate;
2216         DBG_8723A("%s(): rate = %x\n", __func__, rate);
2217 }
2218
2219 void update_mgntframe_attrib23a(struct rtw_adapter *padapter,
2220                                 struct pkt_attrib *pattrib)
2221 {
2222         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2223
2224         memset((u8 *)pattrib, 0, sizeof(struct pkt_attrib));
2225
2226         pattrib->hdrlen = 24;
2227         pattrib->nr_frags = 1;
2228         pattrib->priority = 7;
2229         pattrib->mac_id = 0;
2230         pattrib->qsel = 0x12;
2231
2232         pattrib->pktlen = 0;
2233
2234         if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
2235                 pattrib->raid = 6;/* b mode */
2236         else
2237                 pattrib->raid = 5;/* a/g mode */
2238
2239         pattrib->encrypt = 0;
2240         pattrib->bswenc = false;
2241
2242         pattrib->qos_en = false;
2243         pattrib->ht_en = false;
2244         pattrib->bwmode = HT_CHANNEL_WIDTH_20;
2245         pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
2246         pattrib->sgi = false;
2247
2248         pattrib->seqnum = pmlmeext->mgnt_seq;
2249
2250         pattrib->retry_ctrl = true;
2251 }
2252
2253 void dump_mgntframe23a(struct rtw_adapter *padapter,
2254                        struct xmit_frame *pmgntframe)
2255 {
2256         if (padapter->bSurpriseRemoved == true ||
2257             padapter->bDriverStopped == true)
2258                 return;
2259
2260         rtl8723au_mgnt_xmit(padapter, pmgntframe);
2261 }
2262
2263 int dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
2264                                struct xmit_frame *pmgntframe, int timeout_ms)
2265 {
2266         int ret = _FAIL;
2267         unsigned long irqL;
2268         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2269         struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
2270         struct submit_ctx sctx;
2271
2272         if (padapter->bSurpriseRemoved == true ||
2273             padapter->bDriverStopped == true)
2274                 return ret;
2275
2276         rtw_sctx_init23a(&sctx, timeout_ms);
2277         pxmitbuf->sctx = &sctx;
2278
2279         ret = rtl8723au_mgnt_xmit(padapter, pmgntframe);
2280
2281         if (ret == _SUCCESS)
2282                 ret = rtw_sctx_wait23a(&sctx);
2283
2284         spin_lock_irqsave(&pxmitpriv->lock_sctx, irqL);
2285         pxmitbuf->sctx = NULL;
2286         spin_unlock_irqrestore(&pxmitpriv->lock_sctx, irqL);
2287
2288         return ret;
2289 }
2290
2291 int dump_mgntframe23a_and_wait_ack23a(struct rtw_adapter *padapter,
2292                                       struct xmit_frame *pmgntframe)
2293 {
2294         int ret = _FAIL;
2295         u32 timeout_ms = 500;/*   500ms */
2296         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2297
2298         if (padapter->bSurpriseRemoved == true ||
2299             padapter->bDriverStopped == true)
2300                 return _FAIL;
2301
2302         mutex_lock(&pxmitpriv->ack_tx_mutex);
2303         pxmitpriv->ack_tx = true;
2304
2305         pmgntframe->ack_report = 1;
2306         if (rtl8723au_mgnt_xmit(padapter, pmgntframe) == _SUCCESS)
2307                 ret = rtw_ack_tx_wait23a(pxmitpriv, timeout_ms);
2308
2309         pxmitpriv->ack_tx = false;
2310         mutex_unlock(&pxmitpriv->ack_tx_mutex);
2311
2312         return ret;
2313 }
2314
2315 static int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
2316 {
2317         u8 *ssid_ie;
2318         int ssid_len_ori;
2319         int len_diff = 0;
2320         u8 *next_ie;
2321         u32 remain_len;
2322
2323         ssid_ie = rtw_get_ie23a(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
2324
2325         /* DBG_8723A("%s hidden_ssid_mode:%u, ssid_ie:%p, ssid_len_ori:%d\n",
2326            __func__, hidden_ssid_mode, ssid_ie, ssid_len_ori); */
2327
2328         if (ssid_ie && ssid_len_ori > 0) {
2329                 switch (hidden_ssid_mode) {
2330                 case 1:
2331                         next_ie = ssid_ie + 2 + ssid_len_ori;
2332                         remain_len = ies_len -(next_ie-ies);
2333
2334                         ssid_ie[1] = 0;
2335                         memcpy(ssid_ie+2, next_ie, remain_len);
2336                         len_diff -= ssid_len_ori;
2337
2338                         break;
2339                 case 2:
2340                         memset(&ssid_ie[2], 0, ssid_len_ori);
2341                         break;
2342                 default:
2343                         break;
2344                 }
2345         }
2346
2347         return len_diff;
2348 }
2349
2350 void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
2351 {
2352         struct xmit_frame *pmgntframe;
2353         struct pkt_attrib *pattrib;
2354         unsigned char *pframe;
2355         struct ieee80211_mgmt *mgmt;
2356         unsigned int rate_len;
2357         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2358         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2359         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2360         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
2361         struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
2362         u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
2363         const u8 *wps_ie;
2364         u8 sr = 0;
2365         int len_diff;
2366
2367         /* DBG_8723A("%s\n", __func__); */
2368
2369         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
2370         if (!pmgntframe) {
2371                 DBG_8723A("%s, alloc mgnt frame fail\n", __func__);
2372                 return;
2373         }
2374 #ifdef CONFIG_8723AU_AP_MODE
2375         spin_lock_bh(&pmlmepriv->bcn_update_lock);
2376 #endif
2377
2378         /* update attribute */
2379         pattrib = &pmgntframe->attrib;
2380         update_mgntframe_attrib23a(padapter, pattrib);
2381         pattrib->qsel = 0x10;
2382
2383         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2384
2385         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2386         mgmt = (struct ieee80211_mgmt *)pframe;
2387
2388         mgmt->frame_control =
2389                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_BEACON);
2390         mgmt->seq_ctrl = 0;
2391
2392         ether_addr_copy(mgmt->da, bc_addr);
2393         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
2394         ether_addr_copy(mgmt->bssid, get_my_bssid23a(cur_network));
2395
2396         /* timestamp will be inserted by hardware */
2397
2398         put_unaligned_le16(cur_network->beacon_interval,
2399                            &mgmt->u.beacon.beacon_int);
2400
2401         put_unaligned_le16(cur_network->capability,
2402                            &mgmt->u.beacon.capab_info);
2403
2404         pframe = mgmt->u.beacon.variable;
2405         pattrib->pktlen = offsetof(struct ieee80211_mgmt, u.beacon.variable);
2406
2407         if ((pmlmeinfo->state & 0x03) == MSR_AP) {
2408                 u8 *iebuf;
2409                 int buflen;
2410                 /* DBG_8723A("ie len =%d\n", cur_network->IELength); */
2411                 memcpy(pframe, cur_network->IEs, cur_network->IELength);
2412                 len_diff = update_hidden_ssid(pframe, cur_network->IELength,
2413                                               pmlmeinfo->hidden_ssid_mode);
2414                 pframe += (cur_network->IELength+len_diff);
2415                 pattrib->pktlen += (cur_network->IELength+len_diff);
2416
2417                 iebuf = mgmt->u.beacon.variable;
2418                 buflen = pattrib->pktlen -
2419                         offsetof(struct ieee80211_mgmt, u.beacon.variable);
2420                 wps_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
2421                                                  WLAN_OUI_TYPE_MICROSOFT_WPS,
2422                                                  iebuf, buflen);
2423
2424                 if (wps_ie && wps_ie[1] > 0) {
2425                         rtw_get_wps_attr_content23a(wps_ie, wps_ie[1],
2426                                                     WPS_ATTR_SELECTED_REGISTRAR,
2427                                                     (u8 *)&sr);
2428                 }
2429                 if (sr != 0)
2430                         set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
2431                 else
2432                         _clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
2433
2434                 goto _issue_bcn;
2435         }
2436
2437         /*  SSID */
2438         pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
2439                                cur_network->Ssid.ssid_len,
2440                                cur_network->Ssid.ssid, &pattrib->pktlen);
2441
2442         /*  supported rates... */
2443         rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates);
2444         pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
2445                                ((rate_len > 8)? 8: rate_len),
2446                                cur_network->SupportedRates, &pattrib->pktlen);
2447
2448         /*  DS parameter set */
2449         pframe = rtw_set_ie23a(pframe, WLAN_EID_DS_PARAMS, 1, (unsigned char *)
2450                                &cur_network->DSConfig, &pattrib->pktlen);
2451
2452         /* if ((pmlmeinfo->state&0x03) == MSR_ADHOC) */
2453         {
2454                 u8 erpinfo = 0;
2455                 u32 ATIMWindow;
2456                 /*  IBSS Parameter Set... */
2457                 /* ATIMWindow = cur->ATIMWindow; */
2458                 ATIMWindow = 0;
2459                 pframe = rtw_set_ie23a(pframe, WLAN_EID_IBSS_PARAMS, 2,
2460                                        (unsigned char *)&ATIMWindow,
2461                                        &pattrib->pktlen);
2462
2463                 /* ERP IE */
2464                 pframe = rtw_set_ie23a(pframe, WLAN_EID_ERP_INFO, 1,
2465                                        &erpinfo, &pattrib->pktlen);
2466         }
2467
2468         /*  EXTERNDED SUPPORTED RATE */
2469         if (rate_len > 8)
2470                 pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
2471                                        rate_len - 8,
2472                                        cur_network->SupportedRates + 8,
2473                                        &pattrib->pktlen);
2474
2475         /* todo:HT for adhoc */
2476
2477 _issue_bcn:
2478
2479 #ifdef CONFIG_8723AU_AP_MODE
2480         pmlmepriv->update_bcn = false;
2481
2482         spin_unlock_bh(&pmlmepriv->bcn_update_lock);
2483 #endif
2484
2485         if ((pattrib->pktlen + TXDESC_SIZE) > 512) {
2486                 DBG_8723A("beacon frame too large\n");
2487                 return;
2488         }
2489
2490         pattrib->last_txcmdsz = pattrib->pktlen;
2491
2492         /* DBG_8723A("issue bcn_sz =%d\n", pattrib->last_txcmdsz); */
2493         if (timeout_ms > 0)
2494                 dump_mgntframe23a_and_wait(padapter, pmgntframe, timeout_ms);
2495         else
2496                 dump_mgntframe23a(padapter, pmgntframe);
2497 }
2498
2499 static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da)
2500 {
2501         struct xmit_frame *pmgntframe;
2502         struct pkt_attrib *pattrib;
2503         unsigned char *pframe;
2504         struct ieee80211_mgmt *mgmt;
2505         unsigned char *mac, *bssid;
2506         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2507 #ifdef CONFIG_8723AU_AP_MODE
2508         const u8 *pwps_ie;
2509         u8 *ssid_ie;
2510         int ssid_ielen;
2511         int ssid_ielen_diff;
2512         u8 buf[MAX_IE_SZ];
2513 #endif
2514         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2515         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
2516         struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
2517         unsigned int rate_len;
2518
2519         /* DBG_8723A("%s\n", __func__); */
2520
2521         if (cur_network->IELength > MAX_IE_SZ)
2522                 return;
2523
2524         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
2525         if (!pmgntframe) {
2526                 DBG_8723A("%s, alloc mgnt frame fail\n", __func__);
2527                 return;
2528         }
2529
2530         /* update attribute */
2531         pattrib = &pmgntframe->attrib;
2532         update_mgntframe_attrib23a(padapter, pattrib);
2533
2534         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2535
2536         pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
2537         mgmt = (struct ieee80211_mgmt *)pframe;
2538
2539         mac = myid(&padapter->eeprompriv);
2540         bssid = cur_network->MacAddress;
2541
2542         mgmt->frame_control =
2543                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_RESP);
2544
2545         ether_addr_copy(mgmt->da, da);
2546         ether_addr_copy(mgmt->sa, mac);
2547         ether_addr_copy(mgmt->bssid, bssid);
2548
2549         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
2550         pmlmeext->mgnt_seq++;
2551
2552         pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
2553
2554         /* timestamp will be inserted by hardware */
2555         put_unaligned_le16(cur_network->beacon_interval,
2556                            &mgmt->u.probe_resp.beacon_int);
2557
2558         put_unaligned_le16(cur_network->capability,
2559                            &mgmt->u.probe_resp.capab_info);
2560
2561         pframe = mgmt->u.probe_resp.variable;
2562         pattrib->pktlen =
2563                 offsetof(struct ieee80211_mgmt, u.probe_resp.variable);
2564
2565         /* below for ad-hoc mode */
2566
2567 #ifdef CONFIG_8723AU_AP_MODE
2568         if ((pmlmeinfo->state & 0x03) == MSR_AP) {
2569                 pwps_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
2570                                                   WLAN_OUI_TYPE_MICROSOFT_WPS,
2571                                                   cur_network->IEs,
2572                                                   cur_network->IELength);
2573
2574                 memcpy(pframe, cur_network->IEs, cur_network->IELength);
2575                 pframe += cur_network->IELength;
2576                 pattrib->pktlen += cur_network->IELength;
2577
2578                 /* retrieve SSID IE from cur_network->Ssid */
2579
2580                 ssid_ie = rtw_get_ie23a(mgmt->u.probe_resp.variable,
2581                                         WLAN_EID_SSID, &ssid_ielen,
2582                                         pframe - mgmt->u.probe_resp.variable);
2583
2584                 ssid_ielen_diff = cur_network->Ssid.ssid_len - ssid_ielen;
2585
2586                 if (ssid_ie && cur_network->Ssid.ssid_len) {
2587                         uint remainder_ielen;
2588                         u8 *remainder_ie;
2589
2590                         remainder_ie = ssid_ie + 2;
2591
2592                         remainder_ielen = pframe - remainder_ie;
2593
2594                         DBG_8723A_LEVEL(_drv_warning_, "%s(%s): "
2595                                         "remainder_ielen > MAX_IE_SZ\n",
2596                                         __func__, padapter->pnetdev->name);
2597                         if (remainder_ielen > MAX_IE_SZ)
2598                                 remainder_ielen = MAX_IE_SZ;
2599
2600                         memcpy(buf, remainder_ie, remainder_ielen);
2601                         memcpy(remainder_ie + ssid_ielen_diff, buf,
2602                                remainder_ielen);
2603                         *(ssid_ie + 1) = cur_network->Ssid.ssid_len;
2604                         memcpy(ssid_ie + 2, cur_network->Ssid.ssid,
2605                                cur_network->Ssid.ssid_len);
2606
2607                         pframe += ssid_ielen_diff;
2608                         pattrib->pktlen += ssid_ielen_diff;
2609                 }
2610         } else
2611 #endif
2612         {
2613                 /*  SSID */
2614                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
2615                                        cur_network->Ssid.ssid_len,
2616                                        cur_network->Ssid.ssid,
2617                                        &pattrib->pktlen);
2618
2619                 /*  supported rates... */
2620                 rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates);
2621                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
2622                                        ((rate_len > 8)? 8: rate_len),
2623                                        cur_network->SupportedRates,
2624                                        &pattrib->pktlen);
2625
2626                 /*  DS parameter set */
2627                 pframe = rtw_set_ie23a(pframe, WLAN_EID_DS_PARAMS, 1,
2628                                        (unsigned char *)&cur_network->DSConfig,
2629                                        &pattrib->pktlen);
2630
2631                 if ((pmlmeinfo->state & 0x03) == MSR_ADHOC) {
2632                         u8 erpinfo = 0;
2633                         u32 ATIMWindow;
2634                         /*  IBSS Parameter Set... */
2635                         /* ATIMWindow = cur->ATIMWindow; */
2636                         ATIMWindow = 0;
2637                         pframe = rtw_set_ie23a(pframe, WLAN_EID_IBSS_PARAMS, 2,
2638                                                (unsigned char *)&ATIMWindow,
2639                                                &pattrib->pktlen);
2640
2641                         /* ERP IE */
2642                         pframe = rtw_set_ie23a(pframe, WLAN_EID_ERP_INFO, 1,
2643                                                &erpinfo, &pattrib->pktlen);
2644                 }
2645
2646                 /*  EXTERNDED SUPPORTED RATE */
2647                 if (rate_len > 8)
2648                         pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
2649                                                rate_len - 8,
2650                                                cur_network->SupportedRates + 8,
2651                                                &pattrib->pktlen);
2652
2653                 /* todo:HT for adhoc */
2654         }
2655
2656         pattrib->last_txcmdsz = pattrib->pktlen;
2657
2658         dump_mgntframe23a(padapter, pmgntframe);
2659
2660         return;
2661 }
2662
2663 static int _issue_probereq(struct rtw_adapter *padapter,
2664                            struct cfg80211_ssid *pssid, u8 *da, int wait_ack)
2665 {
2666         int ret = _FAIL;
2667         struct xmit_frame *pmgntframe;
2668         struct pkt_attrib *pattrib;
2669         unsigned char *pframe;
2670         struct ieee80211_hdr *pwlanhdr;
2671         unsigned char *mac;
2672         unsigned char bssrate[NumRates];
2673         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2674         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2675         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2676         int bssrate_len = 0;
2677         u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
2678
2679         RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
2680                  "+%s\n", __func__);
2681
2682         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
2683         if (!pmgntframe)
2684                 goto exit;
2685
2686         /* update attribute */
2687         pattrib = &pmgntframe->attrib;
2688         update_mgntframe_attrib23a(padapter, pattrib);
2689
2690         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2691
2692         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2693         pwlanhdr = (struct ieee80211_hdr *)pframe;
2694
2695         mac = myid(&padapter->eeprompriv);
2696
2697         pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
2698                                               IEEE80211_STYPE_PROBE_REQ);
2699
2700         if (da) {
2701                 /*      unicast probe request frame */
2702                 ether_addr_copy(pwlanhdr->addr1, da);
2703                 ether_addr_copy(pwlanhdr->addr3, da);
2704         } else {
2705                 /*      broadcast probe request frame */
2706                 ether_addr_copy(pwlanhdr->addr1, bc_addr);
2707                 ether_addr_copy(pwlanhdr->addr3, bc_addr);
2708         }
2709
2710         ether_addr_copy(pwlanhdr->addr2, mac);
2711
2712         pwlanhdr->seq_ctrl =
2713                 cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
2714
2715         pmlmeext->mgnt_seq++;
2716
2717         pframe += sizeof (struct ieee80211_hdr_3addr);
2718         pattrib->pktlen = sizeof (struct ieee80211_hdr_3addr);
2719
2720         if (pssid)
2721                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID, pssid->ssid_len,
2722                                        pssid->ssid, &pattrib->pktlen);
2723         else
2724                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID, 0, NULL,
2725                                        &pattrib->pktlen);
2726
2727         get_rate_set23a(padapter, bssrate, &bssrate_len);
2728
2729         if (bssrate_len > 8) {
2730                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
2731                                        bssrate, &pattrib->pktlen);
2732                 pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
2733                                        (bssrate_len - 8), (bssrate + 8),
2734                                        &pattrib->pktlen);
2735         } else {
2736                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
2737                                        bssrate_len, bssrate, &pattrib->pktlen);
2738         }
2739
2740         /* add wps_ie for wps2.0 */
2741         if (pmlmepriv->wps_probe_req_ie_len>0 && pmlmepriv->wps_probe_req_ie) {
2742                 memcpy(pframe, pmlmepriv->wps_probe_req_ie,
2743                        pmlmepriv->wps_probe_req_ie_len);
2744                 pframe += pmlmepriv->wps_probe_req_ie_len;
2745                 pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
2746         }
2747
2748         pattrib->last_txcmdsz = pattrib->pktlen;
2749
2750         RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
2751                  "issuing probe_req, tx_len =%d\n", pattrib->last_txcmdsz);
2752
2753         if (wait_ack) {
2754                 ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
2755         } else {
2756                 dump_mgntframe23a(padapter, pmgntframe);
2757                 ret = _SUCCESS;
2758         }
2759
2760 exit:
2761         return ret;
2762 }
2763
2764 static inline void issue_probereq(struct rtw_adapter *padapter,
2765                                   struct cfg80211_ssid *pssid, u8 *da)
2766 {
2767         _issue_probereq(padapter, pssid, da, false);
2768 }
2769
2770 static int issue_probereq_ex(struct rtw_adapter *padapter,
2771                              struct cfg80211_ssid *pssid, u8 *da,
2772                              int try_cnt, int wait_ms)
2773 {
2774         int ret;
2775         int i = 0;
2776         unsigned long start = jiffies;
2777
2778         do {
2779                 ret = _issue_probereq(padapter, pssid, da,
2780                                       wait_ms > 0 ? true : false);
2781
2782                 i++;
2783
2784                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
2785                         break;
2786
2787                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
2788                         msleep(wait_ms);
2789
2790         } while((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
2791
2792         if (ret != _FAIL) {
2793                 ret = _SUCCESS;
2794                 goto exit;
2795         }
2796
2797         if (try_cnt && wait_ms) {
2798                 if (da)
2799                         DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
2800                                   __func__, padapter->pnetdev->name,
2801                                   da, rtw_get_oper_ch23a(padapter),
2802                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
2803                                   jiffies_to_msecs(jiffies - start));
2804                 else
2805                         DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
2806                                   __func__, padapter->pnetdev->name,
2807                                   rtw_get_oper_ch23a(padapter),
2808                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
2809                                   jiffies_to_msecs(jiffies - start));
2810         }
2811 exit:
2812         return ret;
2813 }
2814
2815 /*  if psta == NULL, indiate we are station(client) now... */
2816 static void issue_auth(struct rtw_adapter *padapter, struct sta_info *psta,
2817                        unsigned short status)
2818 {
2819         struct xmit_frame *pmgntframe;
2820         struct pkt_attrib *pattrib;
2821         unsigned char *pframe;
2822         struct ieee80211_mgmt *mgmt;
2823         unsigned int val32;
2824         u16 auth_algo;
2825         int use_shared_key = 0;
2826         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2827         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2828         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
2829
2830         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
2831         if (!pmgntframe)
2832                 return;
2833
2834         /* update attribute */
2835         pattrib = &pmgntframe->attrib;
2836         update_mgntframe_attrib23a(padapter, pattrib);
2837
2838         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2839
2840         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2841         mgmt = (struct ieee80211_mgmt *)pframe;
2842
2843         mgmt->frame_control =
2844                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_AUTH);
2845         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
2846         pmlmeext->mgnt_seq++;
2847
2848         pattrib->pktlen = offsetof(struct ieee80211_mgmt, u.auth.variable);
2849
2850         if (psta) { /*  for AP mode */
2851 #ifdef CONFIG_8723AU_AP_MODE
2852                 unsigned short val16;
2853
2854                 ether_addr_copy(mgmt->da, psta->hwaddr);
2855                 ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
2856                 ether_addr_copy(mgmt->bssid, myid(&padapter->eeprompriv));
2857
2858                 /*  setting auth algo number */
2859                 val16 = (u16)psta->authalg;
2860
2861                 if (status != WLAN_STATUS_SUCCESS)
2862                         val16 = 0;
2863
2864                 if (val16)
2865                         use_shared_key = 1;
2866
2867                 mgmt->u.auth.auth_alg = cpu_to_le16(val16);
2868
2869                 /*  setting auth seq number */
2870                 mgmt->u.auth.auth_transaction =
2871                         cpu_to_le16((u16)psta->auth_seq);
2872
2873                 /*  setting status code... */
2874                 mgmt->u.auth.status_code = cpu_to_le16(status);
2875
2876                 pframe = mgmt->u.auth.variable;
2877                 /*  added challenging text... */
2878                 if ((psta->auth_seq == 2) &&
2879                     (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1))
2880                         pframe = rtw_set_ie23a(pframe, WLAN_EID_CHALLENGE, 128,
2881                                                psta->chg_txt, &pattrib->pktlen);
2882 #endif
2883         } else {
2884                 struct ieee80211_mgmt *iv_mgmt;
2885
2886                 ether_addr_copy(mgmt->da, get_my_bssid23a(&pmlmeinfo->network));
2887                 ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
2888                 ether_addr_copy(mgmt->bssid,
2889                                 get_my_bssid23a(&pmlmeinfo->network));
2890
2891                 /*  setting auth algo number */
2892                 /*  0:OPEN System, 1:Shared key */
2893                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
2894                         use_shared_key = 1;
2895                         auth_algo = WLAN_AUTH_SHARED_KEY;
2896                 } else
2897                         auth_algo = WLAN_AUTH_OPEN;
2898
2899                 /* DBG_8723A("%s auth_algo = %s auth_seq =%d\n", __func__,
2900                    (pmlmeinfo->auth_algo == 0)?"OPEN":"SHARED",
2901                    pmlmeinfo->auth_seq); */
2902
2903                 /* setting IV for auth seq #3 */
2904                 if ((pmlmeinfo->auth_seq == 3) &&
2905                     (pmlmeinfo->state & WIFI_FW_AUTH_STATE) &&
2906                     (use_shared_key == 1)) {
2907                         u32 *piv = (u32 *)&mgmt->u.auth;
2908
2909                         iv_mgmt = (struct ieee80211_mgmt *)(pframe + 4);
2910                         /* DBG_8723A("==> iv(%d), key_index(%d)\n",
2911                            pmlmeinfo->iv, pmlmeinfo->key_index); */
2912                         val32 = (pmlmeinfo->iv & 0x3fffffff) |
2913                                 (pmlmeinfo->key_index << 30);
2914                         pmlmeinfo->iv++;
2915                         put_unaligned_le32(val32, piv);
2916
2917                         pattrib->pktlen += 4;
2918
2919                         pattrib->iv_len = IEEE80211_WEP_IV_LEN;
2920                 } else
2921                         iv_mgmt = mgmt;
2922
2923                 iv_mgmt->u.auth.auth_alg = cpu_to_le16(auth_algo);
2924
2925                 /*  setting auth seq number */
2926                 iv_mgmt->u.auth.auth_transaction =
2927                         cpu_to_le16(pmlmeinfo->auth_seq);
2928
2929                 /*  setting status code... */
2930                 iv_mgmt->u.auth.status_code = cpu_to_le16(status);
2931
2932                 pframe = iv_mgmt->u.auth.variable;
2933
2934                 /*  then checking to see if sending challenging text... */
2935                 if ((pmlmeinfo->auth_seq == 3) &&
2936                     (pmlmeinfo->state & WIFI_FW_AUTH_STATE) &&
2937                     (use_shared_key == 1)) {
2938                         pframe = rtw_set_ie23a(pframe, WLAN_EID_CHALLENGE, 128,
2939                                                pmlmeinfo->chg_txt,
2940                                                &pattrib->pktlen);
2941
2942                         mgmt->frame_control |=
2943                                 cpu_to_le16(IEEE80211_FCTL_PROTECTED);
2944
2945                         pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
2946
2947                         pattrib->encrypt = WLAN_CIPHER_SUITE_WEP40;
2948
2949                         pattrib->icv_len = IEEE80211_WEP_ICV_LEN;
2950
2951                         pattrib->pktlen += pattrib->icv_len;
2952                 }
2953         }
2954
2955         pattrib->last_txcmdsz = pattrib->pktlen;
2956
2957         rtw_wep_encrypt23a(padapter, pmgntframe);
2958         DBG_8723A("%s\n", __func__);
2959         dump_mgntframe23a(padapter, pmgntframe);
2960
2961         return;
2962 }
2963
2964 #ifdef CONFIG_8723AU_AP_MODE
2965 static void issue_assocrsp(struct rtw_adapter *padapter, unsigned short status,
2966                            struct sta_info *pstat, u16 pkt_type)
2967 {
2968         struct xmit_frame *pmgntframe;
2969         struct ieee80211_mgmt *mgmt;
2970         struct pkt_attrib *pattrib;
2971         unsigned char *pframe;
2972         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2973         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2974         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2975         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
2976         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
2977         const u8 *p;
2978         u8 *ie = pnetwork->IEs;
2979
2980         DBG_8723A("%s\n", __func__);
2981
2982         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
2983         if (!pmgntframe)
2984                 return;
2985
2986         /* update attribute */
2987         pattrib = &pmgntframe->attrib;
2988         update_mgntframe_attrib23a(padapter, pattrib);
2989
2990         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2991
2992         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2993         mgmt = (struct ieee80211_mgmt *)pframe;
2994
2995         mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT | pkt_type);
2996
2997         ether_addr_copy(mgmt->da, pstat->hwaddr);
2998         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
2999         ether_addr_copy(mgmt->bssid, get_my_bssid23a(&pmlmeinfo->network));
3000
3001         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3002
3003         pmlmeext->mgnt_seq++;
3004
3005         pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
3006         pattrib->pktlen =
3007                 offsetof(struct ieee80211_mgmt, u.assoc_resp.variable);
3008
3009         mgmt->u.assoc_resp.capab_info = cpu_to_le16(pnetwork->capability);
3010         mgmt->u.assoc_resp.status_code = cpu_to_le16(status);
3011         mgmt->u.assoc_resp.aid = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
3012
3013         pframe = mgmt->u.assoc_resp.variable;
3014
3015         if (pstat->bssratelen <= 8) {
3016                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
3017                                        pstat->bssratelen, pstat->bssrateset,
3018                                        &pattrib->pktlen);
3019         } else {
3020                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
3021                                        pstat->bssrateset, &pattrib->pktlen);
3022                 pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
3023                                        pstat->bssratelen - 8,
3024                                        pstat->bssrateset + 8, &pattrib->pktlen);
3025         }
3026
3027         if (pstat->flags & WLAN_STA_HT && pmlmepriv->htpriv.ht_option) {
3028                 /* FILL HT CAP INFO IE */
3029                 /* p = hostapd_eid_ht_capabilities_info(hapd, p); */
3030                 p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, ie,
3031                                      pnetwork->IELength);
3032                 if (p && p[1]) {
3033                         memcpy(pframe, p, p[1] + 2);
3034                         pframe += (p[1] + 2);
3035                         pattrib->pktlen += (p[1] + 2);
3036                 }
3037
3038                 /* FILL HT ADD INFO IE */
3039                 /* p = hostapd_eid_ht_operation(hapd, p); */
3040                 p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, ie,
3041                                      pnetwork->IELength);
3042                 if (p && p[1] > 0) {
3043                         memcpy(pframe, p, p[1] + 2);
3044                         pframe += (p[1] + 2);
3045                         pattrib->pktlen += (p[1] + 2);
3046                 }
3047         }
3048
3049         /* FILL WMM IE */
3050         if (pstat->flags & WLAN_STA_WME && pmlmepriv->qos_option) {
3051                 unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02,
3052                                                0x01, 0x01};
3053                 int ie_len = 0;
3054
3055                 for (p = ie; ; p += (ie_len + 2)) {
3056                         p = cfg80211_find_ie(WLAN_EID_VENDOR_SPECIFIC, p,
3057                                              pnetwork->IELength - (ie_len + 2));
3058                         if (p)
3059                                 ie_len = p[1];
3060                         else
3061                                 ie_len = 0;
3062                         if (p && !memcmp(p + 2, WMM_PARA_IE, 6)) {
3063                                 memcpy(pframe, p, ie_len + 2);
3064                                 pframe += (ie_len + 2);
3065                                 pattrib->pktlen += (ie_len + 2);
3066
3067                                 break;
3068                         }
3069
3070                         if (!p || ie_len == 0)
3071                                 break;
3072                 }
3073         }
3074
3075         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK) {
3076                 pframe = rtw_set_ie23a(pframe, WLAN_EID_VENDOR_SPECIFIC, 6,
3077                                        REALTEK_96B_IE, &pattrib->pktlen);
3078         }
3079
3080         pattrib->last_txcmdsz = pattrib->pktlen;
3081
3082         dump_mgntframe23a(padapter, pmgntframe);
3083 }
3084 #endif
3085
3086 static void issue_assocreq(struct rtw_adapter *padapter)
3087 {
3088         int ret = _FAIL;
3089         struct xmit_frame *pmgntframe;
3090         struct pkt_attrib *pattrib;
3091         unsigned char *pframe;
3092         const u8 *p;
3093         struct ieee80211_mgmt *mgmt;
3094         unsigned int i, j, index = 0;
3095         unsigned char rf_type, bssrate[NumRates], sta_bssrate[NumRates];
3096         struct registry_priv *pregpriv = &padapter->registrypriv;
3097         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3098         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
3099         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3100         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3101         int bssrate_len = 0, sta_bssrate_len = 0, pie_len;
3102         u8 *pie;
3103
3104         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3105         if (!pmgntframe)
3106                 goto exit;
3107
3108         /* update attribute */
3109         pattrib = &pmgntframe->attrib;
3110         update_mgntframe_attrib23a(padapter, pattrib);
3111
3112         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3113
3114         pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
3115         mgmt = (struct ieee80211_mgmt *)pframe;
3116
3117         mgmt->frame_control =
3118                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_ASSOC_REQ);
3119
3120         ether_addr_copy(mgmt->da, get_my_bssid23a(&pmlmeinfo->network));
3121         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
3122         ether_addr_copy(mgmt->bssid, get_my_bssid23a(&pmlmeinfo->network));
3123
3124         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3125         pmlmeext->mgnt_seq++;
3126
3127         /* caps */
3128         put_unaligned_le16(pmlmeinfo->network.capability,
3129                            &mgmt->u.assoc_req.capab_info);
3130         /* todo: listen interval for power saving */
3131         put_unaligned_le16(3, &mgmt->u.assoc_req.listen_interval);
3132
3133         pframe = mgmt->u.assoc_req.variable;
3134         pattrib->pktlen = offsetof(struct ieee80211_mgmt, u.assoc_req.variable);
3135
3136         /* SSID */
3137         pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
3138                                pmlmeinfo->network.Ssid.ssid_len,
3139                                pmlmeinfo->network.Ssid.ssid, &pattrib->pktlen);
3140
3141         /* supported rate & extended supported rate */
3142
3143         get_rate_set23a(padapter, sta_bssrate, &sta_bssrate_len);
3144         /* DBG_8723A("sta_bssrate_len =%d\n", sta_bssrate_len); */
3145
3146         /*  for JAPAN, channel 14 can only uses B Mode(CCK) */
3147         if (pmlmeext->cur_channel == 14)
3148                 sta_bssrate_len = 4;
3149
3150         /* for (i = 0; i < sta_bssrate_len; i++) { */
3151         /*      DBG_8723A("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]); */
3152         /*  */
3153
3154         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
3155                 if (pmlmeinfo->network.SupportedRates[i] == 0)
3156                         break;
3157                 DBG_8723A("network.SupportedRates[%d]=%02X\n", i,
3158                           pmlmeinfo->network.SupportedRates[i]);
3159         }
3160
3161         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
3162                 if (pmlmeinfo->network.SupportedRates[i] == 0)
3163                         break;
3164
3165                 /*  Check if the AP's supported rates are also
3166                     supported by STA. */
3167                 for (j = 0; j < sta_bssrate_len; j++) {
3168                          /*  Avoid the proprietary data rate (22Mbps) of
3169                              Handlink WSG-4000 AP */
3170                         if ((pmlmeinfo->network.SupportedRates[i] |
3171                              IEEE80211_BASIC_RATE_MASK) ==
3172                             (sta_bssrate[j] | IEEE80211_BASIC_RATE_MASK)) {
3173                                 /* DBG_8723A("match i = %d, j =%d\n", i, j); */
3174                                 break;
3175                         }
3176                 }
3177
3178                 if (j == sta_bssrate_len) {
3179                         /*  the rate is not supported by STA */
3180                         DBG_8723A("%s(): the rate[%d]=%02X is not supported by "
3181                                   "STA!\n", __func__, i,
3182                                   pmlmeinfo->network.SupportedRates[i]);
3183                 } else {
3184                         /*  the rate is supported by STA */
3185                         bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
3186                 }
3187         }
3188
3189         bssrate_len = index;
3190         DBG_8723A("bssrate_len = %d\n", bssrate_len);
3191
3192         if (bssrate_len == 0) {
3193                 rtw_free_xmitbuf23a(pxmitpriv, pmgntframe->pxmitbuf);
3194                 rtw_free_xmitframe23a(pxmitpriv, pmgntframe);
3195                 goto exit; /* don't connect to AP if no joint supported rate */
3196         }
3197
3198         if (bssrate_len > 8) {
3199                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
3200                                        bssrate, &pattrib->pktlen);
3201                 pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
3202                                        (bssrate_len - 8), (bssrate + 8),
3203                                        &pattrib->pktlen);
3204         } else
3205                 pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
3206                                        bssrate_len, bssrate, &pattrib->pktlen);
3207
3208         /* RSN */
3209
3210         pie = pmlmeinfo->network.IEs;
3211         pie_len = pmlmeinfo->network.IELength;
3212
3213         p = cfg80211_find_ie(WLAN_EID_RSN, pie, pie_len);
3214         if (p)
3215                 pframe = rtw_set_ie23a(pframe, WLAN_EID_RSN, p[1], p + 2,
3216                                        &pattrib->pktlen);
3217
3218         /* HT caps */
3219         if (padapter->mlmepriv.htpriv.ht_option) {
3220                 p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, pie, pie_len);
3221
3222                 if (p && !is_ap_in_tkip23a(padapter)) {
3223                         struct ieee80211_ht_cap *cap = &pmlmeinfo->ht_cap;
3224
3225                         memcpy(cap, p + 2, sizeof(struct ieee80211_ht_cap));
3226
3227                         /* to disable 40M Hz support while gd_bw_40MHz_en = 0 */
3228                         if (pregpriv->cbw40_enable == 0) {
3229                                 cap->cap_info &= ~cpu_to_le16(
3230                                         IEEE80211_HT_CAP_SGI_40 |
3231                                         IEEE80211_HT_CAP_SUP_WIDTH_20_40);
3232                         } else {
3233                                 cap->cap_info |= cpu_to_le16(
3234                                         IEEE80211_HT_CAP_SUP_WIDTH_20_40);
3235                         }
3236
3237                         /* todo: disable SM power save mode */
3238                         cap->cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SM_PS);
3239
3240                         rf_type = rtl8723a_get_rf_type(padapter);
3241                         /* switch (pregpriv->rf_config) */
3242                         switch (rf_type) {
3243                         case RF_1T1R:
3244                                 /* RX STBC One spatial stream */
3245                                 if (pregpriv->rx_stbc)
3246                                         cap->cap_info |= cpu_to_le16(1 << IEEE80211_HT_CAP_RX_STBC_SHIFT);
3247
3248                                 memcpy(&cap->mcs, MCS_rate_1R23A, 16);
3249                                 break;
3250
3251                         case RF_2T2R:
3252                         case RF_1T2R:
3253                         default:
3254                                 /* enable for 2.4/5 GHz */
3255                                 if (pregpriv->rx_stbc == 0x3 ||
3256                                     (pmlmeext->cur_wireless_mode &
3257                                      WIRELESS_11_24N &&
3258                                      /* enable for 2.4GHz */
3259                                      pregpriv->rx_stbc == 0x1) ||
3260                                     (pmlmeext->cur_wireless_mode &
3261                                      WIRELESS_11_5N &&
3262                                      pregpriv->rx_stbc == 0x2) ||
3263                                     /* enable for 5GHz */
3264                                     pregpriv->wifi_spec == 1) {
3265                                         DBG_8723A("declare supporting RX "
3266                                                   "STBC\n");
3267                                         /* RX STBC two spatial stream */
3268                                         cap->cap_info |= cpu_to_le16(2 << IEEE80211_HT_CAP_RX_STBC_SHIFT);
3269                                 }
3270                                 memcpy(&cap->mcs, MCS_rate_2R23A, 16);
3271                                 break;
3272                         }
3273
3274                         if (rtl8723a_BT_coexist(padapter) &&
3275                             rtl8723a_BT_using_antenna_1(padapter)) {
3276                                 /*  set to 8K */
3277                                 cap->ampdu_params_info &=
3278                                         ~IEEE80211_HT_AMPDU_PARM_FACTOR;
3279 /*                              cap->ampdu_params_info |= MAX_AMPDU_FACTOR_8K */
3280                         }
3281
3282                         pframe = rtw_set_ie23a(pframe, WLAN_EID_HT_CAPABILITY,
3283                                                p[1], (u8 *)&pmlmeinfo->ht_cap,
3284                                                &pattrib->pktlen);
3285                 }
3286         }
3287
3288         /* vendor specific IE, such as WPA, WMM, WPS */
3289         for (i = 0;  i < pmlmeinfo->network.IELength;) {
3290                 p = pmlmeinfo->network.IEs + i;
3291
3292                 switch (p[0]) {
3293                 case WLAN_EID_VENDOR_SPECIFIC:
3294                         if (!memcmp(p + 2, RTW_WPA_OUI23A_TYPE, 4) ||
3295                             !memcmp(p + 2, WMM_OUI23A, 4) ||
3296                             !memcmp(p + 2, WPS_OUI23A, 4)) {
3297                                 u8 plen = p[1];
3298
3299                                 if (!padapter->registrypriv.wifi_spec) {
3300                                         /* Commented by Kurt 20110629 */
3301                                         /* In some older APs, WPS handshake */
3302                                         /* would be fail if we append vender
3303                                            extensions informations to AP */
3304                                         if (!memcmp(p + 2, WPS_OUI23A, 4))
3305                                                 plen = 14;
3306                                 }
3307                                 pframe = rtw_set_ie23a(pframe,
3308                                                        WLAN_EID_VENDOR_SPECIFIC,
3309                                                        plen, p + 2,
3310                                                        &pattrib->pktlen);
3311                         }
3312                         break;
3313
3314                 default:
3315                         break;
3316                 }
3317
3318                 i += p[1] + 2;
3319         }
3320
3321         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
3322                 pframe = rtw_set_ie23a(pframe, WLAN_EID_VENDOR_SPECIFIC, 6,
3323                                        REALTEK_96B_IE, &pattrib->pktlen);
3324
3325         pattrib->last_txcmdsz = pattrib->pktlen;
3326         dump_mgntframe23a(padapter, pmgntframe);
3327
3328         ret = _SUCCESS;
3329
3330 exit:
3331         pmlmepriv->assoc_req_len = 0;
3332         if (ret == _SUCCESS) {
3333                 kfree(pmlmepriv->assoc_req);
3334                 pmlmepriv->assoc_req = kmalloc(pattrib->pktlen, GFP_ATOMIC);
3335                 if (pmlmepriv->assoc_req) {
3336                         memcpy(pmlmepriv->assoc_req, mgmt, pattrib->pktlen);
3337                         pmlmepriv->assoc_req_len = pattrib->pktlen;
3338                 }
3339         } else
3340                 kfree(pmlmepriv->assoc_req);
3341
3342         return;
3343 }
3344
3345 /* when wait_ack is true, this function should be called at process context */
3346 static int _issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
3347                               unsigned int power_mode, int wait_ack)
3348 {
3349         int ret = _FAIL;
3350         struct xmit_frame *pmgntframe;
3351         struct pkt_attrib *pattrib;
3352         unsigned char *pframe;
3353         struct ieee80211_hdr *pwlanhdr;
3354         struct xmit_priv *pxmitpriv;
3355         struct mlme_ext_priv *pmlmeext;
3356         struct mlme_ext_info *pmlmeinfo;
3357
3358         /* DBG_8723A("%s:%d\n", __func__, power_mode); */
3359
3360         if (!padapter)
3361                 goto exit;
3362
3363         pxmitpriv = &padapter->xmitpriv;
3364         pmlmeext = &padapter->mlmeextpriv;
3365         pmlmeinfo = &pmlmeext->mlmext_info;
3366
3367         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3368         if (!pmgntframe)
3369                 goto exit;
3370
3371         /* update attribute */
3372         pattrib = &pmgntframe->attrib;
3373         update_mgntframe_attrib23a(padapter, pattrib);
3374         pattrib->retry_ctrl = false;
3375
3376         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3377
3378         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3379         pwlanhdr = (struct ieee80211_hdr *)pframe;
3380
3381         pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
3382                                               IEEE80211_STYPE_NULLFUNC);
3383
3384         if ((pmlmeinfo->state&0x03) == MSR_AP)
3385                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
3386         else if ((pmlmeinfo->state&0x03) == MSR_INFRA)
3387                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_TODS);
3388
3389         if (power_mode)
3390                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
3391
3392         ether_addr_copy(pwlanhdr->addr1, da);
3393         ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
3394         ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
3395
3396         pwlanhdr->seq_ctrl =
3397                 cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3398         pmlmeext->mgnt_seq++;
3399
3400         pframe += sizeof(struct ieee80211_hdr_3addr);
3401         pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
3402
3403         pattrib->last_txcmdsz = pattrib->pktlen;
3404
3405         if (wait_ack)
3406                 ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
3407         else {
3408                 dump_mgntframe23a(padapter, pmgntframe);
3409                 ret = _SUCCESS;
3410         }
3411
3412 exit:
3413         return ret;
3414 }
3415
3416 /* when wait_ms >0 , this function should be called at process context */
3417 /* da == NULL for station mode */
3418 int issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
3419                       unsigned int power_mode, int try_cnt, int wait_ms)
3420 {
3421         int ret;
3422         int i = 0;
3423         unsigned long start = jiffies;
3424         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3425         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3426
3427         /* da == NULL, assume it's null data for sta to ap*/
3428         if (da == NULL)
3429                 da = get_my_bssid23a(&pmlmeinfo->network);
3430
3431         do {
3432                 ret = _issue_nulldata23a(padapter, da, power_mode,
3433                                          wait_ms > 0 ? true : false);
3434
3435                 i++;
3436
3437                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
3438                         break;
3439
3440                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
3441                         msleep(wait_ms);
3442
3443         } while((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
3444
3445         if (ret != _FAIL) {
3446                 ret = _SUCCESS;
3447                 goto exit;
3448         }
3449
3450         if (try_cnt && wait_ms) {
3451                 if (da)
3452                         DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
3453                                   __func__, padapter->pnetdev->name,
3454                                   da, rtw_get_oper_ch23a(padapter),
3455                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3456                                   jiffies_to_msecs(jiffies - start));
3457                 else
3458                         DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
3459                                   __func__, padapter->pnetdev->name,
3460                                   rtw_get_oper_ch23a(padapter),
3461                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3462                                   jiffies_to_msecs(jiffies - start));
3463         }
3464 exit:
3465         return ret;
3466 }
3467
3468 /* when wait_ack is true, this function should be called at process context */
3469 static int _issue_qos_nulldata23a(struct rtw_adapter *padapter,
3470                                   unsigned char *da, u16 tid, int wait_ack)
3471 {
3472         int ret = _FAIL;
3473         struct xmit_frame *pmgntframe;
3474         struct pkt_attrib *pattrib;
3475         unsigned char *pframe;
3476         struct ieee80211_qos_hdr *pwlanhdr;
3477         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3478         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3479         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3480
3481         DBG_8723A("%s\n", __func__);
3482
3483         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3484         if (!pmgntframe)
3485                 goto exit;
3486
3487         /* update attribute */
3488         pattrib = &pmgntframe->attrib;
3489         update_mgntframe_attrib23a(padapter, pattrib);
3490
3491         pattrib->hdrlen += 2;
3492         pattrib->qos_en = true;
3493         pattrib->eosp = 1;
3494         pattrib->ack_policy = 0;
3495         pattrib->mdata = 0;
3496
3497         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3498
3499         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3500         pwlanhdr = (struct ieee80211_qos_hdr *)pframe;
3501
3502         pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
3503                                               IEEE80211_STYPE_QOS_NULLFUNC);
3504
3505         if ((pmlmeinfo->state&0x03) == MSR_AP)
3506                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
3507         else if ((pmlmeinfo->state&0x03) == MSR_INFRA)
3508                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_TODS);
3509
3510         if (pattrib->mdata)
3511                 pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
3512
3513         pwlanhdr->qos_ctrl = cpu_to_le16(tid & IEEE80211_QOS_CTL_TID_MASK);
3514         pwlanhdr->qos_ctrl |= cpu_to_le16((pattrib->ack_policy << 5) &
3515                                           IEEE80211_QOS_CTL_ACK_POLICY_MASK);
3516         if (pattrib->eosp)
3517                 pwlanhdr->qos_ctrl |= cpu_to_le16(IEEE80211_QOS_CTL_EOSP);
3518
3519         ether_addr_copy(pwlanhdr->addr1, da);
3520         ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
3521         ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
3522
3523         pwlanhdr->seq_ctrl =
3524                 cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3525         pmlmeext->mgnt_seq++;
3526
3527         pframe += sizeof(struct ieee80211_qos_hdr);
3528         pattrib->pktlen = sizeof(struct ieee80211_qos_hdr);
3529
3530         pattrib->last_txcmdsz = pattrib->pktlen;
3531
3532         if (wait_ack)
3533                 ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
3534         else {
3535                 dump_mgntframe23a(padapter, pmgntframe);
3536                 ret = _SUCCESS;
3537         }
3538
3539 exit:
3540         return ret;
3541 }
3542
3543 /* when wait_ms >0 , this function should be called at process context */
3544 /* da == NULL for station mode */
3545 int issue_qos_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
3546                           u16 tid, int try_cnt, int wait_ms)
3547 {
3548         int ret;
3549         int i = 0;
3550         unsigned long start = jiffies;
3551         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3552         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3553
3554         /* da == NULL, assume it's null data for sta to ap*/
3555         if (da == NULL)
3556                 da = get_my_bssid23a(&pmlmeinfo->network);
3557
3558         do {
3559                 ret = _issue_qos_nulldata23a(padapter, da, tid,
3560                                              wait_ms > 0 ? true : false);
3561
3562                 i++;
3563
3564                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
3565                         break;
3566
3567                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
3568                         msleep(wait_ms);
3569         } while((i < try_cnt) && ((ret == _FAIL)||(wait_ms == 0)));
3570
3571         if (ret != _FAIL) {
3572                 ret = _SUCCESS;
3573                 goto exit;
3574         }
3575
3576         if (try_cnt && wait_ms) {
3577                 if (da)
3578                         DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
3579                                   __func__, padapter->pnetdev->name,
3580                                   da, rtw_get_oper_ch23a(padapter),
3581                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3582                                   jiffies_to_msecs(jiffies - start));
3583                 else
3584                         DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
3585                                   __func__, padapter->pnetdev->name,
3586                                   rtw_get_oper_ch23a(padapter),
3587                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3588                                   jiffies_to_msecs(jiffies - start));
3589         }
3590 exit:
3591         return ret;
3592 }
3593
3594 static int _issue_deauth(struct rtw_adapter *padapter, unsigned char *da,
3595                          unsigned short reason, u8 wait_ack)
3596 {
3597         struct xmit_frame *pmgntframe;
3598         struct pkt_attrib *pattrib;
3599         struct ieee80211_mgmt *mgmt;
3600         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3601         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3602         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3603         int ret = _FAIL;
3604
3605         /* DBG_8723A("%s to %pM\n", __func__, da); */
3606
3607         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3608         if (!pmgntframe)
3609                 goto exit;
3610
3611         /* update attribute */
3612         pattrib = &pmgntframe->attrib;
3613         update_mgntframe_attrib23a(padapter, pattrib);
3614         pattrib->retry_ctrl = false;
3615
3616         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3617
3618         mgmt = (struct ieee80211_mgmt *)(pmgntframe->buf_addr + TXDESC_OFFSET);
3619
3620         mgmt->frame_control =
3621                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_DEAUTH);
3622
3623         ether_addr_copy(mgmt->da, da);
3624         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
3625         ether_addr_copy(mgmt->bssid, get_my_bssid23a(&pmlmeinfo->network));
3626
3627         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3628         pmlmeext->mgnt_seq++;
3629
3630         pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr) + 2;
3631
3632         mgmt->u.deauth.reason_code = cpu_to_le16(reason);
3633
3634         pattrib->last_txcmdsz = pattrib->pktlen;
3635
3636         if (wait_ack)
3637                 ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
3638         else {
3639                 dump_mgntframe23a(padapter, pmgntframe);
3640                 ret = _SUCCESS;
3641         }
3642
3643 exit:
3644         return ret;
3645 }
3646
3647 int issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da,
3648                     unsigned short reason)
3649 {
3650         DBG_8723A("%s to %pM\n", __func__, da);
3651         return _issue_deauth(padapter, da, reason, false);
3652 }
3653
3654 static int issue_deauth_ex(struct rtw_adapter *padapter, u8 *da,
3655                            unsigned short reason, int try_cnt, int wait_ms)
3656 {
3657         int ret;
3658         int i = 0;
3659         unsigned long start = jiffies;
3660
3661         do {
3662                 ret = _issue_deauth(padapter, da, reason,
3663                                     wait_ms >0 ? true : false);
3664
3665                 i++;
3666
3667                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
3668                         break;
3669
3670                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
3671                         msleep(wait_ms);
3672
3673         } while((i < try_cnt) && ((ret == _FAIL)||(wait_ms == 0)));
3674
3675         if (ret != _FAIL) {
3676                 ret = _SUCCESS;
3677                 goto exit;
3678         }
3679
3680         if (try_cnt && wait_ms) {
3681                 if (da)
3682                         DBG_8723A("%s(%s): to %pM, ch:%u%s, %d/%d in %u ms\n",
3683                                   __func__, padapter->pnetdev->name,
3684                                   da, rtw_get_oper_ch23a(padapter),
3685                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3686                                   jiffies_to_msecs(jiffies - start));
3687                 else
3688                         DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
3689                                   __func__, padapter->pnetdev->name,
3690                                   rtw_get_oper_ch23a(padapter),
3691                                   ret == _SUCCESS ? ", acked" : "", i, try_cnt,
3692                                   jiffies_to_msecs(jiffies - start));
3693         }
3694 exit:
3695         return ret;
3696 }
3697
3698 void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter,
3699                                     u8 *ra, u8 new_ch, u8 ch_offset)
3700 {
3701         struct xmit_frame *pmgntframe;
3702         struct pkt_attrib *pattrib;
3703         unsigned char *pframe;
3704         struct ieee80211_mgmt *mgmt;
3705         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3706         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3707
3708         DBG_8723A("%s(%s): ra=%pM, ch:%u, offset:%u\n",
3709                   __func__, padapter->pnetdev->name, ra, new_ch, ch_offset);
3710
3711         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3712         if (!pmgntframe)
3713                 return;
3714
3715         /* update attribute */
3716         pattrib = &pmgntframe->attrib;
3717         update_mgntframe_attrib23a(padapter, pattrib);
3718
3719         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3720
3721         mgmt = (struct ieee80211_mgmt *)(pmgntframe->buf_addr + TXDESC_OFFSET);
3722
3723         mgmt->frame_control =
3724                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_ACTION);
3725
3726         ether_addr_copy(mgmt->da, ra); /* RA */
3727         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv)); /* TA */
3728         ether_addr_copy(mgmt->bssid, ra); /* DA = RA */
3729
3730         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3731         pmlmeext->mgnt_seq++;
3732
3733         mgmt->u.action.category = WLAN_CATEGORY_SPECTRUM_MGMT;
3734         mgmt->u.action.u.chan_switch.action_code = WLAN_ACTION_SPCT_CHL_SWITCH;
3735
3736         pframe = mgmt->u.action.u.chan_switch.variable;
3737         pattrib->pktlen = offsetof(struct ieee80211_mgmt,
3738                                    u.action.u.chan_switch.variable);
3739
3740         pframe = rtw_set_ie23a_ch_switch (pframe, &pattrib->pktlen, 0,
3741                                           new_ch, 0);
3742         pframe = rtw_set_ie23a_secondary_ch_offset(pframe, &pattrib->pktlen,
3743                 hal_ch_offset_to_secondary_ch_offset23a(ch_offset));
3744
3745         pattrib->last_txcmdsz = pattrib->pktlen;
3746
3747         dump_mgntframe23a(padapter, pmgntframe);
3748 }
3749
3750 void issue_action_BA23a(struct rtw_adapter *padapter,
3751                         const unsigned char *raddr,
3752                         unsigned char action, unsigned short status)
3753 {
3754         u16 start_seq;
3755         u16 BA_para_set;
3756         u16 BA_starting_seqctrl;
3757         u16 BA_para;
3758         int max_rx_ampdu_factor;
3759         struct xmit_frame *pmgntframe;
3760         struct pkt_attrib *pattrib;
3761         struct ieee80211_mgmt *mgmt;
3762         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3763         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3764         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3765         struct sta_info *psta;
3766         struct sta_priv *pstapriv = &padapter->stapriv;
3767         struct registry_priv *pregpriv = &padapter->registrypriv;
3768         u8 tendaAPMac[] = {0xC8, 0x3A, 0x35};
3769
3770         DBG_8723A("%s, action =%d, status =%d\n", __func__, action, status);
3771
3772         pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
3773         if (!pmgntframe)
3774                 return;
3775
3776         /* update attribute */
3777         pattrib = &pmgntframe->attrib;
3778         update_mgntframe_attrib23a(padapter, pattrib);
3779
3780         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3781
3782         mgmt = (struct ieee80211_mgmt *)(pmgntframe->buf_addr + TXDESC_OFFSET);
3783
3784         mgmt->frame_control =
3785                 cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_ACTION);
3786
3787         ether_addr_copy(mgmt->da, raddr);
3788         ether_addr_copy(mgmt->sa, myid(&padapter->eeprompriv));
3789         ether_addr_copy(mgmt->bssid, get_my_bssid23a(&pmlmeinfo->network));
3790
3791         mgmt->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
3792         pmlmeext->mgnt_seq++;
3793
3794         mgmt->u.action.category = WLAN_CATEGORY_BACK;
3795
3796         pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr) + 1;
3797
3798         switch (action) {
3799         case WLAN_ACTION_ADDBA_REQ:
3800                 pattrib->pktlen += sizeof(mgmt->u.action.u.addba_req);
3801
3802                 mgmt->u.action.u.addba_req.action_code = action;
3803
3804                 do {
3805                         pmlmeinfo->dialogToken++;
3806                 } while (pmlmeinfo->dialogToken == 0);
3807
3808                 mgmt->u.action.u.addba_req.dialog_token =
3809                         pmlmeinfo->dialogToken;
3810
3811                 if (rtl8723a_BT_coexist(padapter) &&
3812                     rtl8723a_BT_using_antenna_1(padapter) &&
3813                     (pmlmeinfo->assoc_AP_vendor != broadcomAP ||
3814                      memcmp(raddr, tendaAPMac, 3))) {
3815                         /*  A-MSDU NOT Supported */
3816                         BA_para_set = 0;
3817                         /*  immediate Block Ack */
3818                         BA_para_set |= (1 << 1) &
3819                                 IEEE80211_ADDBA_PARAM_POLICY_MASK;
3820                         /*  TID */
3821                         BA_para_set |= (status << 2) &
3822                                 IEEE80211_ADDBA_PARAM_TID_MASK;
3823                         /*  max buffer size is 8 MSDU */
3824                         BA_para_set |= (8 << 6) &
3825                                 IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
3826                 } else {
3827                         /* immediate ack & 64 buffer size */
3828                         BA_para_set = 0x1002 | ((status & 0xf) << 2);
3829                 }
3830
3831                 put_unaligned_le16(BA_para_set,
3832                                    &mgmt->u.action.u.addba_req.capab);
3833
3834                 /*  5ms */
3835                 put_unaligned_le16(5000, &mgmt->u.action.u.addba_req.timeout);
3836
3837                 psta = rtw_get_stainfo23a(pstapriv, raddr);
3838                 if (psta) {
3839                         int idx;
3840
3841                         idx = status & 0x07;
3842                         start_seq =
3843                                 (psta->sta_xmitpriv.txseq_tid[idx] & 0xfff) + 1;
3844
3845                         DBG_8723A("BA_starting_seqctrl = %d for TID =%d\n",
3846                                   start_seq, idx);
3847
3848                         psta->BA_starting_seqctrl[idx] = start_seq;
3849
3850                         BA_starting_seqctrl = start_seq << 4;
3851                 } else
3852                         BA_starting_seqctrl = 0;
3853
3854                 put_unaligned_le16(BA_starting_seqctrl,
3855                                    &mgmt->u.action.u.addba_req.start_seq_num);
3856
3857                 break;
3858
3859         case WLAN_ACTION_ADDBA_RESP:
3860                 pattrib->pktlen += sizeof(mgmt->u.action.u.addba_resp);
3861
3862                 mgmt->u.action.u.addba_resp.action_code = action;
3863                 mgmt->u.action.u.addba_resp.dialog_token =
3864                         pmlmeinfo->ADDBA_req.dialog_token;
3865                 put_unaligned_le16(status,
3866                                    &mgmt->u.action.u.addba_resp.status);
3867
3868                 GetHalDefVar8192CUsb(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR,
3869                                      &max_rx_ampdu_factor);
3870
3871                 BA_para = le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f;
3872                 if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_64K)
3873                         BA_para_set = BA_para | 0x1000; /* 64 buffer size */
3874                 else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_32K)
3875                         BA_para_set = BA_para | 0x0800; /* 32 buffer size */
3876                 else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_16K)
3877                         BA_para_set = BA_para | 0x0400; /* 16 buffer size */
3878                 else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_8K)
3879                         BA_para_set = BA_para | 0x0200; /* 8 buffer size */
3880                 else
3881                         BA_para_set = BA_para | 0x1000; /* 64 buffer size */
3882
3883                 if (rtl8723a_BT_coexist(padapter) &&
3884                     rtl8723a_BT_using_antenna_1(padapter) &&
3885                     (pmlmeinfo->assoc_AP_vendor != broadcomAP ||
3886                      memcmp(raddr, tendaAPMac, 3))) {
3887                         /*  max buffer size is 8 MSDU */
3888                         BA_para_set &= ~IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
3889                         BA_para_set |= (8 << 6) &
3890                                 IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
3891                 }
3892
3893                 if (pregpriv->ampdu_amsdu == 0)/* disabled */
3894                         BA_para_set &= ~BIT(0);
3895                 else if (pregpriv->ampdu_amsdu == 1)/* enabled */
3896                         BA_para_set |= BIT(0);
3897
3898                 put_unaligned_le16(BA_para_set,
3899                                    &mgmt->u.action.u.addba_resp.capab);
3900
3901                 mgmt->u.action.u.addba_resp.timeout
3902                         = pmlmeinfo->ADDBA_req.BA_timeout_value;
3903
3904                 pattrib->pktlen += 8;
3905                 break;
3906         case WLAN_ACTION_DELBA:
3907                 pattrib->pktlen += sizeof(mgmt->u.action.u.delba);
3908
3909                 mgmt->u.action.u.delba.action_code = action;
3910                 BA_para_set = (status & 0x1F) << 3;
3911                 mgmt->u.action.u.delba.params = cpu_to_le16(BA_para_set);
3912                 mgmt->u.action.u.delba.reason_code =
3913                         cpu_to_le16(WLAN_REASON_QSTA_NOT_USE);
3914
3915                 pattrib->pktlen += 5;
3916                 break;
3917         default:
3918                 break;
3919         }
3920
3921         pattrib->last_txcmdsz = pattrib->pktlen;
3922
3923         dump_mgntframe23a(padapter, pmgntframe);
3924 }
3925
3926 int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr)
3927 {
3928         struct sta_priv *pstapriv = &padapter->stapriv;
3929         struct sta_info *psta = NULL;
3930         /* struct recv_reorder_ctrl *preorder_ctrl; */
3931         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3932         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
3933         u16 tid;
3934
3935         if ((pmlmeinfo->state&0x03) != MSR_AP)
3936                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
3937                         return _SUCCESS;
3938
3939         psta = rtw_get_stainfo23a(pstapriv, addr);
3940         if (psta == NULL)
3941                 return _SUCCESS;
3942
3943         if (initiator == 0) {  /*  recipient */
3944                 for (tid = 0; tid < MAXTID; tid++) {
3945                         if (psta->recvreorder_ctrl[tid].enable == true) {
3946                                 DBG_8723A("rx agg disable tid(%d)\n", tid);
3947                                 issue_action_BA23a(padapter, addr, WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F));
3948                                 psta->recvreorder_ctrl[tid].enable = false;
3949                                 psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
3950                         }
3951                 }
3952         } else if (initiator == 1) { /*  originator */
3953                 for (tid = 0; tid < MAXTID; tid++) {
3954                         if (psta->htpriv.agg_enable_bitmap & BIT(tid)) {
3955                                 DBG_8723A("tx agg disable tid(%d)\n", tid);
3956                                 issue_action_BA23a(padapter, addr, WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F));
3957                                 psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
3958                                 psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
3959
3960                         }
3961                 }
3962         }
3963         return _SUCCESS;
3964 }
3965
3966 int send_beacon23a(struct rtw_adapter *padapter)
3967 {
3968         bool bxmitok;
3969         int issue = 0;
3970         int poll = 0;
3971         unsigned long start = jiffies;
3972         unsigned int passing_time;
3973
3974         rtl8723a_bcn_valid(padapter);
3975         do {
3976                 issue_beacon23a(padapter, 100);
3977                 issue++;
3978                 do {
3979                         yield();
3980                         bxmitok = rtl8723a_get_bcn_valid(padapter);
3981                         poll++;
3982                 } while ((poll % 10) != 0 && !bxmitok &&
3983                          !padapter->bSurpriseRemoved &&
3984                          !padapter->bDriverStopped);
3985
3986         } while (!bxmitok && issue<100 && !padapter->bSurpriseRemoved &&
3987                  !padapter->bDriverStopped);
3988
3989         if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
3990                 return _FAIL;
3991
3992         passing_time = jiffies_to_msecs(jiffies - start);
3993
3994         if (!bxmitok) {
3995                 DBG_8723A("%s fail! %u ms\n", __func__, passing_time);
3996                 return _FAIL;
3997         } else {
3998
3999                 if (passing_time > 100 || issue > 3)
4000                         DBG_8723A("%s success, issue:%d, poll:%d, %u ms\n",
4001                                   __func__, issue, poll, passing_time);
4002                 return _SUCCESS;
4003         }
4004 }
4005
4006 /****************************************************************************
4007
4008 Following are some utitity functions for WiFi MLME
4009
4010 *****************************************************************************/
4011
4012 bool IsLegal5GChannel(struct rtw_adapter *Adapter, u8 channel)
4013 {
4014
4015         int i = 0;
4016         u8 Channel_5G[45] = {36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58,
4017                              60, 62, 64, 100, 102, 104, 106, 108, 110, 112,
4018                              114, 116, 118, 120, 122, 124, 126, 128, 130, 132,
4019                              134, 136, 138, 140, 149, 151, 153, 155, 157, 159,
4020                              161, 163, 165};
4021         for (i = 0; i < sizeof(Channel_5G); i++)
4022                 if (channel == Channel_5G[i])
4023                         return true;
4024         return false;
4025 }
4026
4027 static void rtw_site_survey(struct rtw_adapter *padapter)
4028 {
4029         unsigned char survey_channel = 0;
4030         enum rt_scan_type ScanType = SCAN_PASSIVE;
4031         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4032         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4033         struct rtw_ieee80211_channel *ch;
4034
4035         if (pmlmeext->sitesurvey_res.channel_idx <
4036             pmlmeext->sitesurvey_res.ch_num) {
4037                 ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
4038                 survey_channel = ch->hw_value;
4039                 ScanType = (ch->flags & IEEE80211_CHAN_NO_IR) ?
4040                         SCAN_PASSIVE : SCAN_ACTIVE;
4041         }
4042
4043         if (survey_channel != 0) {
4044                 /* PAUSE 4-AC Queue when site_survey */
4045                 if (pmlmeext->sitesurvey_res.channel_idx == 0)
4046                         set_channel_bwmode23a(padapter, survey_channel,
4047                                               HAL_PRIME_CHNL_OFFSET_DONT_CARE,
4048                                               HT_CHANNEL_WIDTH_20);
4049                 else
4050                         SelectChannel23a(padapter, survey_channel);
4051
4052                 if (ScanType == SCAN_ACTIVE) /* obey the channel plan setting... */
4053                 {
4054                         int i;
4055
4056                         for (i = 0;i<RTW_SSID_SCAN_AMOUNT;i++) {
4057                                 if (pmlmeext->sitesurvey_res.ssid[i].ssid_len) {
4058                                         /* todo: to issue two probe req??? */
4059                                         issue_probereq(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
4060                                         /* msleep(SURVEY_TO>>1); */
4061                                         issue_probereq(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
4062                                 }
4063                         }
4064
4065                         if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
4066                                 /* todo: to issue two probe req??? */
4067                                 issue_probereq(padapter, NULL, NULL);
4068                                 /* msleep(SURVEY_TO>>1); */
4069                                 issue_probereq(padapter, NULL, NULL);
4070                         }
4071                 }
4072
4073                 set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
4074         } else {
4075                 /*      channel number is 0 or this channel is not valid. */
4076                 pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
4077
4078                 /* switch back to the original channel */
4079
4080                 set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
4081                                       pmlmeext->cur_ch_offset,
4082                                       pmlmeext->cur_bwmode);
4083
4084                 /* flush 4-AC Queue after rtw_site_survey */
4085                 /* val8 = 0; */
4086
4087                 /* config MSR */
4088                 rtl8723a_set_media_status(padapter, pmlmeinfo->state & 0x3);
4089
4090                 /* restore RX GAIN */
4091                 rtl8723a_set_initial_gain(padapter, 0xff);
4092                 /* turn on dynamic functions */
4093                 rtl8723a_odm_support_ability_restore(padapter);
4094
4095                 if (is_client_associated_to_ap23a(padapter) == true)
4096                         issue_nulldata23a(padapter, NULL, 0, 3, 500);
4097
4098                 rtl8723a_mlme_sitesurvey(padapter, 0);
4099
4100                 report_surveydone_event23a(padapter);
4101
4102                 pmlmeext->chan_scan_time = SURVEY_TO;
4103                 pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
4104         }
4105
4106         return;
4107 }
4108
4109 /* collect bss info from Beacon and Probe request/response frames. */
4110 static struct wlan_bssid_ex *collect_bss_info(struct rtw_adapter *padapter,
4111                                               struct recv_frame *precv_frame)
4112 {
4113         struct sk_buff *skb = precv_frame->pkt;
4114         struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
4115         struct registry_priv *pregistrypriv = &padapter->registrypriv;
4116         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4117         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4118         struct wlan_bssid_ex *bssid;
4119         const u8 *p;
4120         u8 *pie;
4121         unsigned int length;
4122         int i;
4123
4124         length = skb->len;
4125
4126         bssid = kzalloc(sizeof(struct wlan_bssid_ex), GFP_ATOMIC);
4127         if (!bssid)
4128                 return NULL;
4129
4130         if (ieee80211_is_beacon(mgmt->frame_control)) {
4131                 length -= offsetof(struct ieee80211_mgmt, u.beacon.variable);
4132                 pie = mgmt->u.beacon.variable;
4133                 bssid->reserved = 1;
4134                 bssid->capability =
4135                         get_unaligned_le16(&mgmt->u.beacon.capab_info);
4136                 bssid->beacon_interval =
4137                         get_unaligned_le16(&mgmt->u.beacon.beacon_int);
4138                 bssid->tsf = get_unaligned_le64(&mgmt->u.beacon.timestamp);
4139         } else if (ieee80211_is_probe_req(mgmt->frame_control)) {
4140                 length -= offsetof(struct ieee80211_mgmt, u.probe_req.variable);
4141                 pie = mgmt->u.probe_req.variable;
4142                 bssid->reserved = 2;
4143                 bssid->capability = 0;
4144                 bssid->beacon_interval =
4145                         padapter->registrypriv.dev_network.beacon_interval;
4146                 bssid->tsf = 0;
4147         } else if (ieee80211_is_probe_resp(mgmt->frame_control)) {
4148                 length -=
4149                         offsetof(struct ieee80211_mgmt, u.probe_resp.variable);
4150                 pie = mgmt->u.probe_resp.variable;
4151                 bssid->reserved = 3;
4152                 bssid->capability =
4153                         get_unaligned_le16(&mgmt->u.probe_resp.capab_info);
4154                 bssid->beacon_interval =
4155                         get_unaligned_le16(&mgmt->u.probe_resp.beacon_int);
4156                 bssid->tsf = get_unaligned_le64(&mgmt->u.probe_resp.timestamp);
4157         } else {
4158                 length -= offsetof(struct ieee80211_mgmt, u.beacon.variable);
4159                 pie = mgmt->u.beacon.variable;
4160                 bssid->reserved = 0;
4161                 bssid->capability =
4162                         get_unaligned_le16(&mgmt->u.beacon.capab_info);
4163                 bssid->beacon_interval =
4164                         padapter->registrypriv.dev_network.beacon_interval;
4165                 bssid->tsf = 0;
4166         }
4167
4168         if (length > MAX_IE_SZ) {
4169                 /* DBG_8723A("IE too long for survey event\n"); */
4170                 kfree(bssid);
4171                 return NULL;
4172         }
4173
4174         bssid->Length = offsetof(struct wlan_bssid_ex, IEs) + length;
4175
4176         /* below is to copy the information element */
4177         bssid->IELength = length;
4178         memcpy(bssid->IEs, pie, bssid->IELength);
4179
4180         /* get the signal strength */
4181         /*  in dBM.raw data */
4182         bssid->Rssi = precv_frame->attrib.phy_info.RecvSignalPower;
4183         bssid->SignalQuality =
4184                 precv_frame->attrib.phy_info.SignalQuality;/* in percentage */
4185         bssid->SignalStrength =
4186                 precv_frame->attrib.phy_info.SignalStrength;/* in percentage */
4187
4188         /*  checking SSID */
4189         p = cfg80211_find_ie(WLAN_EID_SSID, bssid->IEs, bssid->IELength);
4190
4191         if (!p) {
4192                 DBG_8723A("marc: cannot find SSID for survey event\n");
4193                 goto fail;
4194         }
4195
4196         if (p[1] > IEEE80211_MAX_SSID_LEN) {
4197                 DBG_8723A("%s()-%d: IE too long (%d) for survey "
4198                           "event\n", __func__, __LINE__, p[1]);
4199                 goto fail;
4200         }
4201         memcpy(bssid->Ssid.ssid, p + 2, p[1]);
4202         bssid->Ssid.ssid_len = p[1];
4203
4204         /* checking rate info... */
4205         i = 0;
4206         p = cfg80211_find_ie(WLAN_EID_SUPP_RATES, bssid->IEs, bssid->IELength);
4207         if (p) {
4208                 if (p[1] > NDIS_802_11_LENGTH_RATES_EX) {
4209                         DBG_8723A("%s()-%d: IE too long (%d) for survey "
4210                                   "event\n", __func__, __LINE__, p[1]);
4211                         goto fail;
4212                 }
4213                 memcpy(bssid->SupportedRates, p + 2, p[1]);
4214                 i = p[1];
4215         }
4216
4217         p = cfg80211_find_ie(WLAN_EID_EXT_SUPP_RATES, bssid->IEs,
4218                              bssid->IELength);
4219         if (p) {
4220                 if (p[1] > (NDIS_802_11_LENGTH_RATES_EX-i)) {
4221                         DBG_8723A("%s()-%d: IE too long (%d) for survey "
4222                                   "event\n", __func__, __LINE__, p[1]);
4223                         goto fail;
4224                 }
4225                 memcpy(bssid->SupportedRates + i, p + 2, p[1]);
4226         }
4227
4228         /*  Checking for DSConfig */
4229         p = cfg80211_find_ie(WLAN_EID_DS_PARAMS, bssid->IEs, bssid->IELength);
4230
4231         bssid->DSConfig = 0;
4232
4233         if (p) {
4234                 bssid->DSConfig = p[2];
4235         } else {/*  In 5G, some ap do not have DSSET IE */
4236                 /*  checking HT info for channel */
4237                 p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, bssid->IEs,
4238                                      bssid->IELength);
4239                 if (p) {
4240                         struct ieee80211_ht_operation *HT_info =
4241                                 (struct ieee80211_ht_operation *)(p + 2);
4242                         bssid->DSConfig = HT_info->primary_chan;
4243                 } else /*  use current channel */
4244                         bssid->DSConfig = rtw_get_oper_ch23a(padapter);
4245         }
4246
4247         if (ieee80211_is_probe_req(mgmt->frame_control)) {
4248                 /*  FIXME */
4249                 bssid->ifmode = NL80211_IFTYPE_STATION;
4250                 ether_addr_copy(bssid->MacAddress, mgmt->sa);
4251                 bssid->Privacy = 1;
4252                 return bssid;
4253         }
4254
4255         if (bssid->capability & WLAN_CAPABILITY_ESS) {
4256                 bssid->ifmode = NL80211_IFTYPE_STATION;
4257                 ether_addr_copy(bssid->MacAddress, mgmt->sa);
4258         } else {
4259                 bssid->ifmode = NL80211_IFTYPE_ADHOC;
4260                 ether_addr_copy(bssid->MacAddress, mgmt->bssid);
4261         }
4262
4263         if (bssid->capability & WLAN_CAPABILITY_PRIVACY)
4264                 bssid->Privacy = 1;
4265         else
4266                 bssid->Privacy = 0;
4267
4268         bssid->ATIMWindow = 0;
4269
4270         /* 20/40 BSS Coexistence check */
4271         if (pregistrypriv->wifi_spec == 1 &&
4272             pmlmeinfo->bwmode_updated == false) {
4273                 struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
4274
4275                 p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, bssid->IEs,
4276                                      bssid->IELength);
4277                 if (p && p[1] > 0) {
4278                         struct ieee80211_ht_cap *pHT_caps;
4279
4280                         pHT_caps = (struct ieee80211_ht_cap *)(p + 2);
4281
4282                         if (pHT_caps->cap_info &
4283                             cpu_to_le16(IEEE80211_HT_CAP_40MHZ_INTOLERANT))
4284                                 pmlmepriv->num_FortyMHzIntolerant++;
4285                 } else
4286                         pmlmepriv->num_sta_no_ht++;
4287         }
4288
4289
4290         /*  mark bss info receiving from nearby channel as SignalQuality 101 */
4291         if (bssid->DSConfig != rtw_get_oper_ch23a(padapter))
4292                 bssid->SignalQuality = 101;
4293
4294         return bssid;
4295 fail:
4296         kfree (bssid);
4297         return NULL;
4298 }
4299
4300 static void start_create_ibss(struct rtw_adapter *padapter)
4301 {
4302         unsigned short caps;
4303         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4304         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4305         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
4306
4307         pmlmeext->cur_channel = (u8)pnetwork->DSConfig;
4308         pmlmeinfo->bcn_interval = pnetwork->beacon_interval;
4309
4310         /* update wireless mode */
4311         update_wireless_mode23a(padapter);
4312
4313         /* update capability */
4314         caps = pnetwork->capability;
4315         update_capinfo23a(padapter, caps);
4316         if (caps & WLAN_CAPABILITY_IBSS) {      /* adhoc master */
4317                 rtl8723a_set_sec_cfg(padapter, 0xcf);
4318
4319                 /* switch channel */
4320                 /* SelectChannel23a(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */
4321                 set_channel_bwmode23a(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
4322
4323                 rtl8723a_SetBeaconRelatedRegisters(padapter);
4324
4325                 /* set msr to MSR_ADHOC */
4326                 pmlmeinfo->state = MSR_ADHOC;
4327                 rtl8723a_set_media_status(padapter, pmlmeinfo->state & 0x3);
4328
4329                 /* issue beacon */
4330                 if (send_beacon23a(padapter) == _FAIL) {
4331                         RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
4332                                  "issuing beacon frame fail....\n");
4333
4334                         report_join_res23a(padapter, -1);
4335                         pmlmeinfo->state = MSR_NOLINK;
4336                 } else {
4337                         hw_var_set_bssid(padapter, padapter->registrypriv.dev_network.MacAddress);
4338                         hw_var_set_mlme_join(padapter, 0);
4339
4340                         report_join_res23a(padapter, 1);
4341                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
4342                 }
4343         } else {
4344                 DBG_8723A("%s: invalid cap:%x\n", __func__, caps);
4345                 return;
4346         }
4347 }
4348
4349 static void start_clnt_join(struct rtw_adapter *padapter)
4350 {
4351         unsigned short caps;
4352         u8 val8;
4353         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4354         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4355         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
4356         int beacon_timeout;
4357
4358         pmlmeext->cur_channel = (u8)pnetwork->DSConfig;
4359         pmlmeinfo->bcn_interval = pnetwork->beacon_interval;
4360
4361         /* update wireless mode */
4362         update_wireless_mode23a(padapter);
4363
4364         /* update capability */
4365         caps = pnetwork->capability;
4366         update_capinfo23a(padapter, caps);
4367         if (caps & WLAN_CAPABILITY_ESS) {
4368                 /* switch channel */
4369                 set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
4370
4371                 rtl8723a_set_media_status(padapter, MSR_INFRA);
4372
4373                 val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ?
4374                         0xcc: 0xcf;
4375
4376                 rtl8723a_set_sec_cfg(padapter, val8);
4377
4378                 /* switch channel */
4379                 /* set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
4380
4381                 /* here wait for receiving the beacon to start auth */
4382                 /* and enable a timer */
4383                 beacon_timeout = decide_wait_for_beacon_timeout23a(pmlmeinfo->bcn_interval);
4384                 set_link_timer(pmlmeext, beacon_timeout);
4385                 mod_timer(&padapter->mlmepriv.assoc_timer, jiffies +
4386                           msecs_to_jiffies((REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) + beacon_timeout));
4387                 pmlmeinfo->state = WIFI_FW_AUTH_NULL | MSR_INFRA;
4388         } else if (caps & WLAN_CAPABILITY_IBSS) {       /* adhoc client */
4389                 rtl8723a_set_media_status(padapter, MSR_ADHOC);
4390
4391                 rtl8723a_set_sec_cfg(padapter, 0xcf);
4392
4393                 /* switch channel */
4394                 set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
4395
4396                 rtl8723a_SetBeaconRelatedRegisters(padapter);
4397
4398                 pmlmeinfo->state = MSR_ADHOC;
4399
4400                 report_join_res23a(padapter, 1);
4401         } else {
4402                 /* DBG_8723A("marc: invalid cap:%x\n", caps); */
4403                 return;
4404         }
4405 }
4406
4407 static void start_clnt_auth(struct rtw_adapter *padapter)
4408 {
4409         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4410         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4411
4412         del_timer_sync(&pmlmeext->link_timer);
4413
4414         pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
4415         pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
4416
4417         pmlmeinfo->auth_seq = 1;
4418         pmlmeinfo->reauth_count = 0;
4419         pmlmeinfo->reassoc_count = 0;
4420         pmlmeinfo->link_count = 0;
4421         pmlmeext->retry = 0;
4422
4423         /*  Because of AP's not receiving deauth before */
4424         /*  AP may: 1)not response auth or 2)deauth us after link is complete */
4425         /*  issue deauth before issuing auth to deal with the situation */
4426         /*      Commented by Albert 2012/07/21 */
4427         /*      For the Win8 P2P connection, it will be hard to have a
4428                 successful connection if this Wi-Fi doesn't connect to it. */
4429         issue_deauth23a(padapter, (&pmlmeinfo->network)->MacAddress,
4430                         WLAN_REASON_DEAUTH_LEAVING);
4431
4432         DBG_8723A_LEVEL(_drv_always_, "start auth\n");
4433         issue_auth(padapter, NULL, 0);
4434
4435         set_link_timer(pmlmeext, REAUTH_TO);
4436 }
4437
4438 static void start_clnt_assoc(struct rtw_adapter *padapter)
4439 {
4440         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4441         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4442
4443         del_timer_sync(&pmlmeext->link_timer);
4444
4445         pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
4446         pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
4447
4448         issue_assocreq(padapter);
4449
4450         set_link_timer(pmlmeext, REASSOC_TO);
4451 }
4452
4453 int receive_disconnect23a(struct rtw_adapter *padapter,
4454                           unsigned char *MacAddr, unsigned short reason)
4455 {
4456         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4457         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4458
4459         /* check A3 */
4460         if (!ether_addr_equal(MacAddr, get_my_bssid23a(&pmlmeinfo->network)))
4461                 return _SUCCESS;
4462
4463         DBG_8723A("%s\n", __func__);
4464
4465         if ((pmlmeinfo->state&0x03) == MSR_INFRA) {
4466                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
4467                         pmlmeinfo->state = MSR_NOLINK;
4468                         report_del_sta_event23a(padapter, MacAddr, reason);
4469
4470                 } else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) {
4471                         pmlmeinfo->state = MSR_NOLINK;
4472                         report_join_res23a(padapter, -2);
4473                 }
4474         }
4475
4476         return _SUCCESS;
4477 }
4478
4479 static void process_80211d(struct rtw_adapter *padapter,
4480                            struct wlan_bssid_ex *bssid)
4481 {
4482         struct registry_priv *pregistrypriv;
4483         struct mlme_ext_priv *pmlmeext;
4484         struct rt_channel_info *chplan_new;
4485         u8 channel;
4486         u8 i;
4487
4488         pregistrypriv = &padapter->registrypriv;
4489         pmlmeext = &padapter->mlmeextpriv;
4490
4491         /*  Adjust channel plan by AP Country IE */
4492         if (pregistrypriv->enable80211d &&
4493             !pmlmeext->update_channel_plan_by_ap_done) {
4494                 const u8 *ie, *p;
4495                 struct rt_channel_plan chplan_ap;
4496                 struct rt_channel_info chplan_sta[MAX_CHANNEL_NUM];
4497                 u8 country[4];
4498                 u8 fcn; /*  first channel number */
4499                 u8 noc; /*  number of channel */
4500                 u8 j, k;
4501
4502                 ie = cfg80211_find_ie(WLAN_EID_COUNTRY, bssid->IEs,
4503                                       bssid->IELength);
4504                 if (!ie || ie[1] < IEEE80211_COUNTRY_IE_MIN_LEN)
4505                         return;
4506
4507                 p = ie + 2;
4508                 ie += ie[1];
4509                 ie += 2;
4510
4511                 memcpy(country, p, 3);
4512                 country[3] = '\0';
4513
4514                 p += 3;
4515                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
4516                          "%s: 802.11d country =%s\n", __func__, country);
4517
4518                 i = 0;
4519                 while ((ie - p) >= 3) {
4520                         fcn = *(p++);
4521                         noc = *(p++);
4522                         p++;
4523
4524                         for (j = 0; j < noc; j++) {
4525                                 if (fcn <= 14)
4526                                         channel = fcn + j; /*  2.4 GHz */
4527                                 else
4528                                         channel = fcn + j * 4; /*  5 GHz */
4529
4530                                 chplan_ap.Channel[i++] = channel;
4531                         }
4532                 }
4533                 chplan_ap.Len = i;
4534
4535                 memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
4536                 memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
4537                 chplan_new = pmlmeext->channel_set;
4538
4539                 i = j = k = 0;
4540                 if (pregistrypriv->wireless_mode & WIRELESS_11G) {
4541                         do {
4542                                 if (i == MAX_CHANNEL_NUM ||
4543                                     chplan_sta[i].ChannelNum == 0 ||
4544                                     chplan_sta[i].ChannelNum > 14)
4545                                         break;
4546
4547                                 if (j == chplan_ap.Len ||
4548                                     chplan_ap.Channel[j] > 14)
4549                                         break;
4550
4551                                 if (chplan_sta[i].ChannelNum ==
4552                                     chplan_ap.Channel[j]) {
4553                                         chplan_new[k].ChannelNum =
4554                                                 chplan_ap.Channel[j];
4555                                         chplan_new[k].ScanType = SCAN_ACTIVE;
4556                                         i++;
4557                                         j++;
4558                                         k++;
4559                                 } else if (chplan_sta[i].ChannelNum <
4560                                            chplan_ap.Channel[j]) {
4561                                         chplan_new[k].ChannelNum =
4562                                                 chplan_sta[i].ChannelNum;
4563                                         chplan_new[k].ScanType =
4564                                                 SCAN_PASSIVE;
4565                                         i++;
4566                                         k++;
4567                                 } else if (chplan_sta[i].ChannelNum >
4568                                            chplan_ap.Channel[j]) {
4569                                         chplan_new[k].ChannelNum =
4570                                                 chplan_ap.Channel[j];
4571                                         chplan_new[k].ScanType =
4572                                                 SCAN_ACTIVE;
4573                                         j++;
4574                                         k++;
4575                                 }
4576                         } while (1);
4577
4578                         /*  change AP not support channel to Passive scan */
4579                         while (i < MAX_CHANNEL_NUM &&
4580                                chplan_sta[i].ChannelNum != 0 &&
4581                                chplan_sta[i].ChannelNum <= 14) {
4582                                 chplan_new[k].ChannelNum =
4583                                         chplan_sta[i].ChannelNum;
4584                                 chplan_new[k].ScanType = SCAN_PASSIVE;
4585                                 i++;
4586                                 k++;
4587                         }
4588
4589                         /*  add channel AP supported */
4590                         while (j < chplan_ap.Len && chplan_ap.Channel[j] <= 14){
4591                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
4592                                 chplan_new[k].ScanType = SCAN_ACTIVE;
4593                                 j++;
4594                                 k++;
4595                         }
4596                 } else {
4597                         /*  keep original STA 2.4G channel plan */
4598                         while (i < MAX_CHANNEL_NUM &&
4599                                chplan_sta[i].ChannelNum != 0 &&
4600                                chplan_sta[i].ChannelNum <= 14) {
4601                                 chplan_new[k].ChannelNum =
4602                                         chplan_sta[i].ChannelNum;
4603                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
4604                                 i++;
4605                                 k++;
4606                         }
4607
4608                         /*  skip AP 2.4G channel plan */
4609                         while (j < chplan_ap.Len && chplan_ap.Channel[j] <= 14)
4610                                 j++;
4611                 }
4612
4613                 if (pregistrypriv->wireless_mode & WIRELESS_11A) {
4614                         do {
4615                                 if (i == MAX_CHANNEL_NUM ||
4616                                     chplan_sta[i].ChannelNum == 0)
4617                                         break;
4618
4619                                 if (j == chplan_ap.Len ||
4620                                     chplan_ap.Channel[j] == 0)
4621                                         break;
4622
4623                                 if (chplan_sta[i].ChannelNum ==
4624                                     chplan_ap.Channel[j]) {
4625                                         chplan_new[k].ChannelNum =
4626                                                 chplan_ap.Channel[j];
4627                                         chplan_new[k].ScanType = SCAN_ACTIVE;
4628                                         i++;
4629                                         j++;
4630                                         k++;
4631                                 } else if (chplan_sta[i].ChannelNum <
4632                                            chplan_ap.Channel[j]) {
4633                                         chplan_new[k].ChannelNum =
4634                                                 chplan_sta[i].ChannelNum;
4635                                         chplan_new[k].ScanType = SCAN_PASSIVE;
4636                                         i++;
4637                                         k++;
4638                                 } else if (chplan_sta[i].ChannelNum >
4639                                            chplan_ap.Channel[j]) {
4640                                         chplan_new[k].ChannelNum =
4641                                                 chplan_ap.Channel[j];
4642                                         chplan_new[k].ScanType = SCAN_ACTIVE;
4643                                         j++;
4644                                         k++;
4645                                 }
4646                         } while (1);
4647
4648                         /*  change AP not support channel to Passive scan */
4649                         while (i < MAX_CHANNEL_NUM &&
4650                                chplan_sta[i].ChannelNum != 0) {
4651                                 chplan_new[k].ChannelNum =
4652                                         chplan_sta[i].ChannelNum;
4653                                 chplan_new[k].ScanType = SCAN_PASSIVE;
4654                                 i++;
4655                                 k++;
4656                         }
4657
4658                         /*  add channel AP supported */
4659                         while (j < chplan_ap.Len && chplan_ap.Channel[j] != 0) {
4660                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
4661                                 chplan_new[k].ScanType = SCAN_ACTIVE;
4662                                 j++;
4663                                 k++;
4664                         }
4665                 } else {
4666                         /*  keep original STA 5G channel plan */
4667                         while (i < MAX_CHANNEL_NUM &&
4668                                chplan_sta[i].ChannelNum != 0) {
4669                                 chplan_new[k].ChannelNum =
4670                                         chplan_sta[i].ChannelNum;
4671                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
4672                                 i++;
4673                                 k++;
4674                         }
4675                 }
4676                 pmlmeext->update_channel_plan_by_ap_done = 1;
4677         }
4678
4679         /*  If channel is used by AP, set channel scan type to active */
4680         channel = bssid->DSConfig;
4681         chplan_new = pmlmeext->channel_set;
4682         i = 0;
4683         while (i < MAX_CHANNEL_NUM && chplan_new[i].ChannelNum != 0) {
4684                 if (chplan_new[i].ChannelNum == channel) {
4685                         if (chplan_new[i].ScanType == SCAN_PASSIVE) {
4686                                 /* 5G Bnad 2, 3 (DFS) doesn't change
4687                                    to active scan */
4688                                 if (channel >= 52 && channel <= 144)
4689                                         break;
4690
4691                                 chplan_new[i].ScanType = SCAN_ACTIVE;
4692                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
4693                                          "%s: change channel %d scan type from passive to active\n",
4694                                          __func__, channel);
4695                         }
4696                         break;
4697                 }
4698                 i++;
4699         }
4700 }
4701
4702 /****************************************************************************
4703
4704 Following are the functions to report events
4705
4706 *****************************************************************************/
4707
4708 void report_survey_event23a(struct rtw_adapter *padapter,
4709                             struct recv_frame *precv_frame)
4710 {
4711         struct cmd_obj *pcmd_obj;
4712         u8 *pevtcmd;
4713         u32 cmdsz;
4714         struct survey_event *psurvey_evt;
4715         struct C2HEvent_Header *pc2h_evt_hdr;
4716         struct mlme_ext_priv *pmlmeext;
4717         struct cmd_priv *pcmdpriv;
4718
4719         if (!padapter)
4720                 return;
4721
4722         pmlmeext = &padapter->mlmeextpriv;
4723         pcmdpriv = &padapter->cmdpriv;
4724
4725         pcmd_obj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
4726         if (!pcmd_obj)
4727                 return;
4728
4729         cmdsz = sizeof(struct survey_event) + sizeof(struct C2HEvent_Header);
4730         pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
4731         if (!pevtcmd) {
4732                 kfree(pcmd_obj);
4733                 return;
4734         }
4735
4736         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
4737         pcmd_obj->cmdsz = cmdsz;
4738         pcmd_obj->parmbuf = pevtcmd;
4739
4740         pcmd_obj->rsp = NULL;
4741         pcmd_obj->rspsz  = 0;
4742
4743         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
4744         pc2h_evt_hdr->len = sizeof(struct survey_event);
4745         pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
4746         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
4747
4748         psurvey_evt = (struct survey_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
4749
4750         psurvey_evt->bss = collect_bss_info(padapter, precv_frame);
4751         if (!psurvey_evt->bss) {
4752                 kfree(pcmd_obj);
4753                 kfree(pevtcmd);
4754                 return;
4755         }
4756
4757         process_80211d(padapter, psurvey_evt->bss);
4758
4759         rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj);
4760
4761         pmlmeext->sitesurvey_res.bss_cnt++;
4762
4763         return;
4764 }
4765
4766 void report_surveydone_event23a(struct rtw_adapter *padapter)
4767 {
4768         struct cmd_obj *pcmd_obj;
4769         u8 *pevtcmd;
4770         u32 cmdsz;
4771         struct surveydone_event *psurveydone_evt;
4772         struct C2HEvent_Header *pc2h_evt_hdr;
4773         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4774         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
4775
4776         pcmd_obj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
4777         if (!pcmd_obj)
4778                 return;
4779
4780         cmdsz = sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header);
4781         pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
4782         if (!pevtcmd) {
4783                 kfree(pcmd_obj);
4784                 return;
4785         }
4786
4787         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
4788         pcmd_obj->cmdsz = cmdsz;
4789         pcmd_obj->parmbuf = pevtcmd;
4790
4791         pcmd_obj->rsp = NULL;
4792         pcmd_obj->rspsz  = 0;
4793
4794         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
4795         pc2h_evt_hdr->len = sizeof(struct surveydone_event);
4796         pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
4797         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
4798
4799         psurveydone_evt = (struct surveydone_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
4800         psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
4801
4802         DBG_8723A("survey done event(%x)\n", psurveydone_evt->bss_cnt);
4803
4804         rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj);
4805
4806         return;
4807 }
4808
4809 void report_join_res23a(struct rtw_adapter *padapter, int res)
4810 {
4811         struct cmd_obj *pcmd_obj;
4812         u8 *pevtcmd;
4813         u32 cmdsz;
4814         struct joinbss_event            *pjoinbss_evt;
4815         struct C2HEvent_Header  *pc2h_evt_hdr;
4816         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
4817         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4818         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
4819
4820         pcmd_obj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
4821         if (!pcmd_obj)
4822                 return;
4823
4824         cmdsz = sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header);
4825         pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
4826         if (!pevtcmd) {
4827                 kfree(pcmd_obj);
4828                 return;
4829         }
4830
4831         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
4832         pcmd_obj->cmdsz = cmdsz;
4833         pcmd_obj->parmbuf = pevtcmd;
4834
4835         pcmd_obj->rsp = NULL;
4836         pcmd_obj->rspsz  = 0;
4837
4838         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
4839         pc2h_evt_hdr->len = sizeof(struct joinbss_event);
4840         pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
4841         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
4842
4843         pjoinbss_evt = (struct joinbss_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
4844         memcpy((unsigned char *)&pjoinbss_evt->network.network,
4845                &pmlmeinfo->network, sizeof(struct wlan_bssid_ex));
4846         pjoinbss_evt->network.join_res = res;
4847
4848         DBG_8723A("report_join_res23a(%d)\n", res);
4849
4850         rtw_joinbss_event_prehandle23a(padapter, (u8 *)&pjoinbss_evt->network);
4851
4852         rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj);
4853
4854         return;
4855 }
4856
4857 void report_del_sta_event23a(struct rtw_adapter *padapter,
4858                              unsigned char *MacAddr, unsigned short reason)
4859 {
4860         struct cmd_obj *pcmd_obj;
4861         u8 *pevtcmd;
4862         u32 cmdsz;
4863         struct sta_info *psta;
4864         int mac_id;
4865         struct stadel_event *pdel_sta_evt;
4866         struct C2HEvent_Header *pc2h_evt_hdr;
4867         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4868         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
4869
4870         pcmd_obj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
4871         if (!pcmd_obj)
4872                 return;
4873
4874         cmdsz = sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header);
4875         pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
4876         if (!pevtcmd) {
4877                 kfree(pcmd_obj);
4878                 return;
4879         }
4880
4881         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
4882         pcmd_obj->cmdsz = cmdsz;
4883         pcmd_obj->parmbuf = pevtcmd;
4884
4885         pcmd_obj->rsp = NULL;
4886         pcmd_obj->rspsz  = 0;
4887
4888         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
4889         pc2h_evt_hdr->len = sizeof(struct stadel_event);
4890         pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
4891         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
4892
4893         pdel_sta_evt = (struct stadel_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
4894         ether_addr_copy((unsigned char *)&pdel_sta_evt->macaddr, MacAddr);
4895         memcpy((unsigned char *)pdel_sta_evt->rsvd, (unsigned char *)&reason,
4896                2);
4897
4898         psta = rtw_get_stainfo23a(&padapter->stapriv, MacAddr);
4899         if (psta)
4900                 mac_id = (int)psta->mac_id;
4901         else
4902                 mac_id = -1;
4903
4904         pdel_sta_evt->mac_id = mac_id;
4905
4906         DBG_8723A("report_del_sta_event23a: delete STA, mac_id =%d\n", mac_id);
4907
4908         rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj);
4909
4910         return;
4911 }
4912
4913 void report_add_sta_event23a(struct rtw_adapter *padapter,
4914                              unsigned char *MacAddr, int cam_idx)
4915 {
4916         struct cmd_obj *pcmd_obj;
4917         u8 *pevtcmd;
4918         u32 cmdsz;
4919         struct stassoc_event *padd_sta_evt;
4920         struct C2HEvent_Header *pc2h_evt_hdr;
4921         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4922         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
4923
4924         pcmd_obj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
4925         if (!pcmd_obj)
4926                 return;
4927
4928         cmdsz = sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header);
4929         pevtcmd = kzalloc(cmdsz, GFP_ATOMIC);
4930         if (!pevtcmd) {
4931                 kfree(pcmd_obj);
4932                 return;
4933         }
4934
4935         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
4936         pcmd_obj->cmdsz = cmdsz;
4937         pcmd_obj->parmbuf = pevtcmd;
4938
4939         pcmd_obj->rsp = NULL;
4940         pcmd_obj->rspsz  = 0;
4941
4942         pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
4943         pc2h_evt_hdr->len = sizeof(struct stassoc_event);
4944         pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
4945         pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
4946
4947         padd_sta_evt = (struct stassoc_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
4948         ether_addr_copy((unsigned char *)&padd_sta_evt->macaddr, MacAddr);
4949         padd_sta_evt->cam_id = cam_idx;
4950
4951         DBG_8723A("report_add_sta_event23a: add STA\n");
4952
4953         rtw_enqueue_cmd23a(pcmdpriv, pcmd_obj);
4954
4955         return;
4956 }
4957
4958 /****************************************************************************
4959
4960 Following are the event callback functions
4961
4962 *****************************************************************************/
4963
4964 /* for sta/adhoc mode */
4965 void update_sta_info23a(struct rtw_adapter *padapter, struct sta_info *psta)
4966 {
4967         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
4968         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
4969         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
4970
4971         /* ERP */
4972         VCS_update23a(padapter, psta);
4973
4974         /* HT */
4975         if (pmlmepriv->htpriv.ht_option) {
4976                 psta->htpriv.ht_option = true;
4977
4978                 psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
4979
4980                 if (support_short_GI23a(padapter, &pmlmeinfo->ht_cap))
4981                         psta->htpriv.sgi = true;
4982
4983                 psta->qos_option = true;
4984
4985         } else {
4986                 psta->htpriv.ht_option = false;
4987
4988                 psta->htpriv.ampdu_enable = false;
4989
4990                 psta->htpriv.sgi = false;
4991                 psta->qos_option = false;
4992
4993         }
4994         psta->htpriv.bwmode = pmlmeext->cur_bwmode;
4995         psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
4996
4997         psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
4998         psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
4999
5000         /* QoS */
5001         if (pmlmepriv->qos_option)
5002                 psta->qos_option = true;
5003
5004         psta->state = _FW_LINKED;
5005 }
5006
5007 void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter,
5008                                        int join_res)
5009 {
5010         struct sta_info *psta, *psta_bmc;
5011         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5012         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5013         struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
5014         struct sta_priv *pstapriv = &padapter->stapriv;
5015
5016         if (join_res < 0) {
5017                 hw_var_set_mlme_join(padapter, 1);
5018                 hw_var_set_bssid(padapter, null_addr);
5019
5020                 /* restore to initial setting. */
5021                 update_tx_basic_rate23a(padapter,
5022                                         padapter->registrypriv.wireless_mode);
5023
5024                 goto exit_mlmeext_joinbss_event_callback23a;
5025         }
5026
5027         if ((pmlmeinfo->state&0x03) == MSR_ADHOC) {
5028                 /* for bc/mc */
5029                 psta_bmc = rtw_get_bcmc_stainfo23a(padapter);
5030                 if (psta_bmc) {
5031                         pmlmeinfo->FW_sta_info[psta_bmc->mac_id].psta = psta_bmc;
5032                         update_bmc_sta_support_rate23a(padapter, psta_bmc->mac_id);
5033                         Update_RA_Entry23a(padapter, psta_bmc);
5034                 }
5035         }
5036
5037         /* turn on dynamic functions */
5038         rtl8723a_odm_support_ability_set(padapter, DYNAMIC_ALL_FUNC_ENABLE);
5039
5040         /*  update IOT-releated issue */
5041         update_IOT_info23a(padapter);
5042
5043         HalSetBrateCfg23a(padapter, cur_network->SupportedRates);
5044
5045         /* BCN interval */
5046         rtl8723a_set_beacon_interval(padapter, pmlmeinfo->bcn_interval);
5047
5048         /* update capability */
5049         update_capinfo23a(padapter, pmlmeinfo->capability);
5050
5051         /* WMM, Update EDCA param */
5052         WMMOnAssocRsp23a(padapter);
5053
5054         /* HT */
5055         HTOnAssocRsp23a(padapter);
5056
5057         /* Set cur_channel&cur_bwmode&cur_ch_offset */
5058         set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
5059
5060         psta = rtw_get_stainfo23a(pstapriv, cur_network->MacAddress);
5061         if (psta) { /* only for infra. mode */
5062                 pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
5063
5064                 /* DBG_8723A("set_sta_rate23a\n"); */
5065
5066                 psta->wireless_mode = pmlmeext->cur_wireless_mode;
5067
5068                 /* set per sta rate after updating HT cap. */
5069                 set_sta_rate23a(padapter, psta);
5070         }
5071
5072         hw_var_set_mlme_join(padapter, 2);
5073
5074         if ((pmlmeinfo->state&0x03) == MSR_INFRA) {
5075                 /*  correcting TSF */
5076                 rtw_correct_TSF(padapter);
5077
5078                 /* set_link_timer(pmlmeext, DISCONNECT_TO); */
5079         }
5080
5081         rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_CONNECT, 0);
5082
5083 exit_mlmeext_joinbss_event_callback23a:
5084         DBG_8723A("=>%s\n", __func__);
5085 }
5086
5087 void mlmeext_sta_add_event_callback23a(struct rtw_adapter *padapter,
5088                                        struct sta_info *psta)
5089 {
5090         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5091         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5092
5093         DBG_8723A("%s\n", __func__);
5094
5095         if ((pmlmeinfo->state & 0x03) == MSR_ADHOC) {
5096         /* adhoc master or sta_count>1 */
5097                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
5098                         /* nothing to do */
5099                 } else { /* adhoc client */
5100                         /*  correcting TSF */
5101                         rtw_correct_TSF(padapter);
5102
5103                         /* start beacon */
5104                         if (send_beacon23a(padapter) != _SUCCESS) {
5105                                 pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
5106
5107                                 pmlmeinfo->state ^= MSR_ADHOC;
5108
5109                                 return;
5110                         }
5111
5112                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
5113                 }
5114                 hw_var_set_mlme_join(padapter, 2);
5115         }
5116
5117         pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
5118
5119         /* rate radaptive */
5120         Update_RA_Entry23a(padapter, psta);
5121
5122         /* update adhoc sta_info */
5123         update_sta_info23a(padapter, psta);
5124 }
5125
5126 void mlmeext_sta_del_event_callback23a(struct rtw_adapter *padapter)
5127 {
5128         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5129         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5130
5131         if (is_client_associated_to_ap23a(padapter) ||
5132             is_IBSS_empty23a(padapter)) {
5133                 /* set_opmode_cmd(padapter, infra_client_with_mlme); */
5134
5135                 hw_var_set_mlme_disconnect(padapter);
5136                 hw_var_set_bssid(padapter, null_addr);
5137
5138                 /* restore to initial setting. */
5139                 update_tx_basic_rate23a(padapter,
5140                                         padapter->registrypriv.wireless_mode);
5141
5142                 /* switch to the 20M Hz mode after disconnect */
5143                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
5144                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
5145
5146                 set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
5147                                       pmlmeext->cur_ch_offset,
5148                                       pmlmeext->cur_bwmode);
5149
5150                 flush_all_cam_entry23a(padapter);
5151
5152                 pmlmeinfo->state = MSR_NOLINK;
5153
5154                 /* set MSR to no link state -> infra. mode */
5155                 rtl8723a_set_media_status(padapter, MSR_INFRA);
5156
5157                 del_timer_sync(&pmlmeext->link_timer);
5158         }
5159 }
5160
5161 static u8 chk_ap_is_alive(struct rtw_adapter *padapter, struct sta_info *psta)
5162 {
5163         u8 ret = false;
5164
5165         if (sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta) &&
5166             sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta) &&
5167             sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta))
5168                 ret = false;
5169         else
5170                 ret = true;
5171
5172         sta_update_last_rx_pkts(psta);
5173         return ret;
5174 }
5175
5176 void linked_status_chk23a(struct rtw_adapter *padapter)
5177 {
5178         u32 i;
5179         struct sta_info *psta;
5180         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
5181         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5182         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5183         struct sta_priv *pstapriv = &padapter->stapriv;
5184
5185         if (is_client_associated_to_ap23a(padapter)) {
5186                 /* linked infrastructure client mode */
5187
5188                 int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
5189                 int rx_chk_limit;
5190
5191                 rx_chk_limit = 4;
5192
5193                 psta = rtw_get_stainfo23a(pstapriv,
5194                                           pmlmeinfo->network.MacAddress);
5195                 if (psta) {
5196                         bool is_p2p_enable = false;
5197
5198                         if (chk_ap_is_alive(padapter, psta) == false)
5199                                 rx_chk = _FAIL;
5200
5201                         if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
5202                                 tx_chk = _FAIL;
5203
5204                         if (pmlmeext->active_keep_alive_check &&
5205                             (rx_chk == _FAIL || tx_chk == _FAIL)) {
5206                                 u8 backup_oper_channel = 0;
5207
5208                                 /* switch to correct channel of current
5209                                    network  before issue keep-alive frames */
5210                                 if (rtw_get_oper_ch23a(padapter) !=
5211                                     pmlmeext->cur_channel) {
5212                                         backup_oper_channel =
5213                                                 rtw_get_oper_ch23a(padapter);
5214                                         SelectChannel23a(padapter,
5215                                                          pmlmeext->cur_channel);
5216                                 }
5217
5218                                 if (rx_chk != _SUCCESS)
5219                                         issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
5220
5221                                 if ((tx_chk != _SUCCESS &&
5222                                      pmlmeinfo->link_count++ == 0xf) ||
5223                                     rx_chk != _SUCCESS) {
5224                                         tx_chk = issue_nulldata23a(padapter,
5225                                                                    psta->hwaddr,
5226                                                                    0, 3, 1);
5227                                         /* if tx acked and p2p disabled,
5228                                            set rx_chk _SUCCESS to reset retry
5229                                            count */
5230                                         if (tx_chk == _SUCCESS &&
5231                                             !is_p2p_enable)
5232                                                 rx_chk = _SUCCESS;
5233                                 }
5234
5235                                 /* back to the original operation channel */
5236                                 if (backup_oper_channel>0)
5237                                         SelectChannel23a(padapter,
5238                                                          backup_oper_channel);
5239                         } else {
5240                                 if (rx_chk != _SUCCESS) {
5241                                         if (pmlmeext->retry == 0) {
5242                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
5243                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
5244                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
5245                                         }
5246                                 }
5247
5248                                 if (tx_chk != _SUCCESS &&
5249                                     pmlmeinfo->link_count++ == 0xf)
5250                                         tx_chk = issue_nulldata23a(padapter,
5251                                                                    NULL, 0, 1,
5252                                                                    0);
5253                         }
5254
5255                         if (rx_chk == _FAIL) {
5256                                 pmlmeext->retry++;
5257                                 if (pmlmeext->retry > rx_chk_limit) {
5258                                         DBG_8723A_LEVEL(_drv_always_,
5259                                                         "%s(%s): disconnect or "
5260                                                         "roaming\n", __func__,
5261                                                         padapter->pnetdev->name);
5262                                         receive_disconnect23a(padapter, pmlmeinfo->network.MacAddress,
5263                                                 WLAN_REASON_EXPIRATION_CHK);
5264                                         return;
5265                                 }
5266                         } else
5267                                 pmlmeext->retry = 0;
5268
5269                         if (tx_chk == _FAIL)
5270                                 pmlmeinfo->link_count &= 0xf;
5271                         else {
5272                                 pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
5273                                 pmlmeinfo->link_count = 0;
5274                         }
5275
5276                 }
5277         } else if (is_client_associated_to_ibss23a(padapter)) {
5278                 /* linked IBSS mode */
5279                 /* for each assoc list entry to check the rx pkt counter */
5280                 for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
5281                         if (pmlmeinfo->FW_sta_info[i].status == 1) {
5282                                 psta = pmlmeinfo->FW_sta_info[i].psta;
5283
5284                                 if (!psta)
5285                                         continue;
5286
5287                                 if (pmlmeinfo->FW_sta_info[i].rx_pkt ==
5288                                     sta_rx_pkts(psta)) {
5289
5290                                         if (pmlmeinfo->FW_sta_info[i].retry<3) {
5291                                                 pmlmeinfo->FW_sta_info[i].retry++;
5292                                         } else {
5293                                                 pmlmeinfo->FW_sta_info[i].retry = 0;
5294                                                 pmlmeinfo->FW_sta_info[i].status = 0;
5295                                                 report_del_sta_event23a(padapter, psta->hwaddr,
5296                                                         65535/*  indicate disconnect caused by no rx */
5297                                                 );
5298                                         }
5299                                 } else {
5300                                         pmlmeinfo->FW_sta_info[i].retry = 0;
5301                                         pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
5302                                 }
5303                         }
5304                 }
5305                 /* set_link_timer(pmlmeext, DISCONNECT_TO); */
5306         }
5307 }
5308
5309 static void survey_timer_hdl(unsigned long data)
5310 {
5311         struct rtw_adapter *padapter = (struct rtw_adapter *)data;
5312         struct cmd_obj *ph2c;
5313         struct sitesurvey_parm *psurveyPara;
5314         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
5315         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5316
5317         /* issue rtw_sitesurvey_cmd23a */
5318         if (pmlmeext->sitesurvey_res.state > SCAN_START) {
5319                 if (pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
5320                         pmlmeext->sitesurvey_res.channel_idx++;
5321
5322                 if (pmlmeext->scan_abort == true) {
5323                         pmlmeext->sitesurvey_res.channel_idx =
5324                                 pmlmeext->sitesurvey_res.ch_num;
5325                         DBG_8723A("%s idx:%d\n", __func__,
5326                                   pmlmeext->sitesurvey_res.channel_idx);
5327
5328                         pmlmeext->scan_abort = false;/* reset */
5329                 }
5330
5331                 ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
5332                 if (!ph2c)
5333                         goto exit_survey_timer_hdl;
5334
5335                 psurveyPara = kzalloc(sizeof(struct sitesurvey_parm),
5336                                         GFP_ATOMIC);
5337                 if (!psurveyPara) {
5338                         kfree(ph2c);
5339                         goto exit_survey_timer_hdl;
5340                 }
5341
5342                 init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara,
5343                                            GEN_CMD_CODE(_SiteSurvey));
5344                 rtw_enqueue_cmd23a(pcmdpriv, ph2c);
5345         }
5346
5347 exit_survey_timer_hdl:
5348         return;
5349 }
5350
5351 static void link_timer_hdl(unsigned long data)
5352 {
5353         struct rtw_adapter *padapter = (struct rtw_adapter *)data;
5354         /* static unsigned int          rx_pkt = 0; */
5355         /* static u64                           tx_cnt = 0; */
5356         /* struct xmit_priv *pxmitpriv = &padapter->xmitpriv; */
5357         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5358         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5359         /* struct sta_priv              *pstapriv = &padapter->stapriv; */
5360
5361         if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
5362                 DBG_8723A("link_timer_hdl:no beacon while connecting\n");
5363                 pmlmeinfo->state = MSR_NOLINK;
5364                 report_join_res23a(padapter, -3);
5365         } else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) {
5366                 /* re-auth timer */
5367                 if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) {
5368                         /* if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto) */
5369                         /*  */
5370                                 pmlmeinfo->state = 0;
5371                                 report_join_res23a(padapter, -1);
5372                                 return;
5373                         /*  */
5374                         /* else */
5375                         /*  */
5376                         /* pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */
5377                         /* pmlmeinfo->reauth_count = 0; */
5378                         /*  */
5379                 }
5380
5381                 DBG_8723A("link_timer_hdl: auth timeout and try again\n");
5382                 pmlmeinfo->auth_seq = 1;
5383                 issue_auth(padapter, NULL, 0);
5384                 set_link_timer(pmlmeext, REAUTH_TO);
5385         } else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) {
5386                 /* re-assoc timer */
5387                 if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) {
5388                         pmlmeinfo->state = MSR_NOLINK;
5389                         report_join_res23a(padapter, -2);
5390                         return;
5391                 }
5392
5393                 DBG_8723A("link_timer_hdl: assoc timeout and try again\n");
5394                 issue_assocreq(padapter);
5395                 set_link_timer(pmlmeext, REASSOC_TO);
5396         }
5397
5398         return;
5399 }
5400
5401 static void addba_timer_hdl(unsigned long data)
5402 {
5403         struct sta_info *psta = (struct sta_info *)data;
5404         struct ht_priv *phtpriv;
5405
5406         if (!psta)
5407                 return;
5408
5409         phtpriv = &psta->htpriv;
5410
5411         if (phtpriv->ht_option && phtpriv->ampdu_enable) {
5412                 if (phtpriv->candidate_tid_bitmap)
5413                         phtpriv->candidate_tid_bitmap = 0x0;
5414         }
5415 }
5416
5417 void init_addba_retry_timer23a(struct sta_info *psta)
5418 {
5419         setup_timer(&psta->addba_retry_timer, addba_timer_hdl,
5420                     (unsigned long)psta);
5421 }
5422
5423 void init_mlme_ext_timer23a(struct rtw_adapter *padapter)
5424 {
5425         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5426
5427         setup_timer(&pmlmeext->survey_timer, survey_timer_hdl,
5428                     (unsigned long)padapter);
5429
5430         setup_timer(&pmlmeext->link_timer, link_timer_hdl,
5431                     (unsigned long)padapter);
5432 }
5433
5434 int NULL_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5435 {
5436         return H2C_SUCCESS;
5437 }
5438
5439 int setopmode_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5440 {
5441         enum nl80211_iftype type;
5442         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5443         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5444         const struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
5445
5446         switch (psetop->mode) {
5447         case NL80211_IFTYPE_P2P_GO:
5448         case NL80211_IFTYPE_AP:
5449                 pmlmeinfo->state = MSR_AP;
5450                 type = MSR_AP;
5451                 break;
5452         case NL80211_IFTYPE_P2P_CLIENT:
5453         case NL80211_IFTYPE_STATION:
5454                 /*  clear state */
5455                 pmlmeinfo->state &= ~(BIT(0)|BIT(1));
5456                 /* set to STATION_STATE */
5457                 pmlmeinfo->state |= MSR_INFRA;
5458                 type = MSR_INFRA;
5459                 break;
5460         case NL80211_IFTYPE_ADHOC:
5461                 type = MSR_ADHOC;
5462                 break;
5463         default:
5464                 type = MSR_NOLINK;
5465                 break;
5466         }
5467
5468         hw_var_set_opmode(padapter, type);
5469         /* Set_NETYPE0_MSR(padapter, type); */
5470
5471         return H2C_SUCCESS;
5472 }
5473
5474 int createbss_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5475 {
5476         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5477         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5478         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
5479         const struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
5480         /* u32  initialgain; */
5481
5482         if (pparm->ifmode == NL80211_IFTYPE_AP ||
5483             pparm->ifmode == NL80211_IFTYPE_P2P_GO) {
5484 #ifdef CONFIG_8723AU_AP_MODE
5485                 if (pmlmeinfo->state == MSR_AP) {
5486                         /* todo: */
5487                         return H2C_SUCCESS;
5488                 }
5489 #endif
5490         }
5491
5492         /* below is for ad-hoc master */
5493         if (pparm->ifmode == NL80211_IFTYPE_ADHOC) {
5494                 rtw_joinbss_reset23a(padapter);
5495
5496                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
5497                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
5498                 pmlmeinfo->ERP_enable = 0;
5499                 pmlmeinfo->WMM_enable = 0;
5500                 pmlmeinfo->HT_enable = 0;
5501                 pmlmeinfo->HT_caps_enable = 0;
5502                 pmlmeinfo->HT_info_enable = 0;
5503
5504                 /* disable dynamic functions, such as high power, DIG */
5505                 rtl8723a_odm_support_ability_backup(padapter);
5506
5507                 rtl8723a_odm_support_ability_clr(padapter,
5508                                                  DYNAMIC_FUNC_DISABLE);
5509
5510                 /* cancel link timer */
5511                 del_timer_sync(&pmlmeext->link_timer);
5512
5513                 /* clear CAM */
5514                 flush_all_cam_entry23a(padapter);
5515
5516                 if (pparm->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
5517                         return H2C_PARAMETERS_ERROR;
5518
5519                 memcpy(pnetwork, pparm, sizeof(struct wlan_bssid_ex));
5520
5521                 start_create_ibss(padapter);
5522         }
5523
5524         return H2C_SUCCESS;
5525 }
5526
5527 int join_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5528 {
5529         struct registry_priv *pregpriv = &padapter->registrypriv;
5530         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5531         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5532         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
5533         const struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
5534         struct ieee80211_ht_operation *pht_info;
5535         u32 i;
5536         u8 *p;
5537         /* u32  initialgain; */
5538         /* u32  acparm; */
5539
5540         /* check already connecting to AP or not */
5541         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
5542                 if (pmlmeinfo->state & MSR_INFRA)
5543                         issue_deauth_ex(padapter, pnetwork->MacAddress,
5544                                         WLAN_REASON_DEAUTH_LEAVING, 5, 100);
5545
5546                 pmlmeinfo->state = MSR_NOLINK;
5547
5548                 /* clear CAM */
5549                 flush_all_cam_entry23a(padapter);
5550
5551                 del_timer_sync(&pmlmeext->link_timer);
5552
5553                 /* set MSR to nolink -> infra. mode */
5554                 rtl8723a_set_media_status(padapter, MSR_INFRA);
5555
5556                 hw_var_set_mlme_disconnect(padapter);
5557         }
5558
5559         rtw_joinbss_reset23a(padapter);
5560
5561         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
5562         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
5563         pmlmeinfo->ERP_enable = 0;
5564         pmlmeinfo->WMM_enable = 0;
5565         pmlmeinfo->HT_enable = 0;
5566         pmlmeinfo->HT_caps_enable = 0;
5567         pmlmeinfo->HT_info_enable = 0;
5568         pmlmeinfo->bwmode_updated = false;
5569         /* pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX; */
5570
5571         if (pparm->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
5572                 return H2C_PARAMETERS_ERROR;
5573
5574         memcpy(pnetwork, pbuf, sizeof(struct wlan_bssid_ex));
5575
5576         /* Check AP vendor to move rtw_joinbss_cmd23a() */
5577         /* pmlmeinfo->assoc_AP_vendor = check_assoc_AP23a(pnetwork->IEs,
5578            pnetwork->IELength); */
5579
5580         for (i = 0; i < pnetwork->IELength;) {
5581                 p = pnetwork->IEs + i;
5582
5583                 switch (p[0]) {
5584                 case WLAN_EID_VENDOR_SPECIFIC:/* Get WMM IE. */
5585                         if (!memcmp(p + 2, WMM_OUI23A, 4))
5586                                 pmlmeinfo->WMM_enable = 1;
5587                         break;
5588
5589                 case WLAN_EID_HT_CAPABILITY:    /* Get HT Cap IE. */
5590                         pmlmeinfo->HT_caps_enable = 1;
5591                         break;
5592
5593                 case WLAN_EID_HT_OPERATION:     /* Get HT Info IE. */
5594                         pmlmeinfo->HT_info_enable = 1;
5595
5596                         /* spec case only for cisco's ap because cisco's ap
5597                          * issue assoc rsp using mcs rate @40MHz or @20MHz */
5598                         pht_info = (struct ieee80211_ht_operation *)(p + 2);
5599
5600                         if (pregpriv->cbw40_enable &&
5601                             (pht_info->ht_param &
5602                              IEEE80211_HT_PARAM_CHAN_WIDTH_ANY)) {
5603                                 /* switch to the 40M Hz mode according to AP */
5604                                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
5605                                 switch (pht_info->ht_param &
5606                                         IEEE80211_HT_PARAM_CHA_SEC_OFFSET) {
5607                                 case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
5608                                         pmlmeext->cur_ch_offset =
5609                                                 HAL_PRIME_CHNL_OFFSET_LOWER;
5610                                         break;
5611
5612                                 case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
5613                                         pmlmeext->cur_ch_offset =
5614                                                 HAL_PRIME_CHNL_OFFSET_UPPER;
5615                                         break;
5616
5617                                 default:
5618                                         pmlmeext->cur_ch_offset =
5619                                                 HAL_PRIME_CHNL_OFFSET_DONT_CARE;
5620                                         break;
5621                                 }
5622
5623                                 DBG_8723A("set ch/bw before connected\n");
5624                         }
5625                         break;
5626
5627                 default:
5628                         break;
5629                 }
5630
5631                 i += (p[1] + 2);
5632         }
5633
5634         hw_var_set_bssid(padapter, pmlmeinfo->network.MacAddress);
5635         hw_var_set_mlme_join(padapter, 0);
5636
5637         /* cancel link timer */
5638         del_timer_sync(&pmlmeext->link_timer);
5639
5640         start_clnt_join(padapter);
5641
5642         return H2C_SUCCESS;
5643 }
5644
5645 int disconnect_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5646 {
5647         const struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
5648         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5649         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5650         struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
5651
5652         if (is_client_associated_to_ap23a(padapter)) {
5653                 issue_deauth_ex(padapter, pnetwork->MacAddress,
5654                                 WLAN_REASON_DEAUTH_LEAVING,
5655                                 param->deauth_timeout_ms/100, 100);
5656         }
5657
5658         /* set_opmode_cmd(padapter, infra_client_with_mlme); */
5659
5660         /* pmlmeinfo->state = MSR_NOLINK; */
5661
5662         hw_var_set_mlme_disconnect(padapter);
5663         hw_var_set_bssid(padapter, null_addr);
5664
5665         /* restore to initial setting. */
5666         update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode);
5667
5668         if ((pmlmeinfo->state & 0x03) == MSR_ADHOC ||
5669             (pmlmeinfo->state & 0x03) == MSR_AP)
5670                 rtl8723a_set_bcn_func(padapter, 0);     /* Stop BCN */
5671
5672         /* set MSR to no link state -> infra. mode */
5673         rtl8723a_set_media_status(padapter, MSR_INFRA);
5674
5675         pmlmeinfo->state = MSR_NOLINK;
5676
5677         /* switch to the 20M Hz mode after disconnect */
5678         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
5679         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
5680
5681         set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
5682                               pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
5683
5684         flush_all_cam_entry23a(padapter);
5685
5686         del_timer_sync(&pmlmeext->link_timer);
5687
5688         rtw_free_uc_swdec_pending_queue23a(padapter);
5689
5690         return H2C_SUCCESS;
5691 }
5692
5693 static int
5694 rtw_scan_ch_decision(struct rtw_adapter *padapter,
5695                      struct rtw_ieee80211_channel *out, u32 out_num,
5696                      const struct rtw_ieee80211_channel *in, u32 in_num)
5697 {
5698         int i, j;
5699         int scan_ch_num = 0;
5700         int set_idx;
5701         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5702
5703         /* clear out first */
5704         memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num);
5705
5706         /* acquire channels from in */
5707         j = 0;
5708         for (i = 0;i<in_num;i++) {
5709                 if (in[i].hw_value &&
5710                     !(in[i].flags & IEEE80211_CHAN_DISABLED) &&
5711                     (set_idx = rtw_ch_set_search_ch23a(pmlmeext->channel_set,
5712                                                        in[i].hw_value)) >= 0) {
5713                         memcpy(&out[j], &in[i],
5714                                sizeof(struct rtw_ieee80211_channel));
5715
5716                         if (pmlmeext->channel_set[set_idx].ScanType ==
5717                             SCAN_PASSIVE)
5718                                 out[j].flags &= IEEE80211_CHAN_NO_IR;
5719
5720                         j++;
5721                 }
5722                 if (j>= out_num)
5723                         break;
5724         }
5725
5726         /* if out is empty, use channel_set as default */
5727         if (j == 0) {
5728                 for (i = 0;i<pmlmeext->max_chan_nums;i++) {
5729                         out[i].hw_value = pmlmeext->channel_set[i].ChannelNum;
5730
5731                         if (pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
5732                                 out[i].flags &= IEEE80211_CHAN_NO_IR;
5733
5734                         j++;
5735                 }
5736         }
5737
5738         if (padapter->setband == GHZ_24) {                      /*  2.4G */
5739                 for (i = 0; i < j ; i++) {
5740                         if (out[i].hw_value > 35)
5741                                 memset(&out[i], 0,
5742                                        sizeof(struct rtw_ieee80211_channel));
5743                         else
5744                                 scan_ch_num++;
5745                 }
5746                 j = scan_ch_num;
5747         } else if  (padapter->setband == GHZ_50) {              /*  5G */
5748                 for (i = 0; i < j ; i++) {
5749                         if (out[i].hw_value > 35) {
5750                                 memcpy(&out[scan_ch_num++], &out[i],
5751                                        sizeof(struct rtw_ieee80211_channel));
5752                         }
5753                 }
5754                 j = scan_ch_num;
5755         } else
5756                 {}
5757
5758         return j;
5759 }
5760
5761 int sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5762 {
5763         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5764         const struct sitesurvey_parm *pparm = (struct sitesurvey_parm *)pbuf;
5765         u8 bdelayscan = false;
5766         u32 initialgain;
5767         u32 i;
5768
5769         if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE) {
5770                 pmlmeext->sitesurvey_res.state = SCAN_START;
5771                 pmlmeext->sitesurvey_res.bss_cnt = 0;
5772                 pmlmeext->sitesurvey_res.channel_idx = 0;
5773
5774                 for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
5775                         if (pparm->ssid[i].ssid_len) {
5776                                 memcpy(pmlmeext->sitesurvey_res.ssid[i].ssid,
5777                                        pparm->ssid[i].ssid,
5778                                        IEEE80211_MAX_SSID_LEN);
5779                                 pmlmeext->sitesurvey_res.ssid[i].ssid_len =
5780                                         pparm->ssid[i].ssid_len;
5781                         } else {
5782                                 pmlmeext->sitesurvey_res.ssid[i].ssid_len = 0;
5783                         }
5784                 }
5785
5786                 pmlmeext->sitesurvey_res.ch_num =
5787                         rtw_scan_ch_decision(padapter,
5788                                              pmlmeext->sitesurvey_res.ch,
5789                                              RTW_CHANNEL_SCAN_AMOUNT,
5790                                              pparm->ch, pparm->ch_num);
5791
5792                 pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
5793
5794                 /* issue null data if associating to the AP */
5795                 if (is_client_associated_to_ap23a(padapter)) {
5796                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
5797
5798                         /* switch to correct channel of current network
5799                            before issue keep-alive frames */
5800                         if (rtw_get_oper_ch23a(padapter) !=
5801                             pmlmeext->cur_channel)
5802                                 SelectChannel23a(padapter,
5803                                                  pmlmeext->cur_channel);
5804
5805                         issue_nulldata23a(padapter, NULL, 1, 3, 500);
5806
5807                         bdelayscan = true;
5808                 }
5809
5810                 if (bdelayscan) {
5811                         /* delay 50ms to protect nulldata(1). */
5812                         set_survey_timer(pmlmeext, 50);
5813                         return H2C_SUCCESS;
5814                 }
5815         }
5816
5817         if (pmlmeext->sitesurvey_res.state == SCAN_START ||
5818             pmlmeext->sitesurvey_res.state == SCAN_TXNULL) {
5819                 /* disable dynamic functions, such as high power, DIG */
5820                 rtl8723a_odm_support_ability_backup(padapter);
5821                 rtl8723a_odm_support_ability_clr(padapter,
5822                                                  DYNAMIC_FUNC_DISABLE);
5823
5824                 /* config the initial gain under scanning, need to
5825                    write the BB registers */
5826                 if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled == true)
5827                         initialgain = 0x30;
5828                 else
5829                         initialgain = 0x1E;
5830
5831                 rtl8723a_set_initial_gain(padapter, initialgain);
5832
5833                 /* set MSR to no link state */
5834                 rtl8723a_set_media_status(padapter, MSR_NOLINK);
5835
5836                 rtl8723a_mlme_sitesurvey(padapter, 1);
5837
5838                 pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
5839         }
5840
5841         rtw_site_survey(padapter);
5842
5843         return H2C_SUCCESS;
5844 }
5845
5846 int setauth_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5847 {
5848         const struct setauth_parm *pparm = (struct setauth_parm *)pbuf;
5849         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5850         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5851
5852         if (pparm->mode < 4)
5853                 pmlmeinfo->auth_algo = pparm->mode;
5854
5855         return H2C_SUCCESS;
5856 }
5857
5858 int setkey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5859 {
5860         unsigned short ctrl;
5861         const struct setkey_parm *pparm = (struct setkey_parm *)pbuf;
5862         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5863         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5864         unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
5865
5866         /* main tx key for wep. */
5867         if (pparm->set_tx)
5868                 pmlmeinfo->key_index = pparm->keyid;
5869
5870         /* write cam */
5871         ctrl = BIT(15) | (pparm->algorithm) << 2 | pparm->keyid;
5872
5873         DBG_8723A_LEVEL(_drv_always_, "set group key to hw: alg:%d(WEP40-1 "
5874                         "WEP104-5 TKIP-2 AES-4) keyid:%d\n",
5875                         pparm->algorithm, pparm->keyid);
5876         rtl8723a_cam_write(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
5877
5878         /* allow multicast packets to driver */
5879         rtl8723a_on_rcr_am(padapter);
5880
5881         return H2C_SUCCESS;
5882 }
5883
5884 int set_stakey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5885 {
5886         u16 ctrl = 0;
5887         u8 cam_id;/* cam_entry */
5888         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5889         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5890         const struct set_stakey_parm *pparm = (struct set_stakey_parm *)pbuf;
5891
5892         /* cam_entry: */
5893         /* 0~3 for default key */
5894
5895         /* for concurrent mode (ap+sta): */
5896         /* default key is disable, using sw encrypt/decrypt */
5897         /* cam_entry = 4  for sta mode (macid = 0) */
5898         /* cam_entry(macid+3) = 5 ~ N for ap mode (aid = 1~N, macid = 2 ~N) */
5899
5900         /* for concurrent mode (sta+sta): */
5901         /* default key is disable, using sw encrypt/decrypt */
5902         /* cam_entry = 4 mapping to macid = 0 */
5903         /* cam_entry = 5 mapping to macid = 2 */
5904
5905         cam_id = 4;
5906
5907         DBG_8723A_LEVEL(_drv_always_, "set pairwise key to hw: alg:%d(WEP40-1 "
5908                         "WEP104-5 TKIP-2 AES-4) camid:%d\n",
5909                         pparm->algorithm, cam_id);
5910         if ((pmlmeinfo->state & 0x03) == MSR_AP) {
5911                 struct sta_info *psta;
5912                 struct sta_priv *pstapriv = &padapter->stapriv;
5913
5914                 if (pparm->algorithm == 0) {    /*  clear cam entry */
5915                         clear_cam_entry23a(padapter, pparm->id);
5916                         return H2C_SUCCESS_RSP;
5917                 }
5918
5919                 psta = rtw_get_stainfo23a(pstapriv, pparm->addr);
5920                 if (psta) {
5921                         ctrl = BIT(15) | (pparm->algorithm << 2);
5922
5923                         DBG_8723A("r871x_set_stakey_hdl23a(): enc_algorithm "
5924                                   "=%d\n", pparm->algorithm);
5925
5926                         if (psta->mac_id < 1 || psta->mac_id > (NUM_STA - 4)) {
5927                                 DBG_8723A("r871x_set_stakey_hdl23a():set_stakey"
5928                                           " failed, mac_id(aid) =%d\n",
5929                                           psta->mac_id);
5930                                 return H2C_REJECTED;
5931                         }
5932
5933                         /* 0~3 for default key, cmd_id = macid + 3,
5934                            macid = aid+1; */
5935                         cam_id = psta->mac_id + 3;
5936
5937                         DBG_8723A("Write CAM, mac_addr =%pM, "
5938                                   "cam_entry =%d\n", pparm->addr, cam_id);
5939
5940                         rtl8723a_cam_write(padapter, cam_id, ctrl,
5941                                            pparm->addr, pparm->key);
5942
5943                         return H2C_SUCCESS_RSP;
5944                 } else {
5945                         DBG_8723A("r871x_set_stakey_hdl23a(): sta has been "
5946                                   "free\n");
5947                         return H2C_REJECTED;
5948                 }
5949         }
5950
5951         /* below for sta mode */
5952
5953         if (pparm->algorithm == 0) {    /*  clear cam entry */
5954                 clear_cam_entry23a(padapter, pparm->id);
5955                 return H2C_SUCCESS;
5956         }
5957
5958         ctrl = BIT(15) | (pparm->algorithm << 2);
5959
5960         rtl8723a_cam_write(padapter, cam_id, ctrl, pparm->addr, pparm->key);
5961
5962         pmlmeinfo->enc_algo = pparm->algorithm;
5963
5964         return H2C_SUCCESS;
5965 }
5966
5967 int add_ba_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
5968 {
5969         const struct addBaReq_parm *pparm = (struct addBaReq_parm *)pbuf;
5970         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5971         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5972         struct sta_info *psta;
5973
5974         psta = rtw_get_stainfo23a(&padapter->stapriv, pparm->addr);
5975
5976         if (!psta)
5977                 return H2C_SUCCESS;
5978
5979         if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) &&
5980              pmlmeinfo->HT_enable) ||
5981             (pmlmeinfo->state & 0x03) == MSR_AP) {
5982                 issue_action_BA23a(padapter, pparm->addr,
5983                                    WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
5984                 mod_timer(&psta->addba_retry_timer,
5985                           jiffies + msecs_to_jiffies(ADDBA_TO));
5986         } else
5987                 psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
5988
5989         return H2C_SUCCESS;
5990 }
5991
5992 int set_tx_beacon_cmd23a(struct rtw_adapter *padapter)
5993 {
5994         struct cmd_obj *ph2c;
5995         struct Tx_Beacon_param *ptxBeacon_parm;
5996         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
5997         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
5998         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
5999         u8 res = _SUCCESS;
6000         int len_diff = 0;
6001
6002         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
6003         if (!ph2c) {
6004                 res = _FAIL;
6005                 goto exit;
6006         }
6007
6008         ptxBeacon_parm = kzalloc(sizeof(struct Tx_Beacon_param), GFP_ATOMIC);
6009         if (!ptxBeacon_parm) {
6010                 kfree(ph2c);
6011                 res = _FAIL;
6012                 goto exit;
6013         }
6014
6015         memcpy(&ptxBeacon_parm->network, &pmlmeinfo->network,
6016                sizeof(struct wlan_bssid_ex));
6017
6018         len_diff = update_hidden_ssid(ptxBeacon_parm->network.IEs,
6019                                       ptxBeacon_parm->network.IELength,
6020                                       pmlmeinfo->hidden_ssid_mode);
6021         ptxBeacon_parm->network.IELength += len_diff;
6022
6023         init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm,
6024                                    GEN_CMD_CODE(_TX_Beacon));
6025
6026         res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
6027
6028 exit:
6029         return res;
6030 }
6031
6032 int mlme_evt_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6033 {
6034         u8 evt_code, evt_seq;
6035         u16 evt_sz;
6036         const struct C2HEvent_Header *c2h;
6037         void (*event_callback)(struct rtw_adapter *dev, const u8 *pbuf);
6038
6039         c2h = (struct C2HEvent_Header *)pbuf;
6040         evt_sz = c2h->len;
6041         evt_seq = c2h->seq;
6042         evt_code = c2h->ID;
6043
6044         /*  checking if event code is valid */
6045         if (evt_code >= MAX_C2HEVT) {
6046                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
6047                          "Event Code(%d) mismatch!\n", evt_code);
6048                 goto _abort_event_;
6049         }
6050
6051         /*  checking if event size match the event parm size */
6052         if (wlanevents[evt_code].parmsize != 0 &&
6053             wlanevents[evt_code].parmsize != evt_sz) {
6054                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
6055                          "Event(%d) Parm Size mismatch (%d vs %d)!\n",
6056                          evt_code, wlanevents[evt_code].parmsize, evt_sz);
6057                 goto _abort_event_;
6058         }
6059
6060         event_callback = wlanevents[evt_code].event_callback;
6061         event_callback(padapter, pbuf + sizeof(struct C2HEvent_Header));
6062
6063 _abort_event_:
6064
6065         return H2C_SUCCESS;
6066 }
6067
6068 int h2c_msg_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6069 {
6070         if (!pbuf)
6071                 return H2C_PARAMETERS_ERROR;
6072
6073         return H2C_SUCCESS;
6074 }
6075
6076 int tx_beacon_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6077 {
6078         if (send_beacon23a(padapter) == _FAIL) {
6079                 DBG_8723A("issue_beacon23a, fail!\n");
6080                 return H2C_PARAMETERS_ERROR;
6081         }
6082 #ifdef CONFIG_8723AU_AP_MODE
6083         else { /* tx bc/mc frames after update TIM */
6084                 struct sta_info *psta_bmc;
6085                 struct list_head *plist, *phead, *ptmp;
6086                 struct xmit_frame *pxmitframe;
6087                 struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
6088                 struct sta_priv  *pstapriv = &padapter->stapriv;
6089
6090                 /* for BC/MC Frames */
6091                 psta_bmc = rtw_get_bcmc_stainfo23a(padapter);
6092                 if (!psta_bmc)
6093                         return H2C_SUCCESS;
6094
6095                 if (pstapriv->tim_bitmap & BIT(0) && psta_bmc->sleepq_len > 0) {
6096                         msleep(10);/*  10ms, ATIM(HIQ) Windows */
6097                         /* spin_lock_bh(&psta_bmc->sleep_q.lock); */
6098                         spin_lock_bh(&pxmitpriv->lock);
6099
6100                         phead = get_list_head(&psta_bmc->sleep_q);
6101
6102                         list_for_each_safe(plist, ptmp, phead) {
6103                                 pxmitframe = container_of(plist,
6104                                                           struct xmit_frame,
6105                                                           list);
6106
6107                                 list_del_init(&pxmitframe->list);
6108
6109                                 psta_bmc->sleepq_len--;
6110                                 if (psta_bmc->sleepq_len>0)
6111                                         pxmitframe->attrib.mdata = 1;
6112                                 else
6113                                         pxmitframe->attrib.mdata = 0;
6114
6115                                 pxmitframe->attrib.triggered = 1;
6116
6117                                 pxmitframe->attrib.qsel = 0x11;/* HIQ */
6118
6119                                 rtl8723au_hal_xmitframe_enqueue(padapter,
6120                                                                 pxmitframe);
6121                         }
6122
6123                         /* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
6124                         spin_unlock_bh(&pxmitpriv->lock);
6125                 }
6126         }
6127 #endif
6128
6129         return H2C_SUCCESS;
6130 }
6131
6132 int set_ch_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6133 {
6134         const struct set_ch_parm *set_ch_parm;
6135         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
6136
6137         if (!pbuf)
6138                 return H2C_PARAMETERS_ERROR;
6139
6140         set_ch_parm = (struct set_ch_parm *)pbuf;
6141
6142         DBG_8723A("%s(%s): ch:%u, bw:%u, ch_offset:%u\n", __func__,
6143                   padapter->pnetdev->name, set_ch_parm->ch,
6144                   set_ch_parm->bw, set_ch_parm->ch_offset);
6145
6146         pmlmeext->cur_channel = set_ch_parm->ch;
6147         pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
6148         pmlmeext->cur_bwmode = set_ch_parm->bw;
6149
6150         set_channel_bwmode23a(padapter, set_ch_parm->ch,
6151                               set_ch_parm->ch_offset, set_ch_parm->bw);
6152
6153         return H2C_SUCCESS;
6154 }
6155
6156 int set_chplan_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6157 {
6158         const struct SetChannelPlan_param *setChannelPlan_param;
6159         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
6160
6161         if (!pbuf)
6162                 return H2C_PARAMETERS_ERROR;
6163
6164         setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
6165
6166         pmlmeext->max_chan_nums =
6167                 init_channel_set(padapter, setChannelPlan_param->channel_plan,
6168                                  pmlmeext->channel_set);
6169         init_channel_list(padapter, pmlmeext->channel_set,
6170                           pmlmeext->max_chan_nums, &pmlmeext->channel_list);
6171
6172         return H2C_SUCCESS;
6173 }
6174
6175 int led_blink_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6176 {
6177         struct LedBlink_param *ledBlink_param;
6178
6179         if (!pbuf)
6180                 return H2C_PARAMETERS_ERROR;
6181
6182         ledBlink_param = (struct LedBlink_param *)pbuf;
6183
6184         return H2C_SUCCESS;
6185 }
6186
6187 int set_csa_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6188 {
6189         return H2C_REJECTED;
6190 }
6191
6192 /*  TDLS_WRCR           : write RCR DATA BIT */
6193 /*  TDLS_SD_PTI         : issue peer traffic indication */
6194 /*  TDLS_CS_OFF         : go back to the channel linked with AP,
6195                           terminating channel switch procedure */
6196 /*  TDLS_INIT_CH_SEN    : init channel sensing, receive all data and
6197                           mgnt frame */
6198 /*  TDLS_DONE_CH_SEN    : channel sensing and report candidate channel */
6199 /*  TDLS_OFF_CH         : first time set channel to off channel */
6200 /*  TDLS_BASE_CH        : go back tp the channel linked with AP when set
6201                           base channel as target channel */
6202 /*  TDLS_P_OFF_CH       : periodically go to off channel */
6203 /*  TDLS_P_BASE_CH      : periodically go back to base channel */
6204 /*  TDLS_RS_RCR         : restore RCR */
6205 /*  TDLS_CKALV_PH1      : check alive timer phase1 */
6206 /*  TDLS_CKALV_PH2      : check alive timer phase2 */
6207 /*  TDLS_FREE_STA       : free tdls sta */
6208 int tdls_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
6209 {
6210         return H2C_REJECTED;
6211 }