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