drivers/net/wireless/rockchip_wlan/rtl8723bu/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  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  *
  19  ******************************************************************************/
  20 #define _RTW_MLME_EXT_C_
  21
  22 #include <drv_types.h>
  23 #ifdef CONFIG_IOCTL_CFG80211
  24 #include <rtw_wifi_regd.h>
  25 #endif //CONFIG_IOCTL_CFG80211
  26
  27
  28 struct mlme_handler mlme_sta_tbl[]={
  29         {WIFI_ASSOCREQ,         "OnAssocReq",   &OnAssocReq},
  30         {WIFI_ASSOCRSP,         "OnAssocRsp",   &OnAssocRsp},
  31         {WIFI_REASSOCREQ,       "OnReAssocReq", &OnAssocReq},
  32         {WIFI_REASSOCRSP,       "OnReAssocRsp", &OnAssocRsp},
  33         {WIFI_PROBEREQ,         "OnProbeReq",   &OnProbeReq},
  34         {WIFI_PROBERSP,         "OnProbeRsp",           &OnProbeRsp},
  35
  36         /*----------------------------------------------------------
  37                                         below 2 are reserved
  38         -----------------------------------------------------------*/
  39         {0,                                     "DoReserved",           &DoReserved},
  40         {0,                                     "DoReserved",           &DoReserved},
  41         {WIFI_BEACON,           "OnBeacon",             &OnBeacon},
  42         {WIFI_ATIM,                     "OnATIM",               &OnAtim},
  43         {WIFI_DISASSOC,         "OnDisassoc",           &OnDisassoc},
  44         {WIFI_AUTH,                     "OnAuth",               &OnAuthClient},
  45         {WIFI_DEAUTH,           "OnDeAuth",             &OnDeAuth},
  46         {WIFI_ACTION,           "OnAction",             &OnAction},
  47         {WIFI_ACTION_NOACK,"OnActionNoAck",     &OnAction},
  48 };
  49
  50 #ifdef _CONFIG_NATIVEAP_MLME_
  51 struct mlme_handler mlme_ap_tbl[]={
  52         {WIFI_ASSOCREQ,         "OnAssocReq",   &OnAssocReq},
  53         {WIFI_ASSOCRSP,         "OnAssocRsp",   &OnAssocRsp},
  54         {WIFI_REASSOCREQ,       "OnReAssocReq", &OnAssocReq},
  55         {WIFI_REASSOCRSP,       "OnReAssocRsp", &OnAssocRsp},
  56         {WIFI_PROBEREQ,         "OnProbeReq",   &OnProbeReq},
  57         {WIFI_PROBERSP,         "OnProbeRsp",           &OnProbeRsp},
  58
  59         /*----------------------------------------------------------
  60                                         below 2 are reserved
  61         -----------------------------------------------------------*/
  62         {0,                                     "DoReserved",           &DoReserved},
  63         {0,                                     "DoReserved",           &DoReserved},
  64         {WIFI_BEACON,           "OnBeacon",             &OnBeacon},
  65         {WIFI_ATIM,                     "OnATIM",               &OnAtim},
  66         {WIFI_DISASSOC,         "OnDisassoc",           &OnDisassoc},
  67         {WIFI_AUTH,                     "OnAuth",               &OnAuth},
  68         {WIFI_DEAUTH,           "OnDeAuth",             &OnDeAuth},
  69         {WIFI_ACTION,           "OnAction",             &OnAction},
  70         {WIFI_ACTION_NOACK,"OnActionNoAck",     &OnAction},
  71 };
  72 #endif
  73
  74 struct action_handler OnAction_tbl[]={
  75         {RTW_WLAN_CATEGORY_SPECTRUM_MGMT,        "ACTION_SPECTRUM_MGMT", on_action_spct},
  76         {RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos},
  77         {RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls},
  78         {RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
  79         {RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
  80         {RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
  81         {RTW_WLAN_CATEGORY_FT, "ACTION_FT",     &DoReserved},
  82         {RTW_WLAN_CATEGORY_HT,  "ACTION_HT",    &OnAction_ht},
  83 #ifdef CONFIG_IEEE80211W
  84         {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &OnAction_sa_query},
  85 #else
  86         {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved},
  87 #endif //CONFIG_IEEE80211W
  88         //add for CONFIG_IEEE80211W
  89         {RTW_WLAN_CATEGORY_UNPROTECTED_WNM, "ACTION_UNPROTECTED_WNM", &DoReserved},
  90         {RTW_WLAN_CATEGORY_SELF_PROTECTED, "ACTION_SELF_PROTECTED", &DoReserved},
  91         {RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm},
  92         {RTW_WLAN_CATEGORY_VHT, "ACTION_VHT", &OnAction_vht},
  93         {RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p},
  94 };
  95
  96
  97 u8      null_addr[ETH_ALEN]= {0,0,0,0,0,0};
  98
  99 /**************************************************
 100 OUI definitions for the vendor specific IE
 101 ***************************************************/
 102 unsigned char   RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
 103 unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
 104 unsigned char   WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
 105 unsigned char   P2P_OUI[] = {0x50,0x6F,0x9A,0x09};
 106 unsigned char   WFD_OUI[] = {0x50,0x6F,0x9A,0x0A};
 107
 108 unsigned char   WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
 109 unsigned char   WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
 110
 111 unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
 112 unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
 113
 114 extern unsigned char REALTEK_96B_IE[];
 115
 116
 117 /********************************************************
 118 ChannelPlan definitions
 119 *********************************************************/
 120 /*static RT_CHANNEL_PLAN        DefaultChannelPlan[RT_CHANNEL_DOMAIN_MAX] = {
 121         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},32},                                     // 0x00, RT_CHANNEL_DOMAIN_FCC
 122         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},31},                                         // 0x01, RT_CHANNEL_DOMAIN_IC
 123         {{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},32},                                               // 0x02, RT_CHANNEL_DOMAIN_ETSI
 124         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x03, RT_CHANNEL_DOMAIN_SPAIN
 125         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x04, RT_CHANNEL_DOMAIN_FRANCE
 126         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x05, RT_CHANNEL_DOMAIN_MKK
 127         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x06, RT_CHANNEL_DOMAIN_MKK1
 128         {{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64},21},                                                                                                                           // 0x07, RT_CHANNEL_DOMAIN_ISRAEL
 129         {{1,2,3,4,5,6,7,8,9,10,11,12,13,14,36,40,44,48,52,56,60,64},22},                                                                                                                        // 0x08, RT_CHANNEL_DOMAIN_TELEC
 130         {{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},                                                                                                                                                                // 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN
 131         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13
 132         {{1,2,3,4,5,6,7,8,9,10,11,56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},26},                                                                        // 0x0B, RT_CHANNEL_DOMAIN_TAIWAN
 133         {{1,2,3,4,5,6,7,8,9,10,11,12,13,149,153,157,161,165},18},                                                                                                                               // 0x0C, RT_CHANNEL_DOMAIN_CHINA
 134         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,149,153,157,161,165},24},                                                                                             // 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO
 135         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,149,153,157,161,165},31},                                         // 0x0E, RT_CHANNEL_DOMAIN_KOREA
 136         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64},19},                                                                                                                                 // 0x0F, RT_CHANNEL_DOMAIN_TURKEY
 137         {{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},32},                                               // 0x10, RT_CHANNEL_DOMAIN_JAPAN
 138         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,149,153,157,161,165},20},                                                                                                                 // 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS
 139         {{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48},17},                                                                                                                                               // 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS
 140         {{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140,149,153,157,161,165},37},   // 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G
 141         {{1,2,3,4,5,6,7,8,9,10,11,56,60,64,149,153,157,161,165},19},                                                                                                                            // 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS
 142 };*/
 143
 144 static RT_CHANNEL_PLAN_2G       RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
 145         {{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
 146         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},           // 0x01, RT_CHANNEL_DOMAIN_2G_ETSI1
 147         {{1,2,3,4,5,6,7,8,9,10,11},11},                 // 0x02, RT_CHANNEL_DOMAIN_2G_FCC1
 148         {{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},        // 0x03, RT_CHANNEL_DOMAIN_2G_MIKK1
 149         {{10,11,12,13},4},                                              // 0x04, RT_CHANNEL_DOMAIN_2G_ETSI2
 150         {{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},        // 0x05, RT_CHANNEL_DOMAIN_2G_GLOBAL , Passive scan CH 12, 13, 14
 151         {{},0},                                                         // 0x06, RT_CHANNEL_DOMAIN_2G_NULL
 152 };
 153
 154 static RT_CHANNEL_PLAN_5G       RTW_ChannelPlan5G[RT_CHANNEL_DOMAIN_5G_MAX] = {
 155         {{},0},                                                                                                                                                                 // 0x00, RT_CHANNEL_DOMAIN_5G_NULL
 156         {{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
 157         {{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
 158         {{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
 159         {{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
 160         {{36,40,44,48,149,153,157,161,165},9},                                                                                                          // 0x05, RT_CHANNEL_DOMAIN_5G_FCC2
 161         {{36,40,44,48,52,56,60,64,149,153,157,161,165},13},                                                                                     // 0x06, RT_CHANNEL_DOMAIN_5G_FCC3
 162         {{36,40,44,48,52,56,60,64,149,153,157,161},12},                                                                                         // 0x07, RT_CHANNEL_DOMAIN_5G_FCC4
 163         {{149,153,157,161,165},5},                                                                                                                                      // 0x08, RT_CHANNEL_DOMAIN_5G_FCC5
 164         {{36,40,44,48,52,56,60,64},8},                                                                                                                          // 0x09, RT_CHANNEL_DOMAIN_5G_FCC6
 165         {{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
 166         {{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
 167         {{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
 168         {{36,40,44,48,52,56,60,64},8},                                                                                                                          // 0x0D, RT_CHANNEL_DOMAIN_5G_MKK2
 169         {{100,104,108,112,116,120,124,128,132,136,140},11},                                                                                     // 0x0E, RT_CHANNEL_DOMAIN_5G_MKK3
 170         {{56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},15},                                                                // 0x0F, RT_CHANNEL_DOMAIN_5G_NCC1
 171         {{56,60,64,149,153,157,161,165},8},                                                                                                                     // 0x10, RT_CHANNEL_DOMAIN_5G_NCC2
 172         {{149,153,157,161,165},5},                                                                                                                                      // 0x11, RT_CHANNEL_DOMAIN_5G_NCC3
 173         {{36,40,44,48},4},                                                                                                                                                      // 0x12, RT_CHANNEL_DOMAIN_5G_ETSI4
 174         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},21},                     // 0x13, RT_CHANNEL_DOMAIN_5G_ETSI5
 175         {{149,153,157,161},4},                                                                                                                                          // 0x14, RT_CHANNEL_DOMAIN_5G_FCC8
 176         {{36,40,44,48,52,56,60,64},8},                                                                                                                          // 0x15, RT_CHANNEL_DOMAIN_5G_ETSI6
 177         {{36,40,44,48,52,56,60,64,149,153,157,161,165},13},                                                                                     // 0x16, RT_CHANNEL_DOMAIN_5G_ETSI7
 178         {{36,40,44,48,149,153,157,161,165},9},                                                                                                          // 0x17, RT_CHANNEL_DOMAIN_5G_ETSI8
 179         {{100,104,108,112,116,120,124,128,132,136,140},11},                                                                                     // 0x18, RT_CHANNEL_DOMAIN_5G_ETSI9
 180         {{149,153,157,161,165},5},                                                                                                                                      // 0x19, RT_CHANNEL_DOMAIN_5G_ETSI10
 181         {{36,40,44,48,52,56,60,64,132,136,140,149,153,157,161,165},16},                                                                 // 0x1A, RT_CHANNEL_DOMAIN_5G_ETSI11
 182         {{52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},17},                                                 // 0x1B, RT_CHANNEL_DOMAIN_5G_NCC4
 183         {{149,153,157,161},4},                                                                                                                                          // 0x1C, RT_CHANNEL_DOMAIN_5G_ETSI12
 184         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},21},                     // 0x1D, RT_CHANNEL_DOMAIN_5G_FCC9
 185         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140},16},                                                         // 0x1E, RT_CHANNEL_DOMAIN_5G_ETSI13
 186         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161},20},                         // 0x1F, RT_CHANNEL_DOMAIN_5G_FCC10
 187         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,149,153,157,161},19},                                     // 0x20, RT_CHANNEL_DOMAIN_5G_KCC2
 188         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},21},                     // 0x21, RT_CHANNEL_DOMAIN_5G_FCC11
 189         {{56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},16},                                            // 0x22, RT_CHANNEL_DOMAIN_5G_NCC5
 190         {{36,40,44,48},4},                                                                                                                                                      // 0x23, RT_CHANNEL_DOMAIN_5G_MKK4
 191         //===== Driver self defined for old channel plan Compatible ,Remember to modify if have new channel plan definition =====
 192         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},21},                     // 0x30, RT_CHANNEL_DOMAIN_5G_FCC
 193         {{36,40,44,48},4},                                                                                                                                                      // 0x31, RT_CHANNEL_DOMAIN_5G_JAPAN_NO_DFS
 194         {{36,40,44,48,149,153,157,161},8},                                                                                                                      // 0x32, RT_CHANNEL_DOMAIN_5G_FCC4_NO_DFS
 195 };
 196
 197 static RT_CHANNEL_PLAN_MAP      RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
 198         //===== 0x00 ~ 0x1F , Old Define =====
 199         {0x02,0x20},    //0x00, RT_CHANNEL_DOMAIN_FCC
 200         {0x02,0x0A},    //0x01, RT_CHANNEL_DOMAIN_IC
 201         {0x01,0x01},    //0x02, RT_CHANNEL_DOMAIN_ETSI
 202         {0x01,0x00},    //0x03, RT_CHANNEL_DOMAIN_SPAIN
 203         {0x01,0x00},    //0x04, RT_CHANNEL_DOMAIN_FRANCE
 204         {0x03,0x00},    //0x05, RT_CHANNEL_DOMAIN_MKK
 205         {0x03,0x00},    //0x06, RT_CHANNEL_DOMAIN_MKK1
 206         {0x01,0x09},    //0x07, RT_CHANNEL_DOMAIN_ISRAEL
 207         {0x03,0x09},    //0x08, RT_CHANNEL_DOMAIN_TELEC
 208         {0x03,0x00},    //0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN
 209         {0x00,0x00},    //0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13
 210         {0x02,0x0F},    //0x0B, RT_CHANNEL_DOMAIN_TAIWAN
 211         {0x01,0x08},    //0x0C, RT_CHANNEL_DOMAIN_CHINA
 212         {0x02,0x06},    //0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO
 213         {0x02,0x0B},    //0x0E, RT_CHANNEL_DOMAIN_KOREA
 214         {0x02,0x09},    //0x0F, RT_CHANNEL_DOMAIN_TURKEY
 215         {0x01,0x01},    //0x10, RT_CHANNEL_DOMAIN_JAPAN
 216         {0x02,0x05},    //0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS
 217         {0x01,0x21},    //0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS
 218         {0x00,0x04},    //0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G
 219         {0x02,0x10},    //0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS
 220         {0x00,0x21},    //0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS
 221         {0x00,0x22},    //0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS
 222         {0x03,0x21},    //0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS
 223         {0x06,0x08},    //0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS
 224         {0x02,0x08},    //0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS
 225         {0x00,0x00},    //0x1A,
 226         {0x00,0x00},    //0x1B,
 227         {0x00,0x00},    //0x1C,
 228         {0x00,0x00},    //0x1D,
 229         {0x00,0x00},    //0x1E,
 230         {0x06,0x04},    //0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G
 231         //===== 0x20 ~ 0x7F ,New Define =====
 232         {0x00,0x00},    //0x20, RT_CHANNEL_DOMAIN_WORLD_NULL
 233         {0x01,0x00},    //0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL
 234         {0x02,0x00},    //0x22, RT_CHANNEL_DOMAIN_FCC1_NULL
 235         {0x03,0x00},    //0x23, RT_CHANNEL_DOMAIN_MKK1_NULL
 236         {0x04,0x00},    //0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL
 237         {0x02,0x04},    //0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1
 238         {0x00,0x01},    //0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1
 239         {0x03,0x0C},    //0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1
 240         {0x00,0x20},    //0x28, RT_CHANNEL_DOMAIN_5G_KCC2
 241         {0x00,0x05},    //0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2
 242         {0x00,0x00},    //0x2A,
 243         {0x00,0x00},    //0x2B,
 244         {0x00,0x00},    //0x2C,
 245         {0x00,0x00},    //0x2D,
 246         {0x00,0x00},    //0x2E,
 247         {0x00,0x00},    //0x2F,
 248         {0x00,0x06},    //0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3
 249         {0x00,0x07},    //0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4
 250         {0x00,0x08},    //0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5
 251         {0x00,0x09},    //0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6
 252         {0x02,0x21},    //0x34, RT_CHANNEL_DOMAIN_5G_FCC11
 253         {0x00,0x02},    //0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2
 254         {0x00,0x03},    //0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3
 255         {0x03,0x0D},    //0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2
 256         {0x03,0x0E},    //0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3
 257         {0x02,0x22},    //0x39, RT_CHANNEL_DOMAIN_5G_NCC5
 258         {0x00,0x00},    //0x3A,
 259         {0x00,0x00},    //0x3B,
 260         {0x00,0x00},    //0x3C,
 261         {0x00,0x00},    //0x3D,
 262         {0x00,0x00},    //0x3E,
 263         {0x00,0x00},    //0x3F,
 264         {0x02,0x10},    //0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2
 265         {0x05,0x00},    //0x41, RT_CHANNEL_DOMAIN_GLOBAL_NULL
 266         {0x01,0x12},    //0x42, RT_CHANNEL_DOMAIN_ETSI1_ETSI4
 267         {0x02,0x05},    //0x43, RT_CHANNEL_DOMAIN_FCC1_FCC2
 268         {0x02,0x11},    //0x44, RT_CHANNEL_DOMAIN_FCC1_NCC3
 269         {0x00,0x13},    //0x45, RT_CHANNEL_DOMAIN_WORLD_ETSI5
 270         {0x02,0x14},    //0x46, RT_CHANNEL_DOMAIN_FCC1_FCC8
 271         {0x00,0x15},    //0x47, RT_CHANNEL_DOMAIN_WORLD_ETSI6
 272         {0x00,0x16},    //0x48, RT_CHANNEL_DOMAIN_WORLD_ETSI7
 273         {0x00,0x17},    //0x49, RT_CHANNEL_DOMAIN_WORLD_ETSI8
 274         {0x00,0x00},    //0x4A,
 275         {0x00,0x00},    //0x4B,
 276         {0x00,0x00},    //0x4C,
 277         {0x00,0x00},    //0x4D,
 278         {0x00,0x00},    //0x4E,
 279         {0x00,0x00},    //0x4F,
 280         {0x00,0x18},    //0x50, RT_CHANNEL_DOMAIN_WORLD_ETSI9
 281         {0x00,0x19},    //0x51, RT_CHANNEL_DOMAIN_WORLD_ETSI10
 282         {0x00,0x1A},    //0x52, RT_CHANNEL_DOMAIN_WORLD_ETSI11
 283         {0x02,0x1B},    //0x53, RT_CHANNEL_DOMAIN_FCC1_NCC4
 284         {0x00,0x1C},    //0x54, RT_CHANNEL_DOMAIN_WORLD_ETSI12
 285         {0x02,0x1D},    //0x55, RT_CHANNEL_DOMAIN_FCC1_FCC9
 286         {0x00,0x1E},    //0x56, RT_CHANNEL_DOMAIN_WORLD_ETSI13
 287         {0x02,0x1F},    //0x57, RT_CHANNEL_DOMAIN_FCC1_FCC10
 288         {0x01,0x23},    //0x58, RT_CHANNEL_DOMAIN_WORLD_MKK4
 289 };
 290
 291 static RT_CHANNEL_PLAN_MAP      RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x00,0x02}; //use the conbination for max channel numbers
 292
 293 /*
 294  * Search the @param ch in given @param ch_set
 295  * @ch_set: the given channel set
 296  * @ch: the given channel number
 297  *
 298  * return the index of channel_num in channel_set, -1 if not found
 299  */
 300 int rtw_ch_set_search_ch(RT_CHANNEL_INFO *ch_set, const u32 ch)
 301 {
 302         int i;
 303         for(i=0;ch_set[i].ChannelNum!=0;i++){
 304                 if(ch == ch_set[i].ChannelNum)
 305                         break;
 306         }
 307
 308         if(i >= ch_set[i].ChannelNum)
 309                 return -1;
 310         return i;
 311 }
 312
 313 /*
 314  * Check the @param ch is fit with setband setting of @param adapter
 315  * @adapter: the given adapter
 316  * @ch: the given channel number
 317  *
 318  * return _TRUE when check valid, _FALSE not valid
 319  */
 320 bool rtw_mlme_band_check(_adapter *adapter, const u32 ch)
 321 {
 322         if (adapter->setband == WIFI_FREQUENCY_BAND_AUTO /* 2.4G and 5G */
 323                 || (adapter->setband == WIFI_FREQUENCY_BAND_2GHZ && ch < 35) /* 2.4G only */
 324                 || (adapter->setband == WIFI_FREQUENCY_BAND_5GHZ && ch > 35) /* 5G only */
 325         ) {
 326                 return _TRUE;
 327         }
 328         return _FALSE;
 329 }
 330
 331 /****************************************************************************
 332
 333 Following are the initialization functions for WiFi MLME
 334
 335 *****************************************************************************/
 336
 337 int init_hw_mlme_ext(_adapter *padapter)
 338 {
 339         struct  mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 340
 341         //set_opmode_cmd(padapter, infra_client_with_mlme);//removed
 342
 343         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 344
 345         return _SUCCESS;
 346 }
 347
 348 void init_mlme_default_rate_set(_adapter* padapter)
 349 {
 350         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 351
 352         unsigned char   mixed_datarate[NumRates] = {_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,_9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_, _48M_RATE_, _54M_RATE_, 0xff};
 353         unsigned char   mixed_basicrate[NumRates] ={_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_, _12M_RATE_, _24M_RATE_, 0xff,};
 354         unsigned char   supported_mcs_set[16] = {0xff, 0xff, 0xff, 0x00, 0x00, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 355
 356         _rtw_memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
 357         _rtw_memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
 358
 359         _rtw_memcpy(pmlmeext->default_supported_mcs_set, supported_mcs_set, sizeof(pmlmeext->default_supported_mcs_set));
 360 }
 361
 362 static void init_mlme_ext_priv_value(_adapter* padapter)
 363 {
 364         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 365         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 366
 367         ATOMIC_SET(&pmlmeext->event_seq, 0);
 368         pmlmeext->mgnt_seq = 0;//reset to zero when disconnect at client mode
 369 #ifdef CONFIG_IEEE80211W
 370         pmlmeext->sa_query_seq = 0;
 371         pmlmeext->mgnt_80211w_IPN=0;
 372         pmlmeext->mgnt_80211w_IPN_rx=0;
 373 #endif //CONFIG_IEEE80211W
 374         pmlmeext->cur_channel = padapter->registrypriv.channel;
 375         pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
 376         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 377
 378         pmlmeext->retry = 0;
 379
 380         pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
 381
 382         init_mlme_default_rate_set(padapter);
 383
 384         if(pmlmeext->cur_channel > 14)
 385                 pmlmeext->tx_rate = IEEE80211_OFDM_RATE_6MB;
 386         else
 387                 pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
 388
 389         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
 390         pmlmeext->sitesurvey_res.channel_idx = 0;
 391         pmlmeext->sitesurvey_res.bss_cnt = 0;
 392         pmlmeext->scan_abort = _FALSE;
 393
 394         pmlmeinfo->state = WIFI_FW_NULL_STATE;
 395         pmlmeinfo->reauth_count = 0;
 396         pmlmeinfo->reassoc_count = 0;
 397         pmlmeinfo->link_count = 0;
 398         pmlmeinfo->auth_seq = 0;
 399         pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
 400         pmlmeinfo->key_index = 0;
 401         pmlmeinfo->iv = 0;
 402
 403         pmlmeinfo->enc_algo = _NO_PRIVACY_;
 404         pmlmeinfo->authModeToggle = 0;
 405
 406         _rtw_memset(pmlmeinfo->chg_txt, 0, 128);
 407
 408         pmlmeinfo->slotTime = SHORT_SLOT_TIME;
 409         pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
 410
 411         pmlmeinfo->dialogToken = 0;
 412
 413         pmlmeext->action_public_rxseq = 0xffff;
 414         pmlmeext->action_public_dialog_token = 0xff;
 415 }
 416
 417 static int has_channel(RT_CHANNEL_INFO *channel_set,
 418                                            u8 chanset_size,
 419                                            u8 chan) {
 420         int i;
 421
 422         for (i = 0; i < chanset_size; i++) {
 423                 if (channel_set[i].ChannelNum == chan) {
 424                         return 1;
 425                 }
 426         }
 427
 428         return 0;
 429 }
 430
 431 static void init_channel_list(_adapter *padapter, RT_CHANNEL_INFO *channel_set,
 432                                                           u8 chanset_size,
 433                                                           struct p2p_channels *channel_list) {
 434
 435         struct p2p_oper_class_map op_class[] = {
 436                 { IEEE80211G,  81,   1,  13,  1, BW20 },
 437                 { IEEE80211G,  82,  14,  14,  1, BW20 },
 438 #if 0 /* Do not enable HT40 on 2 GHz */
 439                 { IEEE80211G,  83,   1,   9,  1, BW40PLUS },
 440                 { IEEE80211G,  84,   5,  13,  1, BW40MINUS },
 441 #endif
 442                 { IEEE80211A, 115,  36,  48,  4, BW20 },
 443                 { IEEE80211A, 116,  36,  44,  8, BW40PLUS },
 444                 { IEEE80211A, 117,  40,  48,  8, BW40MINUS },
 445                 { IEEE80211A, 124, 149, 161,  4, BW20 },
 446                 { IEEE80211A, 125, 149, 169,  4, BW20 },
 447                 { IEEE80211A, 126, 149, 157,  8, BW40PLUS },
 448                 { IEEE80211A, 127, 153, 161,  8, BW40MINUS },
 449                 { -1, 0, 0, 0, 0, BW20 }
 450         };
 451
 452         int cla, op;
 453
 454         cla = 0;
 455
 456         for (op = 0; op_class[op].op_class; op++) {
 457                 u8 ch;
 458                 struct p2p_oper_class_map *o = &op_class[op];
 459                 struct p2p_reg_class *reg = NULL;
 460
 461                 for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
 462                         if (!has_channel(channel_set, chanset_size, ch)) {
 463                                 continue;
 464                         }
 465
 466                         if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
 467                                 continue;
 468
 469                         if ((0 < (padapter->registrypriv.bw_mode & 0xf0)) &&
 470                                 ((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
 471                                 continue;
 472
 473                         if (reg == NULL) {
 474                                 reg = &channel_list->reg_class[cla];
 475                                 cla++;
 476                                 reg->reg_class = o->op_class;
 477                                 reg->channels = 0;
 478                         }
 479                         reg->channel[reg->channels] = ch;
 480                         reg->channels++;
 481                 }
 482         }
 483         channel_list->reg_classes = cla;
 484
 485 }
 486
 487 static u8 init_channel_set(_adapter* padapter, u8 ChannelPlan, RT_CHANNEL_INFO *channel_set)
 488 {
 489         u8      index,chanset_size = 0;
 490         u8      b5GBand = _FALSE, b2_4GBand = _FALSE;
 491         u8      Index2G = 0, Index5G=0;
 492
 493         if (!rtw_is_channel_plan_valid(ChannelPlan)) {
 494                 DBG_871X("ChannelPlan ID %x error !!!!!\n",ChannelPlan);
 495                 return chanset_size;
 496         }
 497
 498         _rtw_memset(channel_set, 0, sizeof(RT_CHANNEL_INFO)*MAX_CHANNEL_NUM);
 499
 500         if(IsSupported24G(padapter->registrypriv.wireless_mode))
 501         {
 502                 b2_4GBand = _TRUE;
 503                 if(RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
 504                         Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
 505                 else
 506                         Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
 507         }
 508
 509         if(IsSupported5G(padapter->registrypriv.wireless_mode))
 510         {
 511                 b5GBand = _TRUE;
 512                 if(RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
 513                         Index5G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index5G;
 514                 else
 515                         Index5G = RTW_ChannelPlanMap[ChannelPlan].Index5G;
 516         }
 517
 518         if(b2_4GBand)
 519         {
 520                 for(index=0;index<RTW_ChannelPlan2G[Index2G].Len;index++)
 521                 {
 522                         channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
 523
 524                         if(     (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||//Channel 1~11 is active, and 12~14 is passive
 525                                 (RT_CHANNEL_DOMAIN_GLOBAL_NULL == ChannelPlan)  )
 526                         {
 527                                 if(channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
 528                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 529                                 else if((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
 530                                         channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
 531                         }
 532                         else if(RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
 533                                 RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan ||
 534                                 RT_CHANNEL_DOMAIN_2G_WORLD == Index2G)// channel 12~13, passive scan
 535                         {
 536                                 if(channel_set[chanset_size].ChannelNum <= 11)
 537                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 538                                 else
 539                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 540                         }
 541                         else
 542                         {
 543                                 channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 544                         }
 545
 546                         chanset_size++;
 547                 }
 548         }
 549
 550         if(b5GBand)
 551         {
 552                 for(index=0;index<RTW_ChannelPlan5G[Index5G].Len;index++)
 553                 {
 554 #ifdef CONFIG_DFS
 555                         channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
 556                         if ( channel_set[chanset_size].ChannelNum <= 48
 557                                 || channel_set[chanset_size].ChannelNum >= 149 )
 558                         {
 559                                 if(RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)//passive scan for all 5G channels
 560                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 561                                 else
 562                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 563                         }
 564                         else
 565                         {
 566                                 channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 567                         }
 568                         chanset_size++;
 569 #else /* CONFIG_DFS */
 570                         if ( RTW_ChannelPlan5G[Index5G].Channel[index] <= 48
 571                                 || RTW_ChannelPlan5G[Index5G].Channel[index] >= 149 ) {
 572                                 channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
 573                                 if(RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)//passive scan for all 5G channels
 574                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 575                                 else
 576                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 577                                 DBG_871X("%s(): channel_set[%d].ChannelNum = %d\n", __FUNCTION__, chanset_size, channel_set[chanset_size].ChannelNum);
 578                                 chanset_size++;
 579                         }
 580 #endif /* CONFIG_DFS */
 581                 }
 582         }
 583
 584         Hal_ChannelPlanToRegulation(padapter, ChannelPlan);
 585
 586         DBG_871X("%s ChannelPlan ID %x Chan num:%d  \n",__FUNCTION__,ChannelPlan,chanset_size);
 587         return chanset_size;
 588 }
 589
 590 int     init_mlme_ext_priv(_adapter* padapter)
 591 {
 592         int     res = _SUCCESS;
 593         struct registry_priv* pregistrypriv = &padapter->registrypriv;
 594         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 595         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
 596         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 597
 598         // We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc().
 599         //_rtw_memset((u8 *)pmlmeext, 0, sizeof(struct mlme_ext_priv));
 600
 601         pmlmeext->padapter = padapter;
 602
 603         //fill_fwpriv(padapter, &(pmlmeext->fwpriv));
 604
 605         init_mlme_ext_priv_value(padapter);
 606         pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
 607
 608         init_mlme_ext_timer(padapter);
 609
 610 #ifdef CONFIG_AP_MODE
 611         init_mlme_ap_info(padapter);
 612 #endif
 613
 614         pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan,pmlmeext->channel_set);
 615         init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
 616         pmlmeext->last_scan_time = 0;
 617         pmlmeext->chan_scan_time = SURVEY_TO;
 618         pmlmeext->mlmeext_init = _TRUE;
 619
 620
 621 #ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
 622         pmlmeext->active_keep_alive_check = _TRUE;
 623 #else
 624         pmlmeext->active_keep_alive_check = _FALSE;
 625 #endif
 626
 627 #ifdef DBG_FIXED_CHAN
 628         pmlmeext->fixed_chan = 0xFF;
 629 #endif
 630
 631         return res;
 632
 633 }
 634
 635 void free_mlme_ext_priv (struct mlme_ext_priv *pmlmeext)
 636 {
 637         _adapter *padapter = pmlmeext->padapter;
 638
 639         if (!padapter)
 640                 return;
 641
 642         if (padapter->bDriverStopped == _TRUE)
 643         {
 644                 _cancel_timer_ex(&pmlmeext->survey_timer);
 645                 _cancel_timer_ex(&pmlmeext->link_timer);
 646                 //_cancel_timer_ex(&pmlmeext->ADDBA_timer);
 647         }
 648 }
 649
 650 static u8 cmp_pkt_chnl_diff(_adapter *padapter,u8* pframe,uint packet_len)
 651 {       // if the channel is same, return 0. else return channel differential
 652         uint len;
 653         u8 channel;
 654         u8 *p;
 655         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_, _DSSET_IE_, &len, packet_len - _BEACON_IE_OFFSET_);
 656         if (p)
 657         {
 658                 channel = *(p + 2);
 659                 if(padapter->mlmeextpriv.cur_channel >= channel)
 660                 {
 661                         return (padapter->mlmeextpriv.cur_channel - channel);
 662                 }
 663                 else
 664                 {
 665                         return (channel-padapter->mlmeextpriv.cur_channel);
 666                 }
 667         }
 668         else
 669         {
 670                 return 0;
 671         }
 672 }
 673
 674 static void _mgt_dispatcher(_adapter *padapter, struct mlme_handler *ptable, union recv_frame *precv_frame)
 675 {
 676         u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
 677         u8 *pframe = precv_frame->u.hdr.rx_data;
 678
 679           if(ptable->func)
 680         {
 681          //receive the frames that ra(a1) is my address or ra(a1) is bc address.
 682                 if (!_rtw_memcmp(GetAddr1Ptr(pframe), adapter_mac_addr(padapter), ETH_ALEN) &&
 683                         !_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
 684                 {
 685                         return;
 686                 }
 687
 688                 ptable->func(padapter, precv_frame);
 689         }
 690
 691 }
 692
 693 void mgt_dispatcher(_adapter *padapter, union recv_frame *precv_frame)
 694 {
 695         int index;
 696         struct mlme_handler *ptable;
 697 #ifdef CONFIG_AP_MODE
 698         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 699 #endif //CONFIG_AP_MODE
 700         u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
 701         u8 *pframe = precv_frame->u.hdr.rx_data;
 702         struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));
 703         struct dvobj_priv *psdpriv = padapter->dvobj;
 704         struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
 705
 706         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 707                  ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
 708                   GetFrameType(pframe), GetFrameSubType(pframe)));
 709
 710 #if 0
 711         {
 712                 u8 *pbuf;
 713                 pbuf = GetAddr1Ptr(pframe);
 714                 DBG_871X("A1-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
 715                 pbuf = GetAddr2Ptr(pframe);
 716                 DBG_871X("A2-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
 717                 pbuf = GetAddr3Ptr(pframe);
 718                 DBG_871X("A3-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
 719         }
 720 #endif
 721
 722         if (GetFrameType(pframe) != WIFI_MGT_TYPE)
 723         {
 724                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
 725                 return;
 726         }
 727
 728         //receive the frames that ra(a1) is my address or ra(a1) is bc address.
 729         if (!_rtw_memcmp(GetAddr1Ptr(pframe), adapter_mac_addr(padapter), ETH_ALEN) &&
 730                 !_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
 731         {
 732                 return;
 733         }
 734
 735         ptable = mlme_sta_tbl;
 736
 737         index = GetFrameSubType(pframe) >> 4;
 738
 739 #ifdef CONFIG_TDLS
 740         if((index << 4)==WIFI_ACTION){
 741                 /* category==public (4), action==TDLS_DISCOVERY_RESPONSE */
 742                 if (*(pframe+24) == RTW_WLAN_CATEGORY_PUBLIC && *(pframe+25) == TDLS_DISCOVERY_RESPONSE) {
 743                         DBG_871X("[TDLS] Recv %s from "MAC_FMT"\n", rtw_tdls_action_txt(TDLS_DISCOVERY_RESPONSE), MAC_ARG(GetAddr2Ptr(pframe)));
 744                         On_TDLS_Dis_Rsp(padapter, precv_frame);
 745                 }
 746         }
 747 #endif //CONFIG_TDLS
 748
 749         if (index >= (sizeof(mlme_sta_tbl) /sizeof(struct mlme_handler)))
 750         {
 751                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Currently we do not support reserved sub-fr-type=%d\n", index));
 752                 return;
 753         }
 754         ptable += index;
 755
 756 #if 1
 757         if (psta != NULL)
 758         {
 759                 if (GetRetry(pframe))
 760                 {
 761                         if (precv_frame->u.hdr.attrib.seq_num == psta->RxMgmtFrameSeqNum)
 762                         {
 763                                 /* drop the duplicate management frame */
 764                                 pdbgpriv->dbg_rx_dup_mgt_frame_drop_count++;
 765                                 DBG_871X("Drop duplicate management frame with seq_num = %d.\n", precv_frame->u.hdr.attrib.seq_num);
 766                                 return;
 767                         }
 768                 }
 769                 psta->RxMgmtFrameSeqNum = precv_frame->u.hdr.attrib.seq_num;
 770         }
 771 #else
 772
 773         if(GetRetry(pframe))
 774         {
 775                 //RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("drop due to decache!\n"));
 776                 //return;
 777         }
 778 #endif
 779
 780 #ifdef CONFIG_AP_MODE
 781         switch (GetFrameSubType(pframe))
 782         {
 783                 case WIFI_AUTH:
 784                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 785                                 ptable->func = &OnAuth;
 786                         else
 787                                 ptable->func = &OnAuthClient;
 788                         //pass through
 789                 case WIFI_ASSOCREQ:
 790                 case WIFI_REASSOCREQ:
 791                         _mgt_dispatcher(padapter, ptable, precv_frame);
 792 #ifdef CONFIG_HOSTAPD_MLME
 793                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 794                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 795 #endif
 796                         break;
 797                 case WIFI_PROBEREQ:
 798                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 799                         {
 800 #ifdef CONFIG_HOSTAPD_MLME
 801                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 802 #else
 803                                 _mgt_dispatcher(padapter, ptable, precv_frame);
 804 #endif
 805                         }
 806                         else
 807                                 _mgt_dispatcher(padapter, ptable, precv_frame);
 808                         break;
 809                 case WIFI_BEACON:
 810                         _mgt_dispatcher(padapter, ptable, precv_frame);
 811                         break;
 812                 case WIFI_ACTION:
 813                         //if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 814                         _mgt_dispatcher(padapter, ptable, precv_frame);
 815                         break;
 816                 default:
 817                         _mgt_dispatcher(padapter, ptable, precv_frame);
 818                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 819                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 820                         break;
 821         }
 822 #else
 823
 824         _mgt_dispatcher(padapter, ptable, precv_frame);
 825
 826 #endif
 827
 828 }
 829
 830 #ifdef CONFIG_P2P
 831 u32 p2p_listen_state_process(_adapter *padapter, unsigned char *da)
 832 {
 833         bool response = _TRUE;
 834
 835 #ifdef CONFIG_IOCTL_CFG80211
 836         if( padapter->wdinfo.driver_interface == DRIVER_CFG80211 )
 837         {
 838                 if(padapter->cfg80211_wdinfo.is_ro_ch == _FALSE
 839                         || rtw_get_oper_ch(padapter) != padapter->wdinfo.listen_channel
 840                         || adapter_wdev_data(padapter)->p2p_enabled == _FALSE
 841                         || padapter->mlmepriv.wps_probe_resp_ie == NULL
 842                         || padapter->mlmepriv.p2p_probe_resp_ie == NULL
 843                 )
 844                 {
 845 #ifdef CONFIG_DEBUG_CFG80211
 846                         DBG_871X("DON'T issue_probersp_p2p: p2p_enabled:%d, wps_probe_resp_ie:%p, p2p_probe_resp_ie:%p, ",
 847                                 adapter_wdev_data(padapter)->p2p_enabled,
 848                                 padapter->mlmepriv.wps_probe_resp_ie,
 849                                 padapter->mlmepriv.p2p_probe_resp_ie);
 850                         DBG_871X("is_ro_ch:%d, op_ch:%d, p2p_listen_channel:%d\n",
 851                                 padapter->cfg80211_wdinfo.is_ro_ch,
 852                                 rtw_get_oper_ch(padapter),
 853                                 padapter->wdinfo.listen_channel);
 854 #endif
 855                         response = _FALSE;
 856                 }
 857         }
 858         else
 859 #endif //CONFIG_IOCTL_CFG80211
 860         if( padapter->wdinfo.driver_interface == DRIVER_WEXT )
 861         {
 862                 //      do nothing if the device name is empty
 863                 if ( !padapter->wdinfo.device_name_len )
 864                 {
 865                         response        = _FALSE;
 866                 }
 867         }
 868
 869         if (response == _TRUE)
 870                 issue_probersp_p2p( padapter, da);
 871
 872         return _SUCCESS;
 873 }
 874 #endif //CONFIG_P2P
 875
 876
 877 /****************************************************************************
 878
 879 Following are the callback functions for each subtype of the management frames
 880
 881 *****************************************************************************/
 882
 883 unsigned int OnProbeReq(_adapter *padapter, union recv_frame *precv_frame)
 884 {
 885         unsigned int    ielen;
 886         unsigned char   *p;
 887         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 888         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 889         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 890         WLAN_BSSID_EX   *cur = &(pmlmeinfo->network);
 891         u8 *pframe = precv_frame->u.hdr.rx_data;
 892         uint len = precv_frame->u.hdr.len;
 893         u8 is_valid_p2p_probereq = _FALSE;
 894
 895 #ifdef CONFIG_ATMEL_RC_PATCH
 896         u8 *target_ie=NULL, *wps_ie=NULL;
 897         u8 *start;
 898         uint search_len = 0, wps_ielen = 0, target_ielen = 0;
 899         struct sta_info *psta;
 900         struct sta_priv *pstapriv = &padapter->stapriv;
 901 #endif
 902
 903
 904 #ifdef CONFIG_P2P
 905         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
 906         struct rx_pkt_attrib    *pattrib = &precv_frame->u.hdr.attrib;
 907         u8 wifi_test_chk_rate = 1;
 908
 909         if (    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
 910                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) &&
 911                 !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
 912                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
 913                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)
 914            )
 915         {
 916                 //      Commented by Albert 2011/03/17
 917                 //      mcs_rate = 0 -> CCK 1M rate
 918                 //      mcs_rate = 1 -> CCK 2M rate
 919                 //      mcs_rate = 2 -> CCK 5.5M rate
 920                 //      mcs_rate = 3 -> CCK 11M rate
 921                 //      In the P2P mode, the driver should not support the CCK rate
 922
 923                 //      Commented by Kurt 2012/10/16
 924                 //      IOT issue: Google Nexus7 use 1M rate to send p2p_probe_req after GO nego completed and Nexus7 is client
 925         if (padapter->registrypriv.wifi_spec == 1)
 926         {
 927             if ( pattrib->data_rate <= 3 )
 928             {
 929                 wifi_test_chk_rate = 0;
 930             }
 931         }
 932
 933                 if( wifi_test_chk_rate == 1 )
 934                 {
 935                         if((is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len)) == _TRUE)
 936                         {
 937                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE))
 938                                 {
 939                                         // FIXME
 940                                         if( padapter->wdinfo.driver_interface == DRIVER_WEXT )
 941                                                 report_survey_event(padapter, precv_frame);
 942
 943                                         p2p_listen_state_process( padapter,  get_sa(pframe));
 944
 945                                         return _SUCCESS;
 946                                 }
 947
 948                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
 949                                 {
 950                                         goto _continue;
 951                                 }
 952                         }
 953                 }
 954         }
 955
 956 _continue:
 957 #endif //CONFIG_P2P
 958
 959         if(check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 960         {
 961                 return _SUCCESS;
 962         }
 963
 964         if(check_fwstate(pmlmepriv, _FW_LINKED) == _FALSE &&
 965                 check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE)==_FALSE)
 966         {
 967                 return _SUCCESS;
 968         }
 969
 970
 971         //DBG_871X("+OnProbeReq\n");
 972
 973
 974 #ifdef CONFIG_ATMEL_RC_PATCH
 975                 if ((wps_ie = rtw_get_wps_ie(
 976                         pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_,
 977                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_,
 978                          NULL, &wps_ielen))) {
 979
 980                         target_ie = rtw_get_wps_attr_content( wps_ie, wps_ielen, WPS_ATTR_MANUFACTURER, NULL, &target_ielen);
 981                 }
 982                 if ((target_ie && (target_ielen == 4)) && (_TRUE ==_rtw_memcmp((void *)target_ie, "Ozmo",4 ))) {
 983                         //psta->flag_atmel_rc = 1;
 984                         unsigned char *sa_addr = get_sa(pframe);
 985                         printk("%s: Find Ozmo RC -- %02x:%02x:%02x:%02x:%02x:%02x  \n\n",
 986                                 __func__, *sa_addr, *(sa_addr+1), *(sa_addr+2), *(sa_addr+3), *(sa_addr+4), *(sa_addr+5));
 987                         _rtw_memcpy(  pstapriv->atmel_rc_pattern, get_sa(pframe), ETH_ALEN);
 988                 }
 989 #endif
 990
 991
 992 #ifdef CONFIG_AUTO_AP_MODE
 993         if(check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE &&
 994                         pmlmepriv->cur_network.join_res == _TRUE)
 995         {
 996                 _irqL   irqL;
 997                 struct sta_info *psta;
 998                 u8 *mac_addr, *peer_addr;
 999                 struct sta_priv *pstapriv = &padapter->stapriv;
1000                 u8 RC_OUI[4]={0x00,0xE0,0x4C,0x0A};
1001                 //EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2]
1002
1003                 p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _VENDOR_SPECIFIC_IE_, (int *)&ielen,
1004                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
1005
1006                 if(!p || ielen !=14)
1007                         goto _non_rc_device;
1008
1009                 if(!_rtw_memcmp(p+2, RC_OUI, sizeof(RC_OUI)))
1010                         goto _non_rc_device;
1011
1012                 if(!_rtw_memcmp(p+6, get_sa(pframe), ETH_ALEN))
1013                 {
1014                         DBG_871X("%s, do rc pairing ("MAC_FMT"), but mac addr mismatch!("MAC_FMT")\n", __FUNCTION__,
1015                                 MAC_ARG(get_sa(pframe)), MAC_ARG(p+6));
1016
1017                         goto _non_rc_device;
1018                 }
1019
1020                 DBG_871X("%s, got the pairing device("MAC_FMT")\n", __FUNCTION__,  MAC_ARG(get_sa(pframe)));
1021
1022                 //new a station
1023                 psta = rtw_get_stainfo(pstapriv, get_sa(pframe));
1024                 if (psta == NULL)
1025                 {
1026                         // allocate a new one
1027                         DBG_871X("going to alloc stainfo for rc="MAC_FMT"\n",  MAC_ARG(get_sa(pframe)));
1028                         psta = rtw_alloc_stainfo(pstapriv, get_sa(pframe));
1029                         if (psta == NULL)
1030                         {
1031                                 //TODO:
1032                                 DBG_871X(" Exceed the upper limit of supported clients...\n");
1033                                 return _SUCCESS;
1034                         }
1035
1036                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1037                         if (rtw_is_list_empty(&psta->asoc_list))
1038                         {
1039                                 psta->expire_to = pstapriv->expire_to;
1040                                 rtw_list_insert_tail(&psta->asoc_list, &pstapriv->asoc_list);
1041                                 pstapriv->asoc_list_cnt++;
1042                         }
1043                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1044
1045                         //generate pairing ID
1046                         mac_addr = adapter_mac_addr(padapter);
1047                         peer_addr = psta->hwaddr;
1048                         psta->pid = (u16)(((mac_addr[4]<<8) + mac_addr[5]) + ((peer_addr[4]<<8) + peer_addr[5]));
1049
1050                         //update peer stainfo
1051                         psta->isrc = _TRUE;
1052                         //psta->aid = 0;
1053                         //psta->mac_id = 2;
1054
1055                         /* get a unique AID */
1056                         if (psta->aid > 0) {
1057                                 DBG_871X("old AID %d\n", psta->aid);
1058                         } else {
1059                                 for (psta->aid = 1; psta->aid <= NUM_STA; psta->aid++)
1060                                         if (pstapriv->sta_aid[psta->aid - 1] == NULL)
1061                                                 break;
1062
1063                                 if (psta->aid > pstapriv->max_num_sta) {
1064                                         psta->aid = 0;
1065                                         DBG_871X("no room for more AIDs\n");
1066                                         return _SUCCESS;
1067                                 } else {
1068                                         pstapriv->sta_aid[psta->aid - 1] = psta;
1069                                         DBG_871X("allocate new AID = (%d)\n", psta->aid);
1070                                 }
1071                         }
1072
1073                         psta->qos_option = 1;
1074                         psta->bw_mode = CHANNEL_WIDTH_20;
1075                         psta->ieee8021x_blocked = _FALSE;
1076 #ifdef CONFIG_80211N_HT
1077                         psta->htpriv.ht_option = _TRUE;
1078                         psta->htpriv.ampdu_enable = _FALSE;
1079                         psta->htpriv.sgi_20m = _FALSE;
1080                         psta->htpriv.sgi_40m = _FALSE;
1081                         psta->htpriv.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
1082                         psta->htpriv.agg_enable_bitmap = 0x0;//reset
1083                         psta->htpriv.candidate_tid_bitmap = 0x0;//reset
1084 #endif
1085
1086                         rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, _TRUE);
1087
1088                         _rtw_memset((void*)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
1089
1090                         _enter_critical_bh(&psta->lock, &irqL);
1091                         psta->state |= _FW_LINKED;
1092                         _exit_critical_bh(&psta->lock, &irqL);
1093
1094                         report_add_sta_event(padapter, psta->hwaddr, psta->aid);
1095
1096                 }
1097
1098                 issue_probersp(padapter, get_sa(pframe), _FALSE);
1099
1100                 return _SUCCESS;
1101
1102         }
1103
1104 _non_rc_device:
1105
1106         return _SUCCESS;
1107
1108 #endif //CONFIG_AUTO_AP_MODE
1109
1110
1111 #ifdef CONFIG_CONCURRENT_MODE
1112         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
1113                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
1114         {
1115                 //don't process probe req
1116                 return _SUCCESS;
1117         }
1118 #endif
1119
1120         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
1121                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
1122
1123
1124         //check (wildcard) SSID
1125         if (p != NULL)
1126         {
1127                 if(is_valid_p2p_probereq == _TRUE)
1128                 {
1129                         goto _issue_probersp;
1130                 }
1131
1132                 if ( (ielen != 0 && _FALSE ==_rtw_memcmp((void *)(p+2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength))
1133                         || (ielen == 0 && pmlmeinfo->hidden_ssid_mode)
1134                 )
1135                 {
1136                         return _SUCCESS;
1137                 }
1138
1139 _issue_probersp:
1140                 if(((check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE &&
1141                         pmlmepriv->cur_network.join_res == _TRUE)) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
1142                 {
1143                         //DBG_871X("+issue_probersp during ap mode\n");
1144                         issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
1145                 }
1146
1147         }
1148
1149         return _SUCCESS;
1150
1151 }
1152
1153 unsigned int OnProbeRsp(_adapter *padapter, union recv_frame *precv_frame)
1154 {
1155         struct sta_info         *psta;
1156         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1157         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1158         struct sta_priv         *pstapriv = &padapter->stapriv;
1159         u8      *pframe = precv_frame->u.hdr.rx_data;
1160 #ifdef CONFIG_P2P
1161         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
1162 #endif
1163
1164
1165 #ifdef CONFIG_P2P
1166         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
1167         {
1168                 if ( _TRUE == pwdinfo->tx_prov_disc_info.benable )
1169                 {
1170                         if( _rtw_memcmp( pwdinfo->tx_prov_disc_info.peerIFAddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
1171                         {
1172                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT))
1173                                 {
1174                                         pwdinfo->tx_prov_disc_info.benable = _FALSE;
1175                                         issue_p2p_provision_request( padapter,
1176                                                                                                 pwdinfo->tx_prov_disc_info.ssid.Ssid,
1177                                                                                                 pwdinfo->tx_prov_disc_info.ssid.SsidLength,
1178                                                                                                 pwdinfo->tx_prov_disc_info.peerDevAddr );
1179                                 }
1180                                 else if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
1181                                 {
1182                                         pwdinfo->tx_prov_disc_info.benable = _FALSE;
1183                                         issue_p2p_provision_request( padapter,
1184                                                                                                 NULL,
1185                                                                                                 0,
1186                                                                                                 pwdinfo->tx_prov_disc_info.peerDevAddr );
1187                                 }
1188                         }
1189                 }
1190                 return _SUCCESS;
1191         }
1192         else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
1193         {
1194                 if ( _TRUE == pwdinfo->nego_req_info.benable )
1195                 {
1196                         DBG_871X( "[%s] P2P State is GONEGO ING!\n", __FUNCTION__ );
1197                         if( _rtw_memcmp( pwdinfo->nego_req_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
1198                         {
1199                                 pwdinfo->nego_req_info.benable = _FALSE;
1200                                 issue_p2p_GO_request( padapter, pwdinfo->nego_req_info.peerDevAddr);
1201                         }
1202                 }
1203         }
1204         else if( rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ ) )
1205         {
1206                 if ( _TRUE == pwdinfo->invitereq_info.benable )
1207                 {
1208                         DBG_871X( "[%s] P2P_STATE_TX_INVITE_REQ!\n", __FUNCTION__ );
1209                         if( _rtw_memcmp( pwdinfo->invitereq_info.peer_macaddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
1210                         {
1211                                 pwdinfo->invitereq_info.benable = _FALSE;
1212                                 issue_p2p_invitation_request( padapter, pwdinfo->invitereq_info.peer_macaddr );
1213                         }
1214                 }
1215         }
1216 #endif
1217
1218
1219         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
1220         {
1221                 report_survey_event(padapter, precv_frame);
1222 #ifdef CONFIG_CONCURRENT_MODE
1223                 report_survey_event(padapter->pbuddy_adapter, precv_frame);
1224 #endif
1225                 return _SUCCESS;
1226         }
1227
1228         #if 0 //move to validate_recv_mgnt_frame
1229         if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
1230         {
1231                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
1232                 {
1233                         if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
1234                         {
1235                                 psta->sta_stats.rx_mgnt_pkts++;
1236                         }
1237                 }
1238         }
1239         #endif
1240
1241         return _SUCCESS;
1242
1243 }
1244
1245 unsigned int OnBeacon(_adapter *padapter, union recv_frame *precv_frame)
1246 {
1247         int cam_idx;
1248         struct sta_info *psta;
1249         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1250         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1251         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1252         struct sta_priv *pstapriv = &padapter->stapriv;
1253         u8 *pframe = precv_frame->u.hdr.rx_data;
1254         uint len = precv_frame->u.hdr.len;
1255         WLAN_BSSID_EX *pbss;
1256         int ret = _SUCCESS;
1257         u8 *p = NULL;
1258         u32 ielen = 0;
1259 #ifdef CONFIG_TDLS
1260         struct sta_info *ptdls_sta;
1261         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
1262 #endif /* CONFIG_TDLS */
1263
1264 #ifdef CONFIG_ATTEMPT_TO_FIX_AP_BEACON_ERROR
1265         p = rtw_get_ie(pframe + sizeof(struct rtw_ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ielen, precv_frame->u.hdr.len -sizeof(struct rtw_ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_);
1266         if ((p != NULL) && (ielen > 0))
1267         {
1268                 if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D))
1269                 {
1270                         /* Invalid value 0x2D is detected in Extended Supported Rates (ESR) IE. Try to fix the IE length to avoid failed Beacon parsing. */
1271                         DBG_871X("[WIFIDBG] Error in ESR IE is detected in Beacon of BSSID:"MAC_FMT". Fix the length of ESR IE to avoid failed Beacon parsing.\n", MAC_ARG(GetAddr3Ptr(pframe)));
1272                         *(p + 1) = ielen - 1;
1273                 }
1274         }
1275 #endif
1276
1277         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
1278         {
1279                 report_survey_event(padapter, precv_frame);
1280 #ifdef CONFIG_CONCURRENT_MODE
1281                 report_survey_event(padapter->pbuddy_adapter, precv_frame);
1282 #endif
1283                 return _SUCCESS;
1284         }
1285
1286         if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
1287         {
1288                 if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
1289                 {
1290                         //we should update current network before auth, or some IE is wrong
1291                         pbss = (WLAN_BSSID_EX*)rtw_malloc(sizeof(WLAN_BSSID_EX));
1292                         if (pbss) {
1293                                 if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
1294                                         struct beacon_keys recv_beacon;
1295
1296                                         update_network(&(pmlmepriv->cur_network.network), pbss, padapter, _TRUE);
1297                                         rtw_get_bcn_info(&(pmlmepriv->cur_network));
1298
1299                                         // update bcn keys
1300                                         if (rtw_get_bcn_keys(padapter, pframe, len, &recv_beacon) == _TRUE) {
1301                                                 DBG_871X("%s: beacon keys ready\n", __func__);
1302                                                 _rtw_memcpy(&pmlmepriv->cur_beacon_keys,
1303                                                         &recv_beacon, sizeof(recv_beacon));
1304                                                 pmlmepriv->new_beacon_cnts = 0;
1305                                         }
1306                                         else {
1307                                                 DBG_871X_LEVEL(_drv_err_, "%s: get beacon keys failed\n", __func__);
1308                                                 _rtw_memset(&pmlmepriv->cur_beacon_keys, 0, sizeof(recv_beacon));
1309                                                 pmlmepriv->new_beacon_cnts = 0;
1310                                         }
1311                                 }
1312                                 rtw_mfree((u8*)pbss, sizeof(WLAN_BSSID_EX));
1313                         }
1314
1315                         //check the vendor of the assoc AP
1316                         pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct rtw_ieee80211_hdr_3addr), len-sizeof(struct rtw_ieee80211_hdr_3addr));
1317
1318                         //update TSF Value
1319                         update_TSF(pmlmeext, pframe, len);
1320
1321                         //reset for adaptive_early_32k
1322                         pmlmeext->adaptive_tsf_done = _FALSE;
1323                         pmlmeext->DrvBcnEarly = 0xff;
1324                         pmlmeext->DrvBcnTimeOut = 0xff;
1325                         pmlmeext->bcn_cnt = 0;
1326                         _rtw_memset(pmlmeext->bcn_delay_cnt, 0, sizeof(pmlmeext->bcn_delay_cnt));
1327                         _rtw_memset(pmlmeext->bcn_delay_ratio, 0, sizeof(pmlmeext->bcn_delay_ratio));
1328
1329 #ifdef CONFIG_P2P_PS
1330                         process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
1331 #endif //CONFIG_P2P_PS
1332
1333 #if defined(CONFIG_P2P)&&defined(CONFIG_CONCURRENT_MODE)
1334                         if (padapter->registrypriv.wifi_spec) {
1335                                 if (process_p2p_cross_connect_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN)) == _FALSE) {
1336                                         if((padapter->pbuddy_adapter->mlmeextpriv.mlmext_info.state&0x03) == WIFI_FW_AP_STATE) {
1337                                                 DBG_871X_LEVEL(_drv_always_, "no issue auth, P2P cross-connect does not permit\n ");
1338                                                 return _SUCCESS;
1339                                         }
1340                                 }
1341                         }
1342 #endif // CONFIG_P2P CONFIG_P2P and CONFIG_CONCURRENT_MODE
1343
1344                         //start auth
1345                         start_clnt_auth(padapter);
1346
1347                         return _SUCCESS;
1348                 }
1349
1350                 if(((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
1351                 {
1352                         if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
1353                         {
1354                                 #ifdef CONFIG_PATCH_JOIN_WRONG_CHANNEL
1355                                 //Merge from 8712 FW code
1356                                 if (cmp_pkt_chnl_diff(padapter,pframe,len) != 0)
1357                                 {            // join wrong channel, deauth and reconnect
1358                                         issue_deauth(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
1359
1360                                         report_del_sta_event(padapter,(&(pmlmeinfo->network))->MacAddress, WLAN_REASON_JOIN_WRONG_CHANNEL);
1361                                         pmlmeinfo->state &= (~WIFI_FW_ASSOC_SUCCESS);
1362                                         return _SUCCESS;
1363                                 }
1364                                 #endif //CONFIG_PATCH_JOIN_WRONG_CHANNEL
1365
1366                                 ret = rtw_check_bcn_info(padapter, pframe, len);
1367                                 if (!ret) {
1368                                                 DBG_871X_LEVEL(_drv_always_, "ap has changed, disconnect now\n ");
1369                                                 receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 0);
1370                                                 return _SUCCESS;
1371                                 }
1372                                 //update WMM, ERP in the beacon
1373                                 //todo: the timer is used instead of the number of the beacon received
1374                                 if ((sta_rx_pkts(psta) & 0xf) == 0)
1375                                 {
1376                                         //DBG_871X("update_bcn_info\n");
1377                                         update_beacon_info(padapter, pframe, len, psta);
1378                                 }
1379
1380                                 adaptive_early_32k(pmlmeext, pframe, len);
1381
1382 #ifdef CONFIG_TDLS
1383 #ifdef CONFIG_TDLS_CH_SW
1384                                 if (padapter->tdlsinfo.ch_switch_prohibited == _FALSE)
1385                                 {
1386                                         /* Send TDLS Channel Switch Request when receiving Beacon */
1387                                         if ((padapter->tdlsinfo.chsw_info.ch_sw_state & TDLS_CH_SW_INITIATOR_STATE) && (pmlmeext->cur_channel == rtw_get_oper_ch(padapter))) {
1388                                                 ptdlsinfo->chsw_info.ch_sw_state |= TDLS_WAIT_CH_RSP_STATE;
1389                                                 /* DBG_871X("[%s] issue_tdls_ch_switch_req to "MAC_FMT"\n", __FUNCTION__, MAC_ARG(padapter->tdlsinfo.chsw_info.addr)); */
1390                                                 ptdls_sta = rtw_get_stainfo(&padapter->stapriv, padapter->tdlsinfo.chsw_info.addr);
1391                                                 if (ptdls_sta != NULL) {
1392                                                         if (ptdls_sta->tdls_sta_state | TDLS_LINKED_STATE)
1393                                                                 issue_tdls_ch_switch_req(padapter, ptdls_sta);
1394                                                 }
1395                                         }
1396                                 }
1397 #endif
1398 #endif /* CONFIG_TDLS */
1399
1400 #ifdef CONFIG_DFS
1401                                 process_csa_ie(padapter, pframe, len);  //channel switch announcement
1402 #endif //CONFIG_DFS
1403
1404 #ifdef CONFIG_P2P_PS
1405                                 process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
1406 #endif //CONFIG_P2P_PS
1407
1408                                 #if 0 //move to validate_recv_mgnt_frame
1409                                 psta->sta_stats.rx_mgnt_pkts++;
1410                                 #endif
1411                         }
1412                 }
1413                 else if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
1414                 {
1415                         if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
1416                         {
1417                                 //update WMM, ERP in the beacon
1418                                 //todo: the timer is used instead of the number of the beacon received
1419                                 if ((sta_rx_pkts(psta) & 0xf) == 0)
1420                                 {
1421                                         //DBG_871X("update_bcn_info\n");
1422                                         update_beacon_info(padapter, pframe, len, psta);
1423                                 }
1424
1425                                 #if 0 //move to validate_recv_mgnt_frame
1426                                 psta->sta_stats.rx_mgnt_pkts++;
1427                                 #endif
1428                         }
1429                         else
1430                         {
1431                                 //allocate a new CAM entry for IBSS station
1432                                 if ((cam_idx = allocate_fw_sta_entry(padapter)) == NUM_STA)
1433                                 {
1434                                         goto _END_ONBEACON_;
1435                                 }
1436
1437                                 //get supported rate
1438                                 if (update_sta_support_rate(padapter, (pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_), (len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_), cam_idx) == _FAIL)
1439                                 {
1440                                         pmlmeinfo->FW_sta_info[cam_idx].status = 0;
1441                                         goto _END_ONBEACON_;
1442                                 }
1443
1444                                 //update TSF Value
1445                                 update_TSF(pmlmeext, pframe, len);
1446
1447                                 //report sta add event
1448                                 report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
1449                         }
1450                 }
1451         }
1452
1453 _END_ONBEACON_:
1454
1455         return _SUCCESS;
1456
1457 }
1458
1459 unsigned int OnAuth(_adapter *padapter, union recv_frame *precv_frame)
1460 {
1461 #ifdef CONFIG_AP_MODE
1462         _irqL irqL;
1463         unsigned int    auth_mode, seq, ie_len;
1464         unsigned char   *sa, *p;
1465         u16     algorithm;
1466         int     status;
1467         static struct sta_info stat;
1468         struct  sta_info        *pstat=NULL;
1469         struct  sta_priv *pstapriv = &padapter->stapriv;
1470         struct security_priv *psecuritypriv = &padapter->securitypriv;
1471         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1472         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1473         u8 *pframe = precv_frame->u.hdr.rx_data;
1474         uint len = precv_frame->u.hdr.len;
1475         u8      offset = 0;
1476
1477
1478 #ifdef CONFIG_CONCURRENT_MODE
1479         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
1480                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
1481         {
1482                 //don't process auth request;
1483                 return _SUCCESS;
1484         }
1485 #endif //CONFIG_CONCURRENT_MODE
1486
1487         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
1488                 return _FAIL;
1489
1490         DBG_871X("+OnAuth\n");
1491
1492         sa = GetAddr2Ptr(pframe);
1493
1494         auth_mode = psecuritypriv->dot11AuthAlgrthm;
1495
1496         if (GetPrivacy(pframe))
1497         {
1498                 u8      *iv;
1499                 struct rx_pkt_attrib     *prxattrib = &(precv_frame->u.hdr.attrib);
1500
1501                 prxattrib->hdrlen = WLAN_HDR_A3_LEN;
1502                 prxattrib->encrypt = _WEP40_;
1503
1504                 iv = pframe+prxattrib->hdrlen;
1505                 prxattrib->key_index = ((iv[3]>>6)&0x3);
1506
1507                 prxattrib->iv_len = 4;
1508                 prxattrib->icv_len = 4;
1509
1510                 rtw_wep_decrypt(padapter, (u8 *)precv_frame);
1511
1512                 offset = 4;
1513         }
1514
1515         algorithm = le16_to_cpu(*(u16*)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset));
1516         seq     = le16_to_cpu(*(u16*)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2));
1517
1518         DBG_871X("auth alg=%x, seq=%X\n", algorithm, seq);
1519
1520         if (auth_mode == 2 &&
1521                         psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
1522                         psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
1523                 auth_mode = 0;
1524
1525         if ((algorithm > 0 && auth_mode == 0) ||        // rx a shared-key auth but shared not enabled
1526                 (algorithm == 0 && auth_mode == 1) )    // rx a open-system auth but shared-key is enabled
1527         {
1528                 DBG_871X("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n",
1529                         algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
1530
1531                 status = _STATS_NO_SUPP_ALG_;
1532
1533                 goto auth_fail;
1534         }
1535
1536 #if 0 //ACL control
1537         phead = &priv->wlan_acl_list;
1538         plist = phead->next;
1539         //check sa
1540         if (acl_mode == 1)              // 1: positive check, only those on acl_list can be connected.
1541                 res = FAIL;
1542         else
1543                 res = SUCCESS;
1544
1545         while(plist != phead)
1546         {
1547                 paclnode = list_entry(plist, struct rtw_wlan_acl_node, list);
1548                 plist = plist->next;
1549                 if (!memcmp((void *)sa, paclnode->addr, 6)) {
1550                         if (paclnode->mode & 2) { // deny
1551                                 res = FAIL;
1552                                 break;
1553                         }
1554                         else {
1555                                 res = SUCCESS;
1556                                 break;
1557                         }
1558                 }
1559         }
1560
1561         if (res != SUCCESS) {
1562                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,"auth abort because ACL!\n");
1563                 return FAIL;
1564         }
1565 #else
1566         if(rtw_access_ctrl(padapter, sa) == _FALSE)
1567         {
1568                 status = _STATS_UNABLE_HANDLE_STA_;
1569                 goto auth_fail;
1570         }
1571 #endif
1572
1573         pstat = rtw_get_stainfo(pstapriv, sa);
1574         if (pstat == NULL)
1575         {
1576
1577                 // allocate a new one
1578                 DBG_871X("going to alloc stainfo for sa="MAC_FMT"\n",  MAC_ARG(sa));
1579                 pstat = rtw_alloc_stainfo(pstapriv, sa);
1580                 if (pstat == NULL)
1581                 {
1582                         DBG_871X(" Exceed the upper limit of supported clients...\n");
1583                         status = _STATS_UNABLE_HANDLE_STA_;
1584                         goto auth_fail;
1585                 }
1586
1587                 pstat->state = WIFI_FW_AUTH_NULL;
1588                 pstat->auth_seq = 0;
1589
1590                 //pstat->flags = 0;
1591                 //pstat->capability = 0;
1592         }
1593         else
1594         {
1595
1596                 _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1597                 if(rtw_is_list_empty(&pstat->asoc_list)==_FALSE)
1598                 {
1599                         rtw_list_delete(&pstat->asoc_list);
1600                         pstapriv->asoc_list_cnt--;
1601                         if (pstat->expire_to > 0)
1602                         {
1603                                 //TODO: STA re_auth within expire_to
1604                         }
1605                 }
1606                 _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1607
1608                 if (seq==1) {
1609                         //TODO: STA re_auth and auth timeout
1610                 }
1611         }
1612
1613         _enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
1614         if (rtw_is_list_empty(&pstat->auth_list))
1615         {
1616
1617                 rtw_list_insert_tail(&pstat->auth_list, &pstapriv->auth_list);
1618                 pstapriv->auth_list_cnt++;
1619         }
1620         _exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
1621
1622         if (pstat->auth_seq == 0)
1623                 pstat->expire_to = pstapriv->auth_to;
1624
1625
1626         if ((pstat->auth_seq + 1) != seq)
1627         {
1628                 DBG_871X("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1629                         seq, pstat->auth_seq+1);
1630                 status = _STATS_OUT_OF_AUTH_SEQ_;
1631                 goto auth_fail;
1632         }
1633
1634         if (algorithm==0 && (auth_mode == 0 || auth_mode == 2 || auth_mode == 3))
1635         {
1636                 if (seq == 1)
1637                 {
1638                         pstat->state &= ~WIFI_FW_AUTH_NULL;
1639                         pstat->state |= WIFI_FW_AUTH_SUCCESS;
1640                         pstat->expire_to = pstapriv->assoc_to;
1641                         pstat->authalg = algorithm;
1642                 }
1643                 else
1644                 {
1645                         DBG_871X("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1646                                 seq, pstat->auth_seq+1);
1647                         status = _STATS_OUT_OF_AUTH_SEQ_;
1648                         goto auth_fail;
1649                 }
1650         }
1651         else // shared system or auto authentication
1652         {
1653                 if (seq == 1)
1654                 {
1655                         //prepare for the challenging txt...
1656
1657                         //get_random_bytes((void *)pstat->chg_txt, 128);//TODO:
1658                         _rtw_memset((void *)pstat->chg_txt, 78, 128);
1659
1660                         pstat->state &= ~WIFI_FW_AUTH_NULL;
1661                         pstat->state |= WIFI_FW_AUTH_STATE;
1662                         pstat->authalg = algorithm;
1663                         pstat->auth_seq = 2;
1664                 }
1665                 else if (seq == 3)
1666                 {
1667                         //checking for challenging txt...
1668                         DBG_871X("checking for challenging txt...\n");
1669
1670                         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len,
1671                                         len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
1672
1673                         if((p==NULL) || (ie_len<=0))
1674                         {
1675                                 DBG_871X("auth rejected because challenge failure!(1)\n");
1676                                 status = _STATS_CHALLENGE_FAIL_;
1677                                 goto auth_fail;
1678                         }
1679
1680                         if (_rtw_memcmp((void *)(p + 2), pstat->chg_txt, 128))
1681                         {
1682                                 pstat->state &= (~WIFI_FW_AUTH_STATE);
1683                                 pstat->state |= WIFI_FW_AUTH_SUCCESS;
1684                                 // challenging txt is correct...
1685                                 pstat->expire_to =  pstapriv->assoc_to;
1686                         }
1687                         else
1688                         {
1689                                 DBG_871X("auth rejected because challenge failure!\n");
1690                                 status = _STATS_CHALLENGE_FAIL_;
1691                                 goto auth_fail;
1692                         }
1693                 }
1694                 else
1695                 {
1696                         DBG_871X("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1697                                 seq, pstat->auth_seq+1);
1698                         status = _STATS_OUT_OF_AUTH_SEQ_;
1699                         goto auth_fail;
1700                 }
1701         }
1702
1703
1704         // Now, we are going to issue_auth...
1705         pstat->auth_seq = seq + 1;
1706
1707 #ifdef CONFIG_NATIVEAP_MLME
1708         issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
1709 #endif
1710
1711         if (pstat->state & WIFI_FW_AUTH_SUCCESS)
1712                 pstat->auth_seq = 0;
1713
1714
1715         return _SUCCESS;
1716
1717 auth_fail:
1718
1719         if(pstat)
1720                 rtw_free_stainfo(padapter , pstat);
1721
1722         pstat = &stat;
1723         _rtw_memset((char *)pstat, '\0', sizeof(stat));
1724         pstat->auth_seq = 2;
1725         _rtw_memcpy(pstat->hwaddr, sa, 6);
1726
1727 #ifdef CONFIG_NATIVEAP_MLME
1728         issue_auth(padapter, pstat, (unsigned short)status);
1729 #endif
1730
1731 #endif
1732         return _FAIL;
1733
1734 }
1735
1736 unsigned int OnAuthClient(_adapter *padapter, union recv_frame *precv_frame)
1737 {
1738         unsigned int    seq, len, status, algthm, offset;
1739         unsigned char   *p;
1740         unsigned int    go2asoc = 0;
1741         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1742         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1743         u8 *pframe = precv_frame->u.hdr.rx_data;
1744         uint pkt_len = precv_frame->u.hdr.len;
1745
1746         DBG_871X("%s\n", __FUNCTION__);
1747
1748         //check A1 matches or not
1749         if (!_rtw_memcmp(adapter_mac_addr(padapter), get_da(pframe), ETH_ALEN))
1750                 return _SUCCESS;
1751
1752         if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
1753                 return _SUCCESS;
1754
1755         offset = (GetPrivacy(pframe))? 4: 0;
1756
1757         algthm  = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset));
1758         seq     = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2));
1759         status  = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 4));
1760
1761         if (status != 0)
1762         {
1763                 DBG_871X("clnt auth fail, status: %d\n", status);
1764                 if(status == 13)//&& pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto)
1765                 {
1766                         if(pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1767                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
1768                         else
1769                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
1770                         //pmlmeinfo->reauth_count = 0;
1771                 }
1772
1773                 set_link_timer(pmlmeext, 1);
1774                 goto authclnt_fail;
1775         }
1776
1777         if (seq == 2)
1778         {
1779                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1780                 {
1781                          // legendary shared system
1782                         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
1783                                 pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
1784
1785                         if (p == NULL)
1786                         {
1787                                 //DBG_871X("marc: no challenge text?\n");
1788                                 goto authclnt_fail;
1789                         }
1790
1791                         _rtw_memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
1792                         pmlmeinfo->auth_seq = 3;
1793                         issue_auth(padapter, NULL, 0);
1794                         set_link_timer(pmlmeext, REAUTH_TO);
1795
1796                         return _SUCCESS;
1797                 }
1798                 else
1799                 {
1800                         // open system
1801                         go2asoc = 1;
1802                 }
1803         }
1804         else if (seq == 4)
1805         {
1806                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1807                 {
1808                         go2asoc = 1;
1809                 }
1810                 else
1811                 {
1812                         goto authclnt_fail;
1813                 }
1814         }
1815         else
1816         {
1817                 // this is also illegal
1818                 //DBG_871X("marc: clnt auth failed due to illegal seq=%x\n", seq);
1819                 goto authclnt_fail;
1820         }
1821
1822         if (go2asoc)
1823         {
1824                 DBG_871X_LEVEL(_drv_always_, "auth success, start assoc\n");
1825                 start_clnt_assoc(padapter);
1826                 return _SUCCESS;
1827         }
1828
1829 authclnt_fail:
1830
1831         //pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE);
1832
1833         return _FAIL;
1834
1835 }
1836
1837 unsigned int OnAssocReq(_adapter *padapter, union recv_frame *precv_frame)
1838 {
1839 #ifdef CONFIG_AP_MODE
1840         _irqL irqL;
1841         u16 capab_info, listen_interval;
1842         struct rtw_ieee802_11_elems elems;
1843         struct sta_info *pstat;
1844         unsigned char           reassoc, *p, *pos, *wpa_ie;
1845         unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
1846         int             i, ie_len, wpa_ie_len, left;
1847         unsigned char           supportRate[16];
1848         int                                     supportRateNum;
1849         unsigned short          status = _STATS_SUCCESSFUL_;
1850         unsigned short          frame_type, ie_offset=0;
1851         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1852         struct security_priv *psecuritypriv = &padapter->securitypriv;
1853         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
1854         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1855         WLAN_BSSID_EX   *cur = &(pmlmeinfo->network);
1856         struct sta_priv *pstapriv = &padapter->stapriv;
1857         u8 *pframe = precv_frame->u.hdr.rx_data;
1858         uint pkt_len = precv_frame->u.hdr.len;
1859 #ifdef CONFIG_P2P
1860         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
1861         u8 p2p_status_code = P2P_STATUS_SUCCESS;
1862         u8 *p2pie;
1863         u32 p2pielen = 0;
1864 #ifdef CONFIG_WFD
1865         u8      wfd_ie[ 128 ] = { 0x00 };
1866         u32     wfd_ielen = 0;
1867 #endif // CONFIG_WFD
1868 #endif //CONFIG_P2P
1869
1870 #ifdef CONFIG_CONCURRENT_MODE
1871         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
1872                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
1873         {
1874                 //don't process assoc request;
1875                 return _SUCCESS;
1876         }
1877 #endif //CONFIG_CONCURRENT_MODE
1878
1879         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
1880                 return _FAIL;
1881
1882         frame_type = GetFrameSubType(pframe);
1883         if (frame_type == WIFI_ASSOCREQ)
1884         {
1885                 reassoc = 0;
1886                 ie_offset = _ASOCREQ_IE_OFFSET_;
1887         }
1888         else // WIFI_REASSOCREQ
1889         {
1890                 reassoc = 1;
1891                 ie_offset = _REASOCREQ_IE_OFFSET_;
1892         }
1893
1894
1895         if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
1896                 DBG_871X("handle_assoc(reassoc=%d) - too short payload (len=%lu)"
1897                        "\n", reassoc, (unsigned long)pkt_len);
1898                 return _FAIL;
1899         }
1900
1901         pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
1902         if (pstat == (struct sta_info *)NULL)
1903         {
1904                 status = _RSON_CLS2_;
1905                 goto asoc_class2_error;
1906         }
1907
1908         capab_info = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN);
1909         //capab_info = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
1910         //listen_interval = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN+2));
1911         listen_interval = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN+2);
1912
1913         left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
1914         pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
1915
1916
1917         DBG_871X("%s\n", __FUNCTION__);
1918
1919         // check if this stat has been successfully authenticated/assocated
1920         if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS))
1921         {
1922                 if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS))
1923                 {
1924                         status = _RSON_CLS2_;
1925                         goto asoc_class2_error;
1926                 }
1927                 else
1928                 {
1929                         pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
1930                         pstat->state |= WIFI_FW_ASSOC_STATE;
1931                 }
1932         }
1933         else
1934         {
1935                 pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
1936                 pstat->state |= WIFI_FW_ASSOC_STATE;
1937         }
1938
1939
1940 #if 0// todo:tkip_countermeasures
1941         if (hapd->tkip_countermeasures) {
1942                 resp = WLAN_REASON_MICHAEL_MIC_FAILURE;
1943                 goto fail;
1944         }
1945 #endif
1946
1947         pstat->capability = capab_info;
1948
1949 #if 0//todo:
1950         //check listen_interval
1951         if (listen_interval > hapd->conf->max_listen_interval) {
1952                 hostapd_logger(hapd, mgmt->sa, HOSTAPD_MODULE_IEEE80211,
1953                                HOSTAPD_LEVEL_DEBUG,
1954                                "Too large Listen Interval (%d)",
1955                                listen_interval);
1956                 resp = WLAN_STATUS_ASSOC_DENIED_LISTEN_INT_TOO_LARGE;
1957                 goto fail;
1958         }
1959
1960         pstat->listen_interval = listen_interval;
1961 #endif
1962
1963         //now parse all ieee802_11 ie to point to elems
1964         if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
1965             !elems.ssid) {
1966                 DBG_871X("STA " MAC_FMT " sent invalid association request\n",
1967                        MAC_ARG(pstat->hwaddr));
1968                 status = _STATS_FAILURE_;
1969                 goto OnAssocReqFail;
1970         }
1971
1972
1973         // now we should check all the fields...
1974         // checking SSID
1975         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
1976                 pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1977         if (p == NULL)
1978         {
1979                 status = _STATS_FAILURE_;
1980         }
1981
1982         if (ie_len == 0) // broadcast ssid, however it is not allowed in assocreq
1983                 status = _STATS_FAILURE_;
1984         else
1985         {
1986                 // check if ssid match
1987                 if (!_rtw_memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
1988                         status = _STATS_FAILURE_;
1989
1990                 if (ie_len != cur->Ssid.SsidLength)
1991                         status = _STATS_FAILURE_;
1992         }
1993
1994         if(_STATS_SUCCESSFUL_ != status)
1995                 goto OnAssocReqFail;
1996
1997         // check if the supported rate is ok
1998         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1999         if (p == NULL) {
2000                 DBG_871X("Rx a sta assoc-req which supported rate is empty!\n");
2001                 // use our own rate set as statoin used
2002                 //_rtw_memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN);
2003                 //supportRateNum = AP_BSSRATE_LEN;
2004
2005                 status = _STATS_FAILURE_;
2006                 goto OnAssocReqFail;
2007         }
2008         else {
2009                 _rtw_memcpy(supportRate, p+2, ie_len);
2010                 supportRateNum = ie_len;
2011
2012                 p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_ , &ie_len,
2013                                 pkt_len - WLAN_HDR_A3_LEN - ie_offset);
2014                 if (p !=  NULL) {
2015
2016                         if(supportRateNum<=sizeof(supportRate))
2017                         {
2018                                 _rtw_memcpy(supportRate+supportRateNum, p+2, ie_len);
2019                                 supportRateNum += ie_len;
2020                         }
2021                 }
2022         }
2023
2024         //todo: mask supportRate between AP & STA -> move to update raid
2025         //get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0);
2026
2027         //update station supportRate
2028         pstat->bssratelen = supportRateNum;
2029         _rtw_memcpy(pstat->bssrateset, supportRate, supportRateNum);
2030         UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
2031
2032         //check RSN/WPA/WPS
2033         pstat->dot8021xalg = 0;
2034         pstat->wpa_psk = 0;
2035         pstat->wpa_group_cipher = 0;
2036         pstat->wpa2_group_cipher = 0;
2037         pstat->wpa_pairwise_cipher = 0;
2038         pstat->wpa2_pairwise_cipher = 0;
2039         _rtw_memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
2040         if((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
2041
2042                 int group_cipher=0, pairwise_cipher=0;
2043
2044                 wpa_ie = elems.rsn_ie;
2045                 wpa_ie_len = elems.rsn_ie_len;
2046
2047                 if(rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS)
2048                 {
2049                         pstat->dot8021xalg = 1;//psk,  todo:802.1x
2050                         pstat->wpa_psk |= BIT(1);
2051
2052                         pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
2053                         pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
2054
2055                         if(!pstat->wpa2_group_cipher)
2056                                 status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
2057
2058                         if(!pstat->wpa2_pairwise_cipher)
2059                                 status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
2060                 }
2061                 else
2062                 {
2063                         status = WLAN_STATUS_INVALID_IE;
2064                 }
2065
2066         } else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
2067
2068                 int group_cipher=0, pairwise_cipher=0;
2069
2070                 wpa_ie = elems.wpa_ie;
2071                 wpa_ie_len = elems.wpa_ie_len;
2072
2073                 if(rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS)
2074                 {
2075                         pstat->dot8021xalg = 1;//psk,  todo:802.1x
2076                         pstat->wpa_psk |= BIT(0);
2077
2078                         pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
2079                         pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
2080
2081                         if(!pstat->wpa_group_cipher)
2082                                 status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
2083
2084                         if(!pstat->wpa_pairwise_cipher)
2085                                 status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
2086
2087                 }
2088                 else
2089                 {
2090                         status = WLAN_STATUS_INVALID_IE;
2091                 }
2092
2093         } else {
2094                 wpa_ie = NULL;
2095                 wpa_ie_len = 0;
2096         }
2097
2098         if(_STATS_SUCCESSFUL_ != status)
2099                 goto OnAssocReqFail;
2100
2101         pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
2102         //if (hapd->conf->wps_state && wpa_ie == NULL) { //todo: to check ap if supporting WPS
2103         if(wpa_ie == NULL) {
2104                 if (elems.wps_ie) {
2105                         DBG_871X("STA included WPS IE in "
2106                                    "(Re)Association Request - assume WPS is "
2107                                    "used\n");
2108                         pstat->flags |= WLAN_STA_WPS;
2109                         //wpabuf_free(sta->wps_ie);
2110                         //sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4,
2111                         //                              elems.wps_ie_len - 4);
2112                 } else {
2113                         DBG_871X("STA did not include WPA/RSN IE "
2114                                    "in (Re)Association Request - possible WPS "
2115                                    "use\n");
2116                         pstat->flags |= WLAN_STA_MAYBE_WPS;
2117                 }
2118
2119
2120                 // AP support WPA/RSN, and sta is going to do WPS, but AP is not ready
2121                 // that the selected registrar of AP is _FLASE
2122                 if((psecuritypriv->wpa_psk >0)
2123                         && (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS)))
2124                 {
2125                         if(pmlmepriv->wps_beacon_ie)
2126                         {
2127                                 u8 selected_registrar = 0;
2128
2129                                 rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR , &selected_registrar, NULL);
2130
2131                                 if(!selected_registrar)
2132                                 {
2133                                         DBG_871X("selected_registrar is _FALSE , or AP is not ready to do WPS\n");
2134
2135                                         status = _STATS_UNABLE_HANDLE_STA_;
2136
2137                                         goto OnAssocReqFail;
2138                                 }
2139                         }
2140                 }
2141
2142         }
2143         else
2144         {
2145                 int copy_len;
2146
2147                 if(psecuritypriv->wpa_psk == 0)
2148                 {
2149                         DBG_871X("STA " MAC_FMT ": WPA/RSN IE in association "
2150                         "request, but AP don't support WPA/RSN\n", MAC_ARG(pstat->hwaddr));
2151
2152                         status = WLAN_STATUS_INVALID_IE;
2153
2154                         goto OnAssocReqFail;
2155
2156                 }
2157
2158                 if (elems.wps_ie) {
2159                         DBG_871X("STA included WPS IE in "
2160                                    "(Re)Association Request - WPS is "
2161                                    "used\n");
2162                         pstat->flags |= WLAN_STA_WPS;
2163                         copy_len=0;
2164                 }
2165                 else
2166                 {
2167                         copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)):(wpa_ie_len+2);
2168                 }
2169
2170
2171                 if(copy_len>0)
2172                         _rtw_memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
2173
2174         }
2175
2176
2177         // check if there is WMM IE & support WWM-PS
2178         pstat->flags &= ~WLAN_STA_WME;
2179         pstat->qos_option = 0;
2180         pstat->qos_info = 0;
2181         pstat->has_legacy_ac = _TRUE;
2182         pstat->uapsd_vo = 0;
2183         pstat->uapsd_vi = 0;
2184         pstat->uapsd_be = 0;
2185         pstat->uapsd_bk = 0;
2186         if (pmlmepriv->qospriv.qos_option)
2187         {
2188                 p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
2189                 for (;;)
2190                 {
2191                         p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
2192                         if (p != NULL) {
2193                                 if (_rtw_memcmp(p+2, WMM_IE, 6)) {
2194
2195                                         pstat->flags |= WLAN_STA_WME;
2196
2197                                         pstat->qos_option = 1;
2198                                         pstat->qos_info = *(p+8);
2199
2200                                         pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
2201
2202                                         if((pstat->qos_info&0xf) !=0xf)
2203                                                 pstat->has_legacy_ac = _TRUE;
2204                                         else
2205                                                 pstat->has_legacy_ac = _FALSE;
2206
2207                                         if(pstat->qos_info&0xf)
2208                                         {
2209                                                 if(pstat->qos_info&BIT(0))
2210                                                         pstat->uapsd_vo = BIT(0)|BIT(1);
2211                                                 else
2212                                                         pstat->uapsd_vo = 0;
2213
2214                                                 if(pstat->qos_info&BIT(1))
2215                                                         pstat->uapsd_vi = BIT(0)|BIT(1);
2216                                                 else
2217                                                         pstat->uapsd_vi = 0;
2218
2219                                                 if(pstat->qos_info&BIT(2))
2220                                                         pstat->uapsd_bk = BIT(0)|BIT(1);
2221                                                 else
2222                                                         pstat->uapsd_bk = 0;
2223
2224                                                 if(pstat->qos_info&BIT(3))
2225                                                         pstat->uapsd_be = BIT(0)|BIT(1);
2226                                                 else
2227                                                         pstat->uapsd_be = 0;
2228
2229                                         }
2230
2231                                         break;
2232                                 }
2233                         }
2234                         else {
2235                                 break;
2236                         }
2237                         p = p + ie_len + 2;
2238                 }
2239         }
2240
2241
2242 #ifdef CONFIG_80211N_HT
2243         /* save HT capabilities in the sta object */
2244         _rtw_memset(&pstat->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));
2245         if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap))
2246         {
2247                 pstat->flags |= WLAN_STA_HT;
2248
2249                 pstat->flags |= WLAN_STA_WME;
2250
2251                 _rtw_memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap));
2252
2253         } else
2254                 pstat->flags &= ~WLAN_STA_HT;
2255
2256
2257         if((pmlmepriv->htpriv.ht_option == _FALSE) && (pstat->flags&WLAN_STA_HT))
2258         {
2259                 status = _STATS_FAILURE_;
2260                 goto OnAssocReqFail;
2261         }
2262
2263
2264         if ((pstat->flags & WLAN_STA_HT) &&
2265                     ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
2266                       (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP)))
2267         {
2268                 DBG_871X("HT: " MAC_FMT " tried to "
2269                                    "use TKIP with HT association\n", MAC_ARG(pstat->hwaddr));
2270
2271                 //status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY;
2272                 //goto OnAssocReqFail;
2273         }
2274 #endif /* CONFIG_80211N_HT */
2275
2276 #ifdef CONFIG_80211AC_VHT
2277         _rtw_memset(&pstat->vhtpriv, 0, sizeof(struct vht_priv));
2278         if (elems.vht_capabilities && elems.vht_capabilities_len == 12) {
2279                 pstat->flags |= WLAN_STA_VHT;
2280
2281                 _rtw_memcpy(pstat->vhtpriv.vht_cap, elems.vht_capabilities, 12);
2282
2283                 if (elems.vht_op_mode_notify && elems.vht_op_mode_notify_len == 1) {
2284                         _rtw_memcpy(&pstat->vhtpriv.vht_op_mode_notify, elems.vht_op_mode_notify, 1);
2285                 }
2286                 else // for Frame without Operating Mode notify ie; default: 80M
2287                 {
2288                         pstat->vhtpriv.vht_op_mode_notify = CHANNEL_WIDTH_80;
2289                 }
2290         }
2291         else {
2292                 pstat->flags &= ~WLAN_STA_VHT;
2293         }
2294
2295         if((pmlmepriv->vhtpriv.vht_option == _FALSE) && (pstat->flags&WLAN_STA_VHT))
2296         {
2297                 status = _STATS_FAILURE_;
2298                 goto OnAssocReqFail;
2299         }
2300 #endif /* CONFIG_80211AC_VHT */
2301
2302        //
2303        //if (hapd->iface->current_mode->mode == HOSTAPD_MODE_IEEE80211G)//?
2304         pstat->flags |= WLAN_STA_NONERP;
2305         for (i = 0; i < pstat->bssratelen; i++) {
2306                 if ((pstat->bssrateset[i] & 0x7f) > 22) {
2307                         pstat->flags &= ~WLAN_STA_NONERP;
2308                         break;
2309                 }
2310         }
2311
2312         if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
2313                 pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
2314         else
2315                 pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
2316
2317
2318
2319         if (status != _STATS_SUCCESSFUL_)
2320                 goto OnAssocReqFail;
2321
2322 #ifdef CONFIG_P2P
2323         pstat->is_p2p_device = _FALSE;
2324         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2325         {
2326                 if( (p2pie=rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , NULL, &p2pielen)))
2327                 {
2328                         pstat->is_p2p_device = _TRUE;
2329                         if((p2p_status_code=(u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat))>0)
2330                         {
2331                                 pstat->p2p_status_code = p2p_status_code;
2332                                 status = _STATS_CAP_FAIL_;
2333                                 goto OnAssocReqFail;
2334                         }
2335                 }
2336 #ifdef CONFIG_WFD
2337                 if(rtw_get_wfd_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , wfd_ie, &wfd_ielen ))
2338                 {
2339                         u8      attr_content[ 10 ] = { 0x00 };
2340                         u32     attr_contentlen = 0;
2341
2342                         DBG_8192C( "[%s] WFD IE Found!!\n", __FUNCTION__ );
2343                         rtw_get_wfd_attr_content( wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
2344                         if ( attr_contentlen )
2345                         {
2346                                 pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16( attr_content + 2 );
2347                                 DBG_8192C( "[%s] Peer PORT NUM = %d\n", __FUNCTION__, pwdinfo->wfd_info->peer_rtsp_ctrlport );
2348                         }
2349                 }
2350 #endif
2351         }
2352         pstat->p2p_status_code = p2p_status_code;
2353 #endif //CONFIG_P2P
2354
2355         //TODO: identify_proprietary_vendor_ie();
2356         // Realtek proprietary IE
2357         // identify if this is Broadcom sta
2358         // identify if this is ralink sta
2359         // Customer proprietary IE
2360
2361
2362
2363         /* get a unique AID */
2364         if (pstat->aid > 0) {
2365                 DBG_871X("  old AID %d\n", pstat->aid);
2366         } else {
2367                 for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++) {
2368                         if (pstapriv->sta_aid[pstat->aid - 1] == NULL) {
2369                                 if (pstat->aid > pstapriv->max_num_sta) {
2370                                         pstat->aid = 0;
2371
2372                                         DBG_871X("  no room for more AIDs\n");
2373
2374                                         status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
2375
2376                                         goto OnAssocReqFail;
2377
2378
2379                                 } else {
2380                                         pstapriv->sta_aid[pstat->aid - 1] = pstat;
2381                                         DBG_871X("allocate new AID = (%d)\n", pstat->aid);
2382                                         break;
2383                                 }
2384                         }
2385                 }
2386         }
2387
2388
2389         pstat->state &= (~WIFI_FW_ASSOC_STATE);
2390         pstat->state |= WIFI_FW_ASSOC_SUCCESS;
2391
2392         _enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
2393         if (!rtw_is_list_empty(&pstat->auth_list))
2394         {
2395                 rtw_list_delete(&pstat->auth_list);
2396                 pstapriv->auth_list_cnt--;
2397         }
2398         _exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
2399
2400         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2401         if (rtw_is_list_empty(&pstat->asoc_list))
2402         {
2403                 pstat->expire_to = pstapriv->expire_to;
2404                 rtw_list_insert_tail(&pstat->asoc_list, &pstapriv->asoc_list);
2405                 pstapriv->asoc_list_cnt++;
2406         }
2407         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2408
2409         // now the station is qualified to join our BSS...
2410         if(pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_==status))
2411         {
2412 #ifdef CONFIG_NATIVEAP_MLME
2413                 //.1 bss_cap_update & sta_info_update
2414                 bss_cap_update_on_sta_join(padapter, pstat);
2415                 sta_info_update(padapter, pstat);
2416
2417                 //.2 issue assoc rsp before notify station join event.
2418                 if (frame_type == WIFI_ASSOCREQ)
2419                         issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
2420                 else
2421                         issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
2422
2423 #ifdef CONFIG_IOCTL_CFG80211
2424                 _enter_critical_bh(&pstat->lock, &irqL);
2425                 if(pstat->passoc_req)
2426                 {
2427                         rtw_mfree(pstat->passoc_req, pstat->assoc_req_len);
2428                         pstat->passoc_req = NULL;
2429                         pstat->assoc_req_len = 0;
2430                 }
2431
2432                 pstat->passoc_req =  rtw_zmalloc(pkt_len);
2433                 if(pstat->passoc_req)
2434                 {
2435                         _rtw_memcpy(pstat->passoc_req, pframe, pkt_len);
2436                         pstat->assoc_req_len = pkt_len;
2437                 }
2438                 _exit_critical_bh(&pstat->lock, &irqL);
2439 #endif //CONFIG_IOCTL_CFG80211
2440
2441                 //.3-(1) report sta add event
2442                 report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
2443
2444 #endif //CONFIG_NATIVEAP_MLME
2445         }
2446
2447         return _SUCCESS;
2448
2449 asoc_class2_error:
2450
2451 #ifdef CONFIG_NATIVEAP_MLME
2452         issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
2453 #endif
2454
2455         return _FAIL;
2456
2457 OnAssocReqFail:
2458
2459
2460 #ifdef CONFIG_NATIVEAP_MLME
2461         pstat->aid = 0;
2462         if (frame_type == WIFI_ASSOCREQ)
2463                 issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
2464         else
2465                 issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
2466 #endif
2467
2468
2469 #endif /* CONFIG_AP_MODE */
2470
2471         return _FAIL;
2472
2473 }
2474
2475 unsigned int OnAssocRsp(_adapter *padapter, union recv_frame *precv_frame)
2476 {
2477         uint i;
2478         int res;
2479         unsigned short  status;
2480         PNDIS_802_11_VARIABLE_IEs       pIE;
2481         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2482         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2483         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2484         //WLAN_BSSID_EX                 *cur_network = &(pmlmeinfo->network);
2485         u8 *pframe = precv_frame->u.hdr.rx_data;
2486         uint pkt_len = precv_frame->u.hdr.len;
2487         PNDIS_802_11_VARIABLE_IEs       pWapiIE = NULL;
2488
2489         DBG_871X("%s\n", __FUNCTION__);
2490
2491         //check A1 matches or not
2492         if (!_rtw_memcmp(adapter_mac_addr(padapter), get_da(pframe), ETH_ALEN))
2493                 return _SUCCESS;
2494
2495         if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
2496                 return _SUCCESS;
2497
2498         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
2499                 return _SUCCESS;
2500
2501         _cancel_timer_ex(&pmlmeext->link_timer);
2502
2503         //status
2504         if ((status = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 2))) > 0)
2505         {
2506                 DBG_871X("assoc reject, status code: %d\n", status);
2507                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
2508                 res = -4;
2509                 goto report_assoc_result;
2510         }
2511
2512         //get capabilities
2513         pmlmeinfo->capability = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2514
2515         //set slot time
2516         pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10))? 9: 20;
2517
2518         //AID
2519         res = pmlmeinfo->aid = (int)(le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
2520
2521         //following are moved to join event callback function
2522         //to handle HT, WMM, rate adaptive, update MAC reg
2523         //for not to handle the synchronous IO in the tasklet
2524         for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;)
2525         {
2526                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + i);
2527
2528                 switch (pIE->ElementID)
2529                 {
2530                         case _VENDOR_SPECIFIC_IE_:
2531                                 if (_rtw_memcmp(pIE->data, WMM_PARA_OUI, 6))    //WMM
2532                                 {
2533                                         WMM_param_handler(padapter, pIE);
2534                                 }
2535 #if defined(CONFIG_P2P) && defined(CONFIG_WFD)
2536                                 else if ( _rtw_memcmp(pIE->data, WFD_OUI, 4))           //WFD
2537                                 {
2538                                         DBG_871X( "[%s] Found WFD IE\n", __FUNCTION__ );
2539                                         WFD_info_handler( padapter, pIE );
2540                                 }
2541 #endif
2542                                 break;
2543
2544 #ifdef CONFIG_WAPI_SUPPORT
2545                         case _WAPI_IE_:
2546                                 pWapiIE = pIE;
2547                                 break;
2548 #endif
2549
2550                         case _HT_CAPABILITY_IE_:        //HT caps
2551                                 HT_caps_handler(padapter, pIE);
2552                                 break;
2553
2554                         case _HT_EXTRA_INFO_IE_:        //HT info
2555                                 HT_info_handler(padapter, pIE);
2556                                 break;
2557
2558 #ifdef CONFIG_80211AC_VHT
2559                         case EID_VHTCapability:
2560                                 VHT_caps_handler(padapter, pIE);
2561                                 break;
2562
2563                         case EID_VHTOperation:
2564                                 VHT_operation_handler(padapter, pIE);
2565                                 break;
2566 #endif
2567
2568                         case _ERPINFO_IE_:
2569                                 ERP_IE_handler(padapter, pIE);
2570                                 break;
2571 #ifdef CONFIG_TDLS
2572                         case _EXT_CAP_IE_:
2573                                 if (check_ap_tdls_prohibited(pIE->data, pIE->Length) == _TRUE)
2574                                         padapter->tdlsinfo.ap_prohibited = _TRUE;
2575                                 if (check_ap_tdls_ch_switching_prohibited(pIE->data, pIE->Length) == _TRUE)
2576                                         padapter->tdlsinfo.ch_switch_prohibited = _TRUE;
2577                                 break;
2578 #endif /* CONFIG_TDLS */
2579                         default:
2580                                 break;
2581                 }
2582
2583                 i += (pIE->Length + 2);
2584         }
2585
2586 #ifdef CONFIG_WAPI_SUPPORT
2587         rtw_wapi_on_assoc_ok(padapter, pIE);
2588 #endif
2589
2590         pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
2591         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
2592
2593         //Update Basic Rate Table for spec, 2010-12-28 , by thomas
2594         UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);
2595
2596 report_assoc_result:
2597         if (res > 0) {
2598                 rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
2599         } else {
2600                 rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
2601         }
2602
2603         report_join_res(padapter, res);
2604
2605         return _SUCCESS;
2606 }
2607
2608 unsigned int OnDeAuth(_adapter *padapter, union recv_frame *precv_frame)
2609 {
2610         unsigned short  reason;
2611         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2612         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2613         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2614         u8 *pframe = precv_frame->u.hdr.rx_data;
2615 #ifdef CONFIG_P2P
2616         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
2617 #endif //CONFIG_P2P
2618
2619         //check A3
2620         if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
2621                 return _SUCCESS;
2622
2623 #ifdef CONFIG_P2P
2624         if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
2625         {
2626                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
2627                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
2628         }
2629 #endif //CONFIG_P2P
2630
2631         reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2632
2633         rtw_lock_rx_suspend_timeout(8000);
2634
2635 #ifdef CONFIG_AP_MODE
2636         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
2637         {
2638                 _irqL irqL;
2639                 struct sta_info *psta;
2640                 struct sta_priv *pstapriv = &padapter->stapriv;
2641
2642                 //_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2643                 //rtw_free_stainfo(padapter, psta);
2644                 //_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2645
2646                 DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" reason=%u, ta=%pM\n"
2647                         , FUNC_ADPT_ARG(padapter), reason, GetAddr2Ptr(pframe));
2648
2649                 psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2650                 if(psta)
2651                 {
2652                         u8 updated = _FALSE;
2653
2654                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2655                         if(rtw_is_list_empty(&psta->asoc_list)==_FALSE)
2656                         {
2657                                 rtw_list_delete(&psta->asoc_list);
2658                                 pstapriv->asoc_list_cnt--;
2659                                 updated = ap_free_sta(padapter, psta, _FALSE, reason);
2660
2661                         }
2662                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2663
2664                         associated_clients_update(padapter, updated);
2665                 }
2666
2667
2668                 return _SUCCESS;
2669         }
2670         else
2671 #endif
2672         {
2673                 int     ignore_received_deauth = 0;
2674
2675                 //      Commented by Albert 20130604
2676                 //      Before sending the auth frame to start the STA/GC mode connection with AP/GO,
2677                 //      we will send the deauth first.
2678                 //      However, the Win8.1 with BRCM Wi-Fi will send the deauth with reason code 6 to us after receieving our deauth.
2679                 //      Added the following code to avoid this case.
2680                 if ( ( pmlmeinfo->state & WIFI_FW_AUTH_STATE ) ||
2681                         ( pmlmeinfo->state & WIFI_FW_ASSOC_STATE ) )
2682                 {
2683                         if ( reason == WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA )
2684                         {
2685                                 ignore_received_deauth = 1;
2686                         } else if (WLAN_REASON_PREV_AUTH_NOT_VALID == reason) {
2687                                 // TODO: 802.11r
2688                                 ignore_received_deauth = 1;
2689                         }
2690                 }
2691
2692                 DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" reason=%u, ta=%pM, ignore=%d\n"
2693                         , FUNC_ADPT_ARG(padapter), reason, GetAddr2Ptr(pframe), ignore_received_deauth);
2694
2695                 if ( 0 == ignore_received_deauth )
2696                 {
2697                         receive_disconnect(padapter, GetAddr2Ptr(pframe), reason);
2698                 }
2699         }
2700         pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE;
2701         return _SUCCESS;
2702
2703 }
2704
2705 unsigned int OnDisassoc(_adapter *padapter, union recv_frame *precv_frame)
2706 {
2707         unsigned short  reason;
2708         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2709         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2710         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2711         u8 *pframe = precv_frame->u.hdr.rx_data;
2712 #ifdef CONFIG_P2P
2713         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
2714 #endif //CONFIG_P2P
2715
2716         //check A3
2717         if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
2718                 return _SUCCESS;
2719
2720 #ifdef CONFIG_P2P
2721         if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
2722         {
2723                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
2724                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
2725         }
2726 #endif //CONFIG_P2P
2727
2728         reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2729
2730         rtw_lock_rx_suspend_timeout(8000);
2731
2732 #ifdef CONFIG_AP_MODE
2733         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
2734         {
2735                 _irqL irqL;
2736                 struct sta_info *psta;
2737                 struct sta_priv *pstapriv = &padapter->stapriv;
2738
2739                 //_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2740                 //rtw_free_stainfo(padapter, psta);
2741                 //_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2742
2743                 DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" reason=%u, ta=%pM\n"
2744                         , FUNC_ADPT_ARG(padapter), reason, GetAddr2Ptr(pframe));
2745
2746                 psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2747                 if(psta)
2748                 {
2749                         u8 updated = _FALSE;
2750
2751                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2752                         if(rtw_is_list_empty(&psta->asoc_list)==_FALSE)
2753                         {
2754                                 rtw_list_delete(&psta->asoc_list);
2755                                 pstapriv->asoc_list_cnt--;
2756                                 updated = ap_free_sta(padapter, psta, _FALSE, reason);
2757
2758                         }
2759                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2760
2761                         associated_clients_update(padapter, updated);
2762                 }
2763
2764                 return _SUCCESS;
2765         }
2766         else
2767 #endif
2768         {
2769                 DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" reason=%u, ta=%pM\n"
2770                         , FUNC_ADPT_ARG(padapter), reason, GetAddr2Ptr(pframe));
2771
2772                 receive_disconnect(padapter, GetAddr2Ptr(pframe), reason);
2773         }
2774         pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE;
2775         return _SUCCESS;
2776
2777 }
2778
2779 unsigned int OnAtim(_adapter *padapter, union recv_frame *precv_frame)
2780 {
2781         DBG_871X("%s\n", __FUNCTION__);
2782         return _SUCCESS;
2783 }
2784
2785 unsigned int on_action_spct_ch_switch(_adapter *padapter, struct sta_info *psta, u8 *ies, uint ies_len)
2786 {
2787         unsigned int ret = _FAIL;
2788         struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
2789         struct mlme_ext_info    *pmlmeinfo = &(mlmeext->mlmext_info);
2790
2791         if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
2792                 ret = _SUCCESS;
2793                 goto exit;
2794         }
2795
2796         if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
2797
2798                 int ch_switch_mode = -1, ch = -1, ch_switch_cnt = -1;
2799                 int ch_offset = -1;
2800                 u8 bwmode;
2801                 struct ieee80211_info_element *ie;
2802
2803                 DBG_871X(FUNC_NDEV_FMT" from "MAC_FMT"\n",
2804                         FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(psta->hwaddr));
2805
2806                 for_each_ie(ie, ies, ies_len) {
2807                         if (ie->id == WLAN_EID_CHANNEL_SWITCH) {
2808                                 ch_switch_mode = ie->data[0];
2809                                 ch = ie->data[1];
2810                                 ch_switch_cnt = ie->data[2];
2811                                 DBG_871X("ch_switch_mode:%d, ch:%d, ch_switch_cnt:%d\n",
2812                                         ch_switch_mode, ch, ch_switch_cnt);
2813                         }
2814                         else if (ie->id == WLAN_EID_SECONDARY_CHANNEL_OFFSET) {
2815                                 ch_offset = secondary_ch_offset_to_hal_ch_offset(ie->data[0]);
2816                                 DBG_871X("ch_offset:%d\n", ch_offset);
2817                         }
2818                 }
2819
2820                 if (ch == -1)
2821                         return _SUCCESS;
2822
2823                 if (ch_offset == -1)
2824                         bwmode = mlmeext->cur_bwmode;
2825                 else
2826                         bwmode = (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) ?
2827                                 CHANNEL_WIDTH_20 : CHANNEL_WIDTH_40;
2828
2829                 ch_offset = (ch_offset == -1) ? mlmeext->cur_ch_offset : ch_offset;
2830
2831                 /* todo:
2832                  * 1. the decision of channel switching
2833                  * 2. things after channel switching
2834                  */
2835
2836                 ret = rtw_set_ch_cmd(padapter, ch, bwmode, ch_offset, _TRUE);
2837         }
2838
2839 exit:
2840         return ret;
2841 }
2842
2843 unsigned int on_action_spct(_adapter *padapter, union recv_frame *precv_frame)
2844 {
2845         unsigned int ret = _FAIL;
2846         struct sta_info *psta = NULL;
2847         struct sta_priv *pstapriv = &padapter->stapriv;
2848         u8 *pframe = precv_frame->u.hdr.rx_data;
2849         uint frame_len = precv_frame->u.hdr.len;
2850         u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
2851         u8 category;
2852         u8 action;
2853
2854         DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
2855
2856         psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2857
2858         if (!psta)
2859                 goto exit;
2860
2861         category = frame_body[0];
2862         if(category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
2863                 goto exit;
2864
2865         action = frame_body[1];
2866         switch (action) {
2867         case RTW_WLAN_ACTION_SPCT_MSR_REQ:
2868         case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
2869         case RTW_WLAN_ACTION_SPCT_TPC_REQ:
2870         case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
2871                 break;
2872         case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
2873                 #ifdef CONFIG_SPCT_CH_SWITCH
2874                 ret = on_action_spct_ch_switch(padapter, psta, &frame_body[2],
2875                         frame_len-(frame_body-pframe)-2);
2876                 #endif
2877                 break;
2878         default:
2879                 break;
2880         }
2881
2882 exit:
2883         return ret;
2884 }
2885
2886 unsigned int OnAction_qos(_adapter *padapter, union recv_frame *precv_frame)
2887 {
2888         return _SUCCESS;
2889 }
2890
2891 unsigned int OnAction_dls(_adapter *padapter, union recv_frame *precv_frame)
2892 {
2893         return _SUCCESS;
2894 }
2895
2896 /**
2897  * rtw_rx_ampdu_size - Get the target RX AMPDU buffer size for the specific @adapter
2898  * @adapter: the adapter to get target RX AMPDU buffer size
2899  *
2900  * Returns: the target RX AMPDU buffer size
2901  */
2902 u8 rtw_rx_ampdu_size(_adapter *adapter)
2903 {
2904         u8 size;
2905         HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor;
2906
2907         if (adapter->fix_rx_ampdu_size != RX_AMPDU_SIZE_INVALID) {
2908                 size = adapter->fix_rx_ampdu_size;
2909                 goto exit;
2910         }
2911
2912 #ifdef CONFIG_BT_COEXIST
2913         if (rtw_btcoex_IsBTCoexCtrlAMPDUSize(adapter) == _TRUE) {
2914                 size = rtw_btcoex_GetAMPDUSize(adapter);
2915                 goto exit;
2916         }
2917 #endif
2918
2919         /* default value based on max_rx_ampdu_factor */
2920         if (adapter->driver_rx_ampdu_factor != 0xFF)
2921                 max_rx_ampdu_factor = (HT_CAP_AMPDU_FACTOR)adapter->driver_rx_ampdu_factor;
2922         else
2923                 rtw_hal_get_def_var(adapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
2924
2925         if (MAX_AMPDU_FACTOR_64K == max_rx_ampdu_factor)
2926                 size = 64;
2927         else if (MAX_AMPDU_FACTOR_32K == max_rx_ampdu_factor)
2928                 size = 32;
2929         else if (MAX_AMPDU_FACTOR_16K == max_rx_ampdu_factor)
2930                 size = 16;
2931         else if (MAX_AMPDU_FACTOR_8K == max_rx_ampdu_factor)
2932                 size = 8;
2933         else
2934                 size = 64;
2935
2936 exit:
2937
2938         if (size > 127)
2939                 size = 127;
2940
2941         return size;
2942 }
2943
2944 /**
2945  * rtw_rx_ampdu_is_accept - Get the permission if RX AMPDU should be set up for the specific @adapter
2946  * @adapter: the adapter to get the permission if RX AMPDU should be set up
2947  *
2948  * Returns: accept or not
2949  */
2950 bool rtw_rx_ampdu_is_accept(_adapter *adapter)
2951 {
2952         bool accept;
2953
2954         if (adapter->fix_rx_ampdu_accept != RX_AMPDU_ACCEPT_INVALID) {
2955                 accept = adapter->fix_rx_ampdu_accept;
2956                 goto exit;
2957         }
2958
2959 #ifdef CONFIG_BT_COEXIST
2960         if (rtw_btcoex_IsBTCoexRejectAMPDU(adapter) == _TRUE) {
2961                 accept = _FALSE;
2962                 goto exit;
2963         }
2964 #endif
2965
2966         /* default value for other cases */
2967         accept = adapter->mlmeextpriv.mlmext_info.bAcceptAddbaReq;
2968
2969 exit:
2970         return accept;
2971 }
2972
2973 /**
2974  * rtw_rx_ampdu_set_size - Set the target RX AMPDU buffer size for the specific @adapter and specific @reason
2975  * @adapter: the adapter to set target RX AMPDU buffer size
2976  * @size: the target RX AMPDU buffer size to set
2977  * @reason: reason for the target RX AMPDU buffer size setting
2978  *
2979  * Returns: whether the target RX AMPDU buffer size is changed
2980  */
2981 bool rtw_rx_ampdu_set_size(_adapter *adapter, u8 size, u8 reason)
2982 {
2983         bool is_adj = _FALSE;
2984         struct mlme_ext_priv *mlmeext;
2985         struct mlme_ext_info *mlmeinfo;
2986
2987         mlmeext = &adapter->mlmeextpriv;
2988         mlmeinfo = &mlmeext->mlmext_info;
2989
2990         if (reason == RX_AMPDU_DRV_FIXED) {
2991                 if (adapter->fix_rx_ampdu_size != size) {
2992                         adapter->fix_rx_ampdu_size = size;
2993                         is_adj = _TRUE;
2994                         DBG_871X(FUNC_ADPT_FMT" fix_rx_ampdu_size:%u\n", FUNC_ADPT_ARG(adapter), size);
2995                 }
2996         }
2997
2998         return is_adj;
2999 }
3000
3001 /**
3002  * rtw_rx_ampdu_set_accept - Set the permission if RX AMPDU should be set up for the specific @adapter and specific @reason
3003  * @adapter: the adapter to set if RX AMPDU should be set up
3004  * @accept: if RX AMPDU should be set up
3005  * @reason: reason for the permission if RX AMPDU should be set up
3006  *
3007  * Returns: whether the permission if RX AMPDU should be set up is changed
3008  */
3009 bool rtw_rx_ampdu_set_accept(_adapter *adapter, u8 accept, u8 reason)
3010 {
3011         bool is_adj = _FALSE;
3012         struct mlme_ext_priv *mlmeext;
3013         struct mlme_ext_info *mlmeinfo;
3014
3015         mlmeext = &adapter->mlmeextpriv;
3016         mlmeinfo = &mlmeext->mlmext_info;
3017
3018         if (reason == RX_AMPDU_DRV_FIXED) {
3019                 if (adapter->fix_rx_ampdu_accept != accept) {
3020                         adapter->fix_rx_ampdu_accept = accept;
3021                         is_adj = _TRUE;
3022                         DBG_871X(FUNC_ADPT_FMT" fix_rx_ampdu_accept:%u\n", FUNC_ADPT_ARG(adapter), accept);
3023                 }
3024         }
3025
3026         return is_adj;
3027 }
3028
3029 /**
3030  * rx_ampdu_apply_sta_tid - Apply RX AMPDU setting to the specific @sta and @tid
3031  * @adapter: the adapter to which @sta belongs
3032  * @sta: the sta to be checked
3033  * @tid: the tid to be checked
3034  * @accept: the target permission if RX AMPDU should be set up
3035  * @size: the target RX AMPDU buffer size
3036  *
3037  * Returns:
3038  * 0: no canceled
3039  * 1: canceled by no permission
3040  * 2: canceled by different buffer size
3041  * 3: canceled by potential mismatched status
3042  *
3043  * Blocking function, may sleep
3044  */
3045 u8 rx_ampdu_apply_sta_tid(_adapter *adapter, struct sta_info *sta, u8 tid, u8 accept, u8 size)
3046 {
3047         u8 ret = 0;
3048         struct recv_reorder_ctrl *reorder_ctl = &sta->recvreorder_ctrl[tid];
3049
3050         if (reorder_ctl->enable == _FALSE) {
3051                 if (reorder_ctl->ampdu_size != RX_AMPDU_SIZE_INVALID) {
3052                         send_delba_sta_tid_wait_ack(adapter, 0, sta, tid, 1);
3053                         ret = 3;
3054                 }
3055                 goto exit;
3056         }
3057
3058         if (accept == _FALSE) {
3059                 send_delba_sta_tid_wait_ack(adapter, 0, sta, tid, 0);
3060                 ret = 1;
3061         } else if (reorder_ctl->ampdu_size != size) {
3062                 send_delba_sta_tid_wait_ack(adapter, 0, sta, tid, 0);
3063                 ret = 2;
3064         }
3065
3066 exit:
3067         return ret;
3068 }
3069
3070 /**
3071  * rx_ampdu_apply_sta - Apply RX AMPDU setting to the specific @sta
3072  * @adapter: the adapter to which @sta belongs
3073  * @sta: the sta to be checked
3074  * @accept: the target permission if RX AMPDU should be set up
3075  * @size: the target RX AMPDU buffer size
3076  *
3077  * Returns: number of the RX AMPDU assciation canceled for applying current target setting
3078  *
3079  * Blocking function, may sleep
3080  */
3081 u8 rx_ampdu_apply_sta(_adapter *adapter, struct sta_info *sta, u8 accept, u8 size)
3082 {
3083         u8 change_cnt = 0;
3084         int i;
3085
3086         for (i = 0; i < TID_NUM; i++) {
3087                 if (rx_ampdu_apply_sta_tid(adapter, sta, i, accept, size) != 0)
3088                         change_cnt++;
3089         }
3090
3091         return change_cnt;
3092 }
3093
3094 /**
3095  * rtw_rx_ampdu_apply - Apply the current target RX AMPDU setting for the specific @adapter
3096  * @adapter: the adapter to be applied
3097  *
3098  * Returns: number of the RX AMPDU assciation canceled for applying current target setting
3099  */
3100 u16 rtw_rx_ampdu_apply(_adapter *adapter)
3101 {
3102         u16 adj_cnt = 0;
3103         struct mlme_ext_priv *mlmeext;
3104         struct sta_info *sta;
3105         u8 accept = rtw_rx_ampdu_is_accept(adapter);
3106         u8 size = rtw_rx_ampdu_size(adapter);
3107
3108         mlmeext = &adapter->mlmeextpriv;
3109
3110         if (mlmeext_msr(mlmeext) == WIFI_FW_STATION_STATE) {
3111                 sta = rtw_get_stainfo(&adapter->stapriv, get_bssid(&adapter->mlmepriv));
3112                 if (sta)
3113                         adj_cnt += rx_ampdu_apply_sta(adapter, sta, accept, size);
3114
3115         } else if (mlmeext_msr(mlmeext) == WIFI_FW_AP_STATE) {
3116                 _irqL irqL;
3117                 _list *phead, *plist;
3118                 u8 peer_num = 0;
3119                 char peers[NUM_STA];
3120                 struct sta_priv *pstapriv = &adapter->stapriv;
3121                 int i;
3122
3123                 _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
3124
3125                 phead = &pstapriv->asoc_list;
3126                 plist = get_next(phead);
3127
3128                 while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) {
3129                         int stainfo_offset;
3130
3131                         sta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
3132                         plist = get_next(plist);
3133
3134                         stainfo_offset = rtw_stainfo_offset(pstapriv, sta);
3135                         if (stainfo_offset_valid(stainfo_offset))
3136                                 peers[peer_num++] = stainfo_offset;
3137                 }
3138
3139                 _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
3140
3141                 for (i = 0; i < peer_num; i++) {
3142                         sta = rtw_get_stainfo_by_offset(pstapriv, peers[i]);
3143                         if (sta)
3144                                 adj_cnt += rx_ampdu_apply_sta(adapter, sta, accept, size);
3145                 }
3146         }
3147
3148         return adj_cnt;
3149 }
3150
3151 unsigned int OnAction_back(_adapter *padapter, union recv_frame *precv_frame)
3152 {
3153         u8 *addr;
3154         struct sta_info *psta=NULL;
3155         struct recv_reorder_ctrl *preorder_ctrl;
3156         unsigned char           *frame_body;
3157         unsigned char           category, action;
3158         unsigned short  tid, status, reason_code = 0;
3159         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
3160         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3161         u8 *pframe = precv_frame->u.hdr.rx_data;
3162         struct sta_priv *pstapriv = &padapter->stapriv;
3163 #ifdef CONFIG_80211N_HT
3164
3165         DBG_871X("%s\n", __FUNCTION__);
3166
3167         //check RA matches or not
3168         if (!_rtw_memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN))
3169                 return _SUCCESS;
3170
3171 /*
3172         //check A1 matches or not
3173         if (!_rtw_memcmp(adapter_mac_addr(padapter), get_da(pframe), ETH_ALEN))
3174                 return _SUCCESS;
3175 */
3176
3177         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
3178                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
3179                         return _SUCCESS;
3180
3181         addr = GetAddr2Ptr(pframe);
3182         psta = rtw_get_stainfo(pstapriv, addr);
3183
3184         if(psta==NULL)
3185                 return _SUCCESS;
3186
3187         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
3188
3189         category = frame_body[0];
3190         if (category == RTW_WLAN_CATEGORY_BACK)// representing Block Ack
3191         {
3192 #ifdef CONFIG_TDLS
3193                 if((psta->tdls_sta_state & TDLS_LINKED_STATE) &&
3194                         (psta->htpriv.ht_option==_TRUE) &&
3195                         (psta->htpriv.ampdu_enable==_TRUE))
3196                 {
3197                         DBG_871X("Recv [%s] from direc link\n", __FUNCTION__);
3198                 }
3199                 else
3200 #endif //CONFIG_TDLS
3201                 if (!pmlmeinfo->HT_enable)
3202                 {
3203                         return _SUCCESS;
3204                 }
3205
3206                 action = frame_body[1];
3207                 DBG_871X("%s, action=%d\n", __FUNCTION__, action);
3208                 switch (action)
3209                 {
3210                         case RTW_WLAN_ACTION_ADDBA_REQ: //ADDBA request
3211
3212                                 _rtw_memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
3213                                 //process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), GetAddr3Ptr(pframe));
3214                                 process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), addr);
3215
3216                                 break;
3217
3218                         case RTW_WLAN_ACTION_ADDBA_RESP: //ADDBA response
3219
3220                                 //status = frame_body[3] | (frame_body[4] << 8); //endian issue
3221                                 status = RTW_GET_LE16(&frame_body[3]);
3222                                 tid = ((frame_body[5] >> 2) & 0x7);
3223
3224                                 if (status == 0)
3225                                 {       //successful
3226                                         DBG_871X("agg_enable for TID=%d\n", tid);
3227                                         psta->htpriv.agg_enable_bitmap |= 1 << tid;
3228                                         psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
3229                                 }
3230                                 else
3231                                 {
3232                                         psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
3233                                 }
3234
3235                                 if(psta->state & WIFI_STA_ALIVE_CHK_STATE)
3236                                 {
3237                                         DBG_871X("%s alive check - rx ADDBA response\n", __func__);
3238                                         psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
3239                                         psta->expire_to = pstapriv->expire_to;
3240                                         psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
3241                                 }
3242
3243                                 //DBG_871X("marc: ADDBA RSP: %x\n", pmlmeinfo->agg_enable_bitmap);
3244                                 break;
3245
3246                         case RTW_WLAN_ACTION_DELBA: //DELBA
3247                                 if ((frame_body[3] & BIT(3)) == 0)
3248                                 {
3249                                         psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
3250                                         psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
3251
3252                                         //reason_code = frame_body[4] | (frame_body[5] << 8);
3253                                         reason_code = RTW_GET_LE16(&frame_body[4]);
3254                                 }
3255                                 else if((frame_body[3] & BIT(3)) == BIT(3))
3256                                 {
3257                                         tid = (frame_body[3] >> 4) & 0x0F;
3258
3259                                         preorder_ctrl = &psta->recvreorder_ctrl[tid];
3260                                         preorder_ctrl->enable = _FALSE;
3261                                         preorder_ctrl->ampdu_size = RX_AMPDU_SIZE_INVALID;
3262                                 }
3263
3264                                 DBG_871X("%s(): DELBA: %x(%x)\n", __FUNCTION__,pmlmeinfo->agg_enable_bitmap, reason_code);
3265                                 //todo: how to notify the host while receiving DELETE BA
3266                                 break;
3267
3268                         default:
3269                                 break;
3270                 }
3271         }
3272 #endif //CONFIG_80211N_HT
3273         return _SUCCESS;
3274 }
3275
3276 #ifdef CONFIG_P2P
3277
3278 static int get_reg_classes_full_count(struct p2p_channels channel_list) {
3279         int cnt = 0;
3280         int i;
3281
3282         for (i = 0; i < channel_list.reg_classes; i++) {
3283                 cnt += channel_list.reg_class[i].channels;
3284         }
3285
3286         return cnt;
3287 }
3288
3289 static void get_channel_cnt_24g_5gl_5gh(  struct mlme_ext_priv *pmlmeext, u8* p24g_cnt, u8* p5gl_cnt, u8* p5gh_cnt )
3290 {
3291         int     i = 0;
3292
3293         *p24g_cnt = 0;
3294         *p5gl_cnt = 0;
3295         *p5gh_cnt = 0;
3296
3297         for( i = 0; i < pmlmeext->max_chan_nums; i++ )
3298         {
3299                 if ( pmlmeext->channel_set[ i ].ChannelNum <= 14 )
3300                 {
3301                         (*p24g_cnt)++;
3302                 }
3303                 else if ( ( pmlmeext->channel_set[ i ].ChannelNum > 14 ) && ( pmlmeext->channel_set[ i ].ChannelNum <= 48 ) )
3304                 {
3305                         //      Just include the channel 36, 40, 44, 48 channels for 5G low
3306                         (*p5gl_cnt)++;
3307                 }
3308                 else if ( ( pmlmeext->channel_set[ i ].ChannelNum >= 149 ) && ( pmlmeext->channel_set[ i ].ChannelNum <= 161 ) )
3309                 {
3310                         //      Just include the channel 149, 153, 157, 161 channels for 5G high
3311                         (*p5gh_cnt)++;
3312                 }
3313         }
3314 }
3315
3316 void issue_p2p_GO_request(_adapter *padapter, u8* raddr)
3317 {
3318
3319         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3320         u8                      action = P2P_PUB_ACTION_ACTION;
3321         u32                     p2poui = cpu_to_be32(P2POUI);
3322         u8                      oui_subtype = P2P_GO_NEGO_REQ;
3323         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
3324         u8                      wpsielen = 0, p2pielen = 0, i;
3325         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
3326         u16                     len_channellist_attr = 0;
3327 #ifdef CONFIG_WFD
3328         u32                                     wfdielen = 0;
3329 #endif //CONFIG_WFD
3330
3331         struct xmit_frame                       *pmgntframe;
3332         struct pkt_attrib                       *pattrib;
3333         unsigned char                                   *pframe;
3334         struct rtw_ieee80211_hdr        *pwlanhdr;
3335         unsigned short                          *fctrl;
3336         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3337         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3338         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3339         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
3340
3341
3342         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
3343         {
3344                 return;
3345         }
3346
3347         DBG_871X( "[%s] In\n", __FUNCTION__ );
3348         //update attribute
3349         pattrib = &pmgntframe->attrib;
3350         update_mgntframe_attrib(padapter, pattrib);
3351
3352         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3353
3354         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3355         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3356
3357         fctrl = &(pwlanhdr->frame_ctl);
3358         *(fctrl) = 0;
3359
3360         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
3361         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
3362         _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN);
3363
3364         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3365         pmlmeext->mgnt_seq++;
3366         SetFrameSubType(pframe, WIFI_ACTION);
3367
3368         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
3369         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3370
3371         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
3372         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
3373         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
3374         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3375         pwdinfo->negotiation_dialog_token = 1;  //      Initialize the dialog value
3376         pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &(pattrib->pktlen));
3377
3378
3379
3380         //      WPS Section
3381         wpsielen = 0;
3382         //      WPS OUI
3383         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
3384         wpsielen += 4;
3385
3386         //      WPS version
3387         //      Type:
3388         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
3389         wpsielen += 2;
3390
3391         //      Length:
3392         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
3393         wpsielen += 2;
3394
3395         //      Value:
3396         wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
3397
3398         //      Device Password ID
3399         //      Type:
3400         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
3401         wpsielen += 2;
3402
3403         //      Length:
3404         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
3405         wpsielen += 2;
3406
3407         //      Value:
3408
3409         if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN )
3410         {
3411                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_USER_SPEC );
3412         }
3413         else if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN )
3414         {
3415                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );
3416         }
3417         else if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC )
3418         {
3419                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_PBC );
3420         }
3421
3422         wpsielen += 2;
3423
3424         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
3425
3426
3427         //      P2P IE Section.
3428
3429         //      P2P OUI
3430         p2pielen = 0;
3431         p2pie[ p2pielen++ ] = 0x50;
3432         p2pie[ p2pielen++ ] = 0x6F;
3433         p2pie[ p2pielen++ ] = 0x9A;
3434         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
3435
3436         //      Commented by Albert 20110306
3437         //      According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes
3438         //      1. P2P Capability
3439         //      2. Group Owner Intent
3440         //      3. Configuration Timeout
3441         //      4. Listen Channel
3442         //      5. Extended Listen Timing
3443         //      6. Intended P2P Interface Address
3444         //      7. Channel List
3445         //      8. P2P Device Info
3446         //      9. Operating Channel
3447
3448
3449         //      P2P Capability
3450         //      Type:
3451         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
3452
3453         //      Length:
3454         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3455         p2pielen += 2;
3456
3457         //      Value:
3458         //      Device Capability Bitmap, 1 byte
3459         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
3460
3461         //      Group Capability Bitmap, 1 byte
3462         if ( pwdinfo->persistent_supported )
3463         {
3464                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
3465         }
3466         else
3467         {
3468                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
3469         }
3470
3471
3472         //      Group Owner Intent
3473         //      Type:
3474         p2pie[ p2pielen++ ] = P2P_ATTR_GO_INTENT;
3475
3476         //      Length:
3477         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3478         p2pielen += 2;
3479
3480         //      Value:
3481         //      Todo the tie breaker bit.
3482         p2pie[ p2pielen++ ] = ( ( pwdinfo->intent << 1 ) &  0xFE );
3483
3484         //      Configuration Timeout
3485         //      Type:
3486         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
3487
3488         //      Length:
3489         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3490         p2pielen += 2;
3491
3492         //      Value:
3493         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
3494         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
3495
3496
3497         //      Listen Channel
3498         //      Type:
3499         p2pie[ p2pielen++ ] = P2P_ATTR_LISTEN_CH;
3500
3501         //      Length:
3502         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
3503         p2pielen += 2;
3504
3505         //      Value:
3506         //      Country String
3507         p2pie[ p2pielen++ ] = 'X';
3508         p2pie[ p2pielen++ ] = 'X';
3509
3510         //      The third byte should be set to 0x04.
3511         //      Described in the "Operating Channel Attribute" section.
3512         p2pie[ p2pielen++ ] = 0x04;
3513
3514         //      Operating Class
3515         p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
3516
3517         //      Channel Number
3518         p2pie[ p2pielen++ ] = pwdinfo->listen_channel;  //      listening channel number
3519
3520
3521         //      Extended Listen Timing ATTR
3522         //      Type:
3523         p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
3524
3525         //      Length:
3526         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
3527         p2pielen += 2;
3528
3529         //      Value:
3530         //      Availability Period
3531         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
3532         p2pielen += 2;
3533
3534         //      Availability Interval
3535         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
3536         p2pielen += 2;
3537
3538
3539         //      Intended P2P Interface Address
3540         //      Type:
3541         p2pie[ p2pielen++ ] = P2P_ATTR_INTENTED_IF_ADDR;
3542
3543         //      Length:
3544         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
3545         p2pielen += 2;
3546
3547         //      Value:
3548         _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
3549         p2pielen += ETH_ALEN;
3550
3551
3552         //      Channel List
3553         //      Type:
3554         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
3555
3556         // Length:
3557         // Country String(3)
3558         // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
3559         // + number of channels in all classes
3560         len_channellist_attr = 3
3561            + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
3562            + get_reg_classes_full_count(pmlmeext->channel_list);
3563
3564 #ifdef CONFIG_CONCURRENT_MODE
3565         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3566         {
3567                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
3568         }
3569         else
3570         {
3571                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3572         }
3573 #else
3574
3575         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3576
3577 #endif
3578         p2pielen += 2;
3579
3580         //      Value:
3581         //      Country String
3582         p2pie[ p2pielen++ ] = 'X';
3583         p2pie[ p2pielen++ ] = 'X';
3584
3585         //      The third byte should be set to 0x04.
3586         //      Described in the "Operating Channel Attribute" section.
3587         p2pie[ p2pielen++ ] = 0x04;
3588
3589         //      Channel Entry List
3590
3591 #ifdef CONFIG_CONCURRENT_MODE
3592         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3593         {
3594                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
3595                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3596
3597                 //      Operating Class
3598                 if ( pbuddy_mlmeext->cur_channel > 14 )
3599                 {
3600                         if ( pbuddy_mlmeext->cur_channel >= 149 )
3601                         {
3602                                 p2pie[ p2pielen++ ] = 0x7c;
3603                         }
3604                         else
3605                         {
3606                                 p2pie[ p2pielen++ ] = 0x73;
3607                         }
3608                 }
3609                 else
3610                 {
3611                         p2pie[ p2pielen++ ] = 0x51;
3612                 }
3613
3614                 //      Number of Channels
3615                 //      Just support 1 channel and this channel is AP's channel
3616                 p2pie[ p2pielen++ ] = 1;
3617
3618                 //      Channel List
3619                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
3620         }
3621         else
3622         {
3623                 int i,j;
3624                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3625                         //      Operating Class
3626                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3627
3628                         //      Number of Channels
3629                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3630
3631                         //      Channel List
3632                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3633                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3634                         }
3635                 }
3636         }
3637 #else // CONFIG_CONCURRENT_MODE
3638         {
3639                 int i,j;
3640                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3641                         //      Operating Class
3642                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3643
3644                         //      Number of Channels
3645                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3646
3647                         //      Channel List
3648                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3649                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3650                         }
3651                 }
3652         }
3653 #endif // CONFIG_CONCURRENT_MODE
3654
3655         //      Device Info
3656         //      Type:
3657         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
3658
3659         //      Length:
3660         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
3661         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
3662         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
3663         p2pielen += 2;
3664
3665         //      Value:
3666         //      P2P Device Address
3667         _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
3668         p2pielen += ETH_ALEN;
3669
3670         //      Config Method
3671         //      This field should be big endian. Noted by P2P specification.
3672
3673         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
3674
3675         p2pielen += 2;
3676
3677         //      Primary Device Type
3678         //      Category ID
3679         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
3680         p2pielen += 2;
3681
3682         //      OUI
3683         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
3684         p2pielen += 4;
3685
3686         //      Sub Category ID
3687         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
3688         p2pielen += 2;
3689
3690         //      Number of Secondary Device Types
3691         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
3692
3693         //      Device Name
3694         //      Type:
3695         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
3696         p2pielen += 2;
3697
3698         //      Length:
3699         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
3700         p2pielen += 2;
3701
3702         //      Value:
3703         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len );
3704         p2pielen += pwdinfo->device_name_len;
3705
3706
3707         //      Operating Channel
3708         //      Type:
3709         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
3710
3711         //      Length:
3712         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
3713         p2pielen += 2;
3714
3715         //      Value:
3716         //      Country String
3717         p2pie[ p2pielen++ ] = 'X';
3718         p2pie[ p2pielen++ ] = 'X';
3719
3720         //      The third byte should be set to 0x04.
3721         //      Described in the "Operating Channel Attribute" section.
3722         p2pie[ p2pielen++ ] = 0x04;
3723
3724         //      Operating Class
3725         if ( pwdinfo->operating_channel <= 14 )
3726         {
3727                 //      Operating Class
3728                 p2pie[ p2pielen++ ] = 0x51;
3729         }
3730         else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
3731         {
3732                 //      Operating Class
3733                 p2pie[ p2pielen++ ] = 0x73;
3734         }
3735         else
3736         {
3737                 //      Operating Class
3738                 p2pie[ p2pielen++ ] = 0x7c;
3739         }
3740
3741         //      Channel Number
3742         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
3743
3744         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
3745
3746 #ifdef CONFIG_WFD
3747         wfdielen = build_nego_req_wfd_ie(pwdinfo, pframe);
3748         pframe += wfdielen;
3749         pattrib->pktlen += wfdielen;
3750 #endif //CONFIG_WFD
3751
3752         pattrib->last_txcmdsz = pattrib->pktlen;
3753
3754         dump_mgntframe(padapter, pmgntframe);
3755
3756         return;
3757
3758 }
3759
3760
3761 void issue_p2p_GO_response(_adapter *padapter, u8* raddr, u8* frame_body,uint len, u8 result)
3762 {
3763
3764         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3765         u8                      action = P2P_PUB_ACTION_ACTION;
3766         u32                     p2poui = cpu_to_be32(P2POUI);
3767         u8                      oui_subtype = P2P_GO_NEGO_RESP;
3768         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
3769         u8                      p2pielen = 0, i;
3770         uint                    wpsielen = 0;
3771         u16                     wps_devicepassword_id = 0x0000;
3772         uint                    wps_devicepassword_id_len = 0;
3773         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh;
3774         u16                     len_channellist_attr = 0;
3775
3776         struct xmit_frame                       *pmgntframe;
3777         struct pkt_attrib                       *pattrib;
3778         unsigned char                                   *pframe;
3779         struct rtw_ieee80211_hdr        *pwlanhdr;
3780         unsigned short                          *fctrl;
3781         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3782         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3783         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3784         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
3785
3786 #ifdef CONFIG_WFD
3787         u32                                     wfdielen = 0;
3788 #endif //CONFIG_WFD
3789
3790         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
3791         {
3792                 return;
3793         }
3794
3795         DBG_871X( "[%s] In, result = %d\n", __FUNCTION__,  result );
3796         //update attribute
3797         pattrib = &pmgntframe->attrib;
3798         update_mgntframe_attrib(padapter, pattrib);
3799
3800         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3801
3802         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3803         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3804
3805         fctrl = &(pwlanhdr->frame_ctl);
3806         *(fctrl) = 0;
3807
3808         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
3809         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
3810         _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN);
3811
3812         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3813         pmlmeext->mgnt_seq++;
3814         SetFrameSubType(pframe, WIFI_ACTION);
3815
3816         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
3817         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3818
3819         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
3820         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
3821         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
3822         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3823         pwdinfo->negotiation_dialog_token = frame_body[7];      //      The Dialog Token of provisioning discovery request frame.
3824         pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
3825
3826         //      Commented by Albert 20110328
3827         //      Try to get the device password ID from the WPS IE of group negotiation request frame
3828         //      WiFi Direct test plan 5.1.15
3829         rtw_get_wps_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
3830         rtw_get_wps_attr_content( wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len);
3831         wps_devicepassword_id = be16_to_cpu( wps_devicepassword_id );
3832
3833         _rtw_memset( wpsie, 0x00, 255 );
3834         wpsielen = 0;
3835
3836         //      WPS Section
3837         wpsielen = 0;
3838         //      WPS OUI
3839         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
3840         wpsielen += 4;
3841
3842         //      WPS version
3843         //      Type:
3844         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
3845         wpsielen += 2;
3846
3847         //      Length:
3848         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
3849         wpsielen += 2;
3850
3851         //      Value:
3852         wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
3853
3854         //      Device Password ID
3855         //      Type:
3856         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
3857         wpsielen += 2;
3858
3859         //      Length:
3860         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
3861         wpsielen += 2;
3862
3863         //      Value:
3864         if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
3865         {
3866                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );
3867         }
3868         else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
3869         {
3870                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_USER_SPEC );
3871         }
3872         else
3873         {
3874                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_PBC );
3875         }
3876         wpsielen += 2;
3877
3878         //      Commented by Kurt 20120113
3879         //      If some device wants to do p2p handshake without sending prov_disc_req
3880         //      We have to get peer_req_cm from here.
3881         if(_rtw_memcmp( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3) )
3882         {
3883                 if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
3884                 {
3885                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3 );
3886                 }
3887                 else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
3888                 {
3889                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3 );
3890                 }
3891                 else
3892                 {
3893                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3 );
3894                 }
3895         }
3896
3897         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
3898
3899
3900         //      P2P IE Section.
3901
3902         //      P2P OUI
3903         p2pielen = 0;
3904         p2pie[ p2pielen++ ] = 0x50;
3905         p2pie[ p2pielen++ ] = 0x6F;
3906         p2pie[ p2pielen++ ] = 0x9A;
3907         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
3908
3909         //      Commented by Albert 20100908
3910         //      According to the P2P Specification, the group negoitation response frame should contain 9 P2P attributes
3911         //      1. Status
3912         //      2. P2P Capability
3913         //      3. Group Owner Intent
3914         //      4. Configuration Timeout
3915         //      5. Operating Channel
3916         //      6. Intended P2P Interface Address
3917         //      7. Channel List
3918         //      8. Device Info
3919         //      9. Group ID     ( Only GO )
3920
3921
3922         //      ToDo:
3923
3924         //      P2P Status
3925         //      Type:
3926         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
3927
3928         //      Length:
3929         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3930         p2pielen += 2;
3931
3932         //      Value:
3933         p2pie[ p2pielen++ ] = result;
3934
3935         //      P2P Capability
3936         //      Type:
3937         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
3938
3939         //      Length:
3940         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3941         p2pielen += 2;
3942
3943         //      Value:
3944         //      Device Capability Bitmap, 1 byte
3945
3946         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
3947         {
3948                 //      Commented by Albert 2011/03/08
3949                 //      According to the P2P specification
3950                 //      if the sending device will be client, the P2P Capability should be reserved of group negotation response frame
3951                 p2pie[ p2pielen++ ] = 0;
3952         }
3953         else
3954         {
3955                 //      Be group owner or meet the error case
3956                 p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
3957         }
3958
3959         //      Group Capability Bitmap, 1 byte
3960         if ( pwdinfo->persistent_supported )
3961         {
3962                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
3963         }
3964         else
3965         {
3966                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
3967         }
3968
3969         //      Group Owner Intent
3970         //      Type:
3971         p2pie[ p2pielen++ ] = P2P_ATTR_GO_INTENT;
3972
3973         //      Length:
3974         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3975         p2pielen += 2;
3976
3977         //      Value:
3978         if ( pwdinfo->peer_intent & 0x01 )
3979         {
3980                 //      Peer's tie breaker bit is 1, our tie breaker bit should be 0
3981                 p2pie[ p2pielen++ ] = ( pwdinfo->intent << 1 );
3982         }
3983         else
3984         {
3985                 //      Peer's tie breaker bit is 0, our tie breaker bit should be 1
3986                 p2pie[ p2pielen++ ] = ( ( pwdinfo->intent << 1 ) | BIT(0) );
3987         }
3988
3989
3990         //      Configuration Timeout
3991         //      Type:
3992         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
3993
3994         //      Length:
3995         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3996         p2pielen += 2;
3997
3998         //      Value:
3999         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
4000         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
4001
4002         //      Operating Channel
4003         //      Type:
4004         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4005
4006         //      Length:
4007         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4008         p2pielen += 2;
4009
4010         //      Value:
4011         //      Country String
4012         p2pie[ p2pielen++ ] = 'X';
4013         p2pie[ p2pielen++ ] = 'X';
4014
4015         //      The third byte should be set to 0x04.
4016         //      Described in the "Operating Channel Attribute" section.
4017         p2pie[ p2pielen++ ] = 0x04;
4018
4019         //      Operating Class
4020         if ( pwdinfo->operating_channel <= 14 )
4021         {
4022                 //      Operating Class
4023                 p2pie[ p2pielen++ ] = 0x51;
4024         }
4025         else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
4026         {
4027                 //      Operating Class
4028                 p2pie[ p2pielen++ ] = 0x73;
4029         }
4030         else
4031         {
4032                 //      Operating Class
4033                 p2pie[ p2pielen++ ] = 0x7c;
4034         }
4035
4036         //      Channel Number
4037         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
4038
4039         //      Intended P2P Interface Address
4040         //      Type:
4041         p2pie[ p2pielen++ ] = P2P_ATTR_INTENTED_IF_ADDR;
4042
4043         //      Length:
4044         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
4045         p2pielen += 2;
4046
4047         //      Value:
4048         _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
4049         p2pielen += ETH_ALEN;
4050
4051         //      Channel List
4052         //      Type:
4053         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
4054
4055         // Country String(3)
4056         // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
4057         // + number of channels in all classes
4058         len_channellist_attr = 3
4059            + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
4060            + get_reg_classes_full_count(pmlmeext->channel_list);
4061
4062 #ifdef CONFIG_CONCURRENT_MODE
4063         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4064         {
4065                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
4066         }
4067         else
4068         {
4069                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4070         }
4071 #else
4072
4073         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4074
4075  #endif
4076         p2pielen += 2;
4077
4078         //      Value:
4079         //      Country String
4080         p2pie[ p2pielen++ ] = 'X';
4081         p2pie[ p2pielen++ ] = 'X';
4082
4083         //      The third byte should be set to 0x04.
4084         //      Described in the "Operating Channel Attribute" section.
4085         p2pie[ p2pielen++ ] = 0x04;
4086
4087         //      Channel Entry List
4088
4089 #ifdef CONFIG_CONCURRENT_MODE
4090         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4091         {
4092                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
4093                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4094
4095                 //      Operating Class
4096                 if ( pbuddy_mlmeext->cur_channel > 14 )
4097                 {
4098                         if ( pbuddy_mlmeext->cur_channel >= 149 )
4099                         {
4100                                 p2pie[ p2pielen++ ] = 0x7c;
4101                         }
4102                         else
4103                         {
4104                                 p2pie[ p2pielen++ ] = 0x73;
4105                         }
4106                 }
4107                 else
4108                 {
4109                         p2pie[ p2pielen++ ] = 0x51;
4110                 }
4111
4112                 //      Number of Channels
4113                 //      Just support 1 channel and this channel is AP's channel
4114                 p2pie[ p2pielen++ ] = 1;
4115
4116                 //      Channel List
4117                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
4118         }
4119         else
4120         {
4121                 int i, j;
4122                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4123                         //      Operating Class
4124                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4125
4126                         //      Number of Channels
4127                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4128
4129                         //      Channel List
4130                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4131                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4132                         }
4133                 }
4134         }
4135 #else // CONFIG_CONCURRENT_MODE
4136         {
4137                 int i, j;
4138                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4139                         //      Operating Class
4140                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4141
4142                         //      Number of Channels
4143                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4144
4145                         //      Channel List
4146                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4147                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4148                         }
4149                 }
4150         }
4151 #endif // CONFIG_CONCURRENT_MODE
4152
4153
4154         //      Device Info
4155         //      Type:
4156         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
4157
4158         //      Length:
4159         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
4160         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
4161         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
4162         p2pielen += 2;
4163
4164         //      Value:
4165         //      P2P Device Address
4166         _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
4167         p2pielen += ETH_ALEN;
4168
4169         //      Config Method
4170         //      This field should be big endian. Noted by P2P specification.
4171
4172         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
4173
4174         p2pielen += 2;
4175
4176         //      Primary Device Type
4177         //      Category ID
4178         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
4179         p2pielen += 2;
4180
4181         //      OUI
4182         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
4183         p2pielen += 4;
4184
4185         //      Sub Category ID
4186         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
4187         p2pielen += 2;
4188
4189         //      Number of Secondary Device Types
4190         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
4191
4192         //      Device Name
4193         //      Type:
4194         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
4195         p2pielen += 2;
4196
4197         //      Length:
4198         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
4199         p2pielen += 2;
4200
4201         //      Value:
4202         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len );
4203         p2pielen += pwdinfo->device_name_len;
4204
4205         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
4206         {
4207                 //      Group ID Attribute
4208                 //      Type:
4209                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
4210
4211                 //      Length:
4212                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + pwdinfo->nego_ssidlen );
4213                 p2pielen += 2;
4214
4215                 //      Value:
4216                 //      p2P Device Address
4217                 _rtw_memcpy( p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN );
4218                 p2pielen += ETH_ALEN;
4219
4220                 //      SSID
4221                 _rtw_memcpy( p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
4222                 p2pielen += pwdinfo->nego_ssidlen;
4223
4224         }
4225
4226         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
4227
4228 #ifdef CONFIG_WFD
4229         wfdielen = build_nego_resp_wfd_ie(pwdinfo, pframe);
4230         pframe += wfdielen;
4231         pattrib->pktlen += wfdielen;
4232 #endif //CONFIG_WFD
4233
4234         pattrib->last_txcmdsz = pattrib->pktlen;
4235
4236         dump_mgntframe(padapter, pmgntframe);
4237
4238         return;
4239
4240 }
4241
4242 void issue_p2p_GO_confirm(_adapter *padapter, u8* raddr, u8 result)
4243 {
4244
4245         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
4246         u8                      action = P2P_PUB_ACTION_ACTION;
4247         u32                     p2poui = cpu_to_be32(P2POUI);
4248         u8                      oui_subtype = P2P_GO_NEGO_CONF;
4249         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
4250         u8                      wpsielen = 0, p2pielen = 0;
4251
4252         struct xmit_frame                       *pmgntframe;
4253         struct pkt_attrib                       *pattrib;
4254         unsigned char                                   *pframe;
4255         struct rtw_ieee80211_hdr        *pwlanhdr;
4256         unsigned short                          *fctrl;
4257         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
4258         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4259         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4260         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
4261 #ifdef CONFIG_WFD
4262         u32                                     wfdielen = 0;
4263 #endif //CONFIG_WFD
4264
4265         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4266         {
4267                 return;
4268         }
4269
4270         DBG_871X( "[%s] In\n", __FUNCTION__ );
4271         //update attribute
4272         pattrib = &pmgntframe->attrib;
4273         update_mgntframe_attrib(padapter, pattrib);
4274
4275         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4276
4277         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4278         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4279
4280         fctrl = &(pwlanhdr->frame_ctl);
4281         *(fctrl) = 0;
4282
4283         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
4284         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
4285         _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN);
4286
4287         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4288         pmlmeext->mgnt_seq++;
4289         SetFrameSubType(pframe, WIFI_ACTION);
4290
4291         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4292         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4293
4294         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4295         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4296         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4297         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4298         pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
4299
4300
4301
4302         //      P2P IE Section.
4303
4304         //      P2P OUI
4305         p2pielen = 0;
4306         p2pie[ p2pielen++ ] = 0x50;
4307         p2pie[ p2pielen++ ] = 0x6F;
4308         p2pie[ p2pielen++ ] = 0x9A;
4309         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
4310
4311         //      Commented by Albert 20110306
4312         //      According to the P2P Specification, the group negoitation request frame should contain 5 P2P attributes
4313         //      1. Status
4314         //      2. P2P Capability
4315         //      3. Operating Channel
4316         //      4. Channel List
4317         //      5. Group ID     ( if this WiFi is GO )
4318
4319         //      P2P Status
4320         //      Type:
4321         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
4322
4323         //      Length:
4324         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
4325         p2pielen += 2;
4326
4327         //      Value:
4328         p2pie[ p2pielen++ ] = result;
4329
4330         //      P2P Capability
4331         //      Type:
4332         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
4333
4334         //      Length:
4335         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
4336         p2pielen += 2;
4337
4338         //      Value:
4339         //      Device Capability Bitmap, 1 byte
4340         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
4341
4342         //      Group Capability Bitmap, 1 byte
4343         if ( pwdinfo->persistent_supported )
4344         {
4345                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
4346         }
4347         else
4348         {
4349                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
4350         }
4351
4352
4353         //      Operating Channel
4354         //      Type:
4355         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4356
4357         //      Length:
4358         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4359         p2pielen += 2;
4360
4361         //      Value:
4362         //      Country String
4363         p2pie[ p2pielen++ ] = 'X';
4364         p2pie[ p2pielen++ ] = 'X';
4365
4366         //      The third byte should be set to 0x04.
4367         //      Described in the "Operating Channel Attribute" section.
4368         p2pie[ p2pielen++ ] = 0x04;
4369
4370
4371         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
4372         {
4373                 if ( pwdinfo->peer_operating_ch <= 14 )
4374                 {
4375                         //      Operating Class
4376                         p2pie[ p2pielen++ ] = 0x51;
4377                 }
4378                 else if ( ( pwdinfo->peer_operating_ch >= 36 ) && ( pwdinfo->peer_operating_ch <= 48 ) )
4379                 {
4380                         //      Operating Class
4381                         p2pie[ p2pielen++ ] = 0x73;
4382                 }
4383                 else
4384                 {
4385                         //      Operating Class
4386                         p2pie[ p2pielen++ ] = 0x7c;
4387                 }
4388
4389                 p2pie[ p2pielen++ ] = pwdinfo->peer_operating_ch;
4390         }
4391         else
4392         {
4393                 if ( pwdinfo->operating_channel <= 14 )
4394                 {
4395                         //      Operating Class
4396                         p2pie[ p2pielen++ ] = 0x51;
4397                 }
4398                 else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
4399                 {
4400                         //      Operating Class
4401                         p2pie[ p2pielen++ ] = 0x73;
4402                 }
4403                 else
4404                 {
4405                         //      Operating Class
4406                         p2pie[ p2pielen++ ] = 0x7c;
4407                 }
4408
4409                 //      Channel Number
4410                 p2pie[ p2pielen++ ] = pwdinfo->operating_channel;               //      Use the listen channel as the operating channel
4411         }
4412
4413
4414         //      Channel List
4415         //      Type:
4416         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
4417
4418         *(u16*) ( p2pie + p2pielen ) = 6;
4419         p2pielen += 2;
4420
4421         //      Country String
4422         p2pie[ p2pielen++ ] = 'X';
4423         p2pie[ p2pielen++ ] = 'X';
4424
4425         //      The third byte should be set to 0x04.
4426         //      Described in the "Operating Channel Attribute" section.
4427         p2pie[ p2pielen++ ] = 0x04;
4428
4429         //      Value:
4430         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
4431         {
4432                 if ( pwdinfo->peer_operating_ch <= 14 )
4433                 {
4434                         //      Operating Class
4435                         p2pie[ p2pielen++ ] = 0x51;
4436                 }
4437                 else if ( ( pwdinfo->peer_operating_ch >= 36 ) && ( pwdinfo->peer_operating_ch <= 48 ) )
4438                 {
4439                         //      Operating Class
4440                         p2pie[ p2pielen++ ] = 0x73;
4441                 }
4442                 else
4443                 {
4444                         //      Operating Class
4445                         p2pie[ p2pielen++ ] = 0x7c;
4446                 }
4447                 p2pie[ p2pielen++ ] = 1;
4448                 p2pie[ p2pielen++ ] = pwdinfo->peer_operating_ch;
4449         }
4450         else
4451         {
4452                 if ( pwdinfo->operating_channel <= 14 )
4453                 {
4454                         //      Operating Class
4455                         p2pie[ p2pielen++ ] = 0x51;
4456                 }
4457                 else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
4458                 {
4459                         //      Operating Class
4460                         p2pie[ p2pielen++ ] = 0x73;
4461                 }
4462                 else
4463                 {
4464                         //      Operating Class
4465                         p2pie[ p2pielen++ ] = 0x7c;
4466                 }
4467
4468                 //      Channel Number
4469                 p2pie[ p2pielen++ ] = 1;
4470                 p2pie[ p2pielen++ ] = pwdinfo->operating_channel;               //      Use the listen channel as the operating channel
4471         }
4472
4473         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
4474         {
4475                 //      Group ID Attribute
4476                 //      Type:
4477                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
4478
4479                 //      Length:
4480                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + pwdinfo->nego_ssidlen );
4481                 p2pielen += 2;
4482
4483                 //      Value:
4484                 //      p2P Device Address
4485                 _rtw_memcpy( p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN );
4486                 p2pielen += ETH_ALEN;
4487
4488                 //      SSID
4489                 _rtw_memcpy( p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
4490                 p2pielen += pwdinfo->nego_ssidlen;
4491         }
4492
4493         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
4494
4495 #ifdef CONFIG_WFD
4496         wfdielen = build_nego_confirm_wfd_ie(pwdinfo, pframe);
4497         pframe += wfdielen;
4498         pattrib->pktlen += wfdielen;
4499 #endif //CONFIG_WFD
4500
4501         pattrib->last_txcmdsz = pattrib->pktlen;
4502
4503         dump_mgntframe(padapter, pmgntframe);
4504
4505         return;
4506
4507 }
4508
4509 void issue_p2p_invitation_request(_adapter *padapter, u8* raddr )
4510 {
4511
4512         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
4513         u8                      action = P2P_PUB_ACTION_ACTION;
4514         u32                     p2poui = cpu_to_be32(P2POUI);
4515         u8                      oui_subtype = P2P_INVIT_REQ;
4516         u8                      p2pie[ 255 ] = { 0x00 };
4517         u8                      p2pielen = 0, i;
4518         u8                      dialogToken = 3;
4519         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
4520         u16                     len_channellist_attr = 0;
4521 #ifdef CONFIG_WFD
4522         u32                                     wfdielen = 0;
4523 #endif //CONFIG_WFD
4524 #ifdef CONFIG_CONCURRENT_MODE
4525         _adapter                                *pbuddy_adapter = padapter->pbuddy_adapter;
4526         struct wifidirect_info  *pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
4527         struct mlme_priv                *pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
4528         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4529 #endif
4530
4531         struct xmit_frame                       *pmgntframe;
4532         struct pkt_attrib                       *pattrib;
4533         unsigned char                                   *pframe;
4534         struct rtw_ieee80211_hdr        *pwlanhdr;
4535         unsigned short                          *fctrl;
4536         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
4537         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4538         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4539         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
4540
4541
4542         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4543         {
4544                 return;
4545         }
4546
4547         //update attribute
4548         pattrib = &pmgntframe->attrib;
4549         update_mgntframe_attrib(padapter, pattrib);
4550
4551         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4552
4553         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4554         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4555
4556         fctrl = &(pwlanhdr->frame_ctl);
4557         *(fctrl) = 0;
4558
4559         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
4560         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
4561         _rtw_memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
4562
4563         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4564         pmlmeext->mgnt_seq++;
4565         SetFrameSubType(pframe, WIFI_ACTION);
4566
4567         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4568         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4569
4570         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4571         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4572         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4573         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4574         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
4575
4576         //      P2P IE Section.
4577
4578         //      P2P OUI
4579         p2pielen = 0;
4580         p2pie[ p2pielen++ ] = 0x50;
4581         p2pie[ p2pielen++ ] = 0x6F;
4582         p2pie[ p2pielen++ ] = 0x9A;
4583         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
4584
4585         //      Commented by Albert 20101011
4586         //      According to the P2P Specification, the P2P Invitation request frame should contain 7 P2P attributes
4587         //      1. Configuration Timeout
4588         //      2. Invitation Flags
4589         //      3. Operating Channel    ( Only GO )
4590         //      4. P2P Group BSSID      ( Should be included if I am the GO )
4591         //      5. Channel List
4592         //      6. P2P Group ID
4593         //      7. P2P Device Info
4594
4595         //      Configuration Timeout
4596         //      Type:
4597         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
4598
4599         //      Length:
4600         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
4601         p2pielen += 2;
4602
4603         //      Value:
4604         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
4605         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
4606
4607         //      Invitation Flags
4608         //      Type:
4609         p2pie[ p2pielen++ ] = P2P_ATTR_INVITATION_FLAGS;
4610
4611         //      Length:
4612         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
4613         p2pielen += 2;
4614
4615         //      Value:
4616         p2pie[ p2pielen++ ] = P2P_INVITATION_FLAGS_PERSISTENT;
4617
4618
4619         //      Operating Channel
4620         //      Type:
4621         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4622
4623         //      Length:
4624         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4625         p2pielen += 2;
4626
4627         //      Value:
4628         //      Country String
4629         p2pie[ p2pielen++ ] = 'X';
4630         p2pie[ p2pielen++ ] = 'X';
4631
4632         //      The third byte should be set to 0x04.
4633         //      Described in the "Operating Channel Attribute" section.
4634         p2pie[ p2pielen++ ] = 0x04;
4635
4636         //      Operating Class
4637         if ( pwdinfo->invitereq_info.operating_ch <= 14 )
4638                 p2pie[ p2pielen++ ] = 0x51;
4639         else if ( ( pwdinfo->invitereq_info.operating_ch >= 36 ) && ( pwdinfo->invitereq_info.operating_ch <= 48 ) )
4640                 p2pie[ p2pielen++ ] = 0x73;
4641         else
4642                 p2pie[ p2pielen++ ] = 0x7c;
4643
4644         //      Channel Number
4645         p2pie[ p2pielen++ ] = pwdinfo->invitereq_info.operating_ch;     //      operating channel number
4646
4647         if (_rtw_memcmp(adapter_mac_addr(padapter), pwdinfo->invitereq_info.go_bssid, ETH_ALEN))
4648         {
4649                 //      P2P Group BSSID
4650                 //      Type:
4651                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_BSSID;
4652
4653                 //      Length:
4654                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
4655                 p2pielen += 2;
4656
4657                 //      Value:
4658                 //      P2P Device Address for GO
4659                 _rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN );
4660                 p2pielen += ETH_ALEN;
4661         }
4662
4663         //      Channel List
4664         //      Type:
4665         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
4666
4667
4668         //      Length:
4669         // Country String(3)
4670         // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
4671         // + number of channels in all classes
4672         len_channellist_attr = 3
4673            + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
4674            + get_reg_classes_full_count(pmlmeext->channel_list);
4675
4676 #ifdef CONFIG_CONCURRENT_MODE
4677         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4678         {
4679                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
4680         }
4681         else
4682         {
4683                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4684         }
4685 #else
4686
4687         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4688
4689  #endif
4690         p2pielen += 2;
4691
4692         //      Value:
4693         //      Country String
4694         p2pie[ p2pielen++ ] = 'X';
4695         p2pie[ p2pielen++ ] = 'X';
4696
4697         //      The third byte should be set to 0x04.
4698         //      Described in the "Operating Channel Attribute" section.
4699         p2pie[ p2pielen++ ] = 0x04;
4700
4701         //      Channel Entry List
4702 #ifdef CONFIG_CONCURRENT_MODE
4703         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4704         {
4705                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
4706                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4707
4708                 //      Operating Class
4709                 if ( pbuddy_mlmeext->cur_channel > 14 )
4710                 {
4711                         if ( pbuddy_mlmeext->cur_channel >= 149 )
4712                         {
4713                                 p2pie[ p2pielen++ ] = 0x7c;
4714                         }
4715                         else
4716                         {
4717                                 p2pie[ p2pielen++ ] = 0x73;
4718                         }
4719                 }
4720                 else
4721                 {
4722                         p2pie[ p2pielen++ ] = 0x51;
4723                 }
4724
4725                 //      Number of Channels
4726                 //      Just support 1 channel and this channel is AP's channel
4727                 p2pie[ p2pielen++ ] = 1;
4728
4729                 //      Channel List
4730                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
4731         }
4732         else
4733         {
4734                 int i, j;
4735                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4736                         //      Operating Class
4737                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4738
4739                         //      Number of Channels
4740                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4741
4742                         //      Channel List
4743                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4744                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4745                         }
4746                 }
4747         }
4748 #else // CONFIG_CONCURRENT_MODE
4749         {
4750                 int i, j;
4751                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4752                         //      Operating Class
4753                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4754
4755                         //      Number of Channels
4756                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4757
4758                         //      Channel List
4759                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4760                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4761                         }
4762                 }
4763         }
4764 #endif // CONFIG_CONCURRENT_MODE
4765
4766
4767         //      P2P Group ID
4768         //      Type:
4769         p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
4770
4771         //      Length:
4772         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 6 + pwdinfo->invitereq_info.ssidlen );
4773         p2pielen += 2;
4774
4775         //      Value:
4776         //      P2P Device Address for GO
4777         _rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN );
4778         p2pielen += ETH_ALEN;
4779
4780         //      SSID
4781         _rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid, pwdinfo->invitereq_info.ssidlen );
4782         p2pielen += pwdinfo->invitereq_info.ssidlen;
4783
4784
4785         //      Device Info
4786         //      Type:
4787         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
4788
4789         //      Length:
4790         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
4791         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
4792         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
4793         p2pielen += 2;
4794
4795         //      Value:
4796         //      P2P Device Address
4797         _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
4798         p2pielen += ETH_ALEN;
4799
4800         //      Config Method
4801         //      This field should be big endian. Noted by P2P specification.
4802         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
4803         p2pielen += 2;
4804
4805         //      Primary Device Type
4806         //      Category ID
4807         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
4808         p2pielen += 2;
4809
4810         //      OUI
4811         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
4812         p2pielen += 4;
4813
4814         //      Sub Category ID
4815         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
4816         p2pielen += 2;
4817
4818         //      Number of Secondary Device Types
4819         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
4820
4821         //      Device Name
4822         //      Type:
4823         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
4824         p2pielen += 2;
4825
4826         //      Length:
4827         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
4828         p2pielen += 2;
4829
4830         //      Value:
4831         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
4832         p2pielen += pwdinfo->device_name_len;
4833
4834         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
4835
4836 #ifdef CONFIG_WFD
4837         wfdielen = build_invitation_req_wfd_ie(pwdinfo, pframe);
4838         pframe += wfdielen;
4839         pattrib->pktlen += wfdielen;
4840 #endif //CONFIG_WFD
4841
4842         pattrib->last_txcmdsz = pattrib->pktlen;
4843
4844         dump_mgntframe(padapter, pmgntframe);
4845
4846         return;
4847
4848 }
4849
4850 void issue_p2p_invitation_response(_adapter *padapter, u8* raddr, u8 dialogToken, u8 status_code)
4851 {
4852
4853         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
4854         u8                      action = P2P_PUB_ACTION_ACTION;
4855         u32                     p2poui = cpu_to_be32(P2POUI);
4856         u8                      oui_subtype = P2P_INVIT_RESP;
4857         u8                      p2pie[ 255 ] = { 0x00 };
4858         u8                      p2pielen = 0, i;
4859         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
4860         u16                     len_channellist_attr = 0;
4861 #ifdef CONFIG_CONCURRENT_MODE
4862         _adapter                                *pbuddy_adapter = padapter->pbuddy_adapter;
4863         struct wifidirect_info  *pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
4864         struct mlme_priv                *pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
4865         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4866 #endif
4867 #ifdef CONFIG_WFD
4868         u32                                     wfdielen = 0;
4869 #endif //CONFIG_WFD
4870
4871         struct xmit_frame                       *pmgntframe;
4872         struct pkt_attrib                       *pattrib;
4873         unsigned char                                   *pframe;
4874         struct rtw_ieee80211_hdr        *pwlanhdr;
4875         unsigned short                          *fctrl;
4876         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
4877         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4878         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4879         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
4880
4881
4882         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4883         {
4884                 return;
4885         }
4886
4887         //update attribute
4888         pattrib = &pmgntframe->attrib;
4889         update_mgntframe_attrib(padapter, pattrib);
4890
4891         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4892
4893         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4894         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4895
4896         fctrl = &(pwlanhdr->frame_ctl);
4897         *(fctrl) = 0;
4898
4899         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
4900         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
4901         _rtw_memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
4902
4903         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4904         pmlmeext->mgnt_seq++;
4905         SetFrameSubType(pframe, WIFI_ACTION);
4906
4907         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4908         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4909
4910         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4911         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4912         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4913         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4914         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
4915
4916         //      P2P IE Section.
4917
4918         //      P2P OUI
4919         p2pielen = 0;
4920         p2pie[ p2pielen++ ] = 0x50;
4921         p2pie[ p2pielen++ ] = 0x6F;
4922         p2pie[ p2pielen++ ] = 0x9A;
4923         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
4924
4925         //      Commented by Albert 20101005
4926         //      According to the P2P Specification, the P2P Invitation response frame should contain 5 P2P attributes
4927         //      1. Status
4928         //      2. Configuration Timeout
4929         //      3. Operating Channel    ( Only GO )
4930         //      4. P2P Group BSSID      ( Only GO )
4931         //      5. Channel List
4932
4933         //      P2P Status
4934         //      Type:
4935         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
4936
4937         //      Length:
4938         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
4939         p2pielen += 2;
4940
4941         //      Value:
4942         //      When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE.
4943         //      Sent the event receiving the P2P Invitation Req frame to DMP UI.
4944         //      DMP had to compare the MAC address to find out the profile.
4945         //      So, the WiFi driver will send the P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB.
4946         //      If the UI found the corresponding profile, the WiFi driver sends the P2P Invitation Req
4947         //      to NB to rebuild the persistent group.
4948         p2pie[ p2pielen++ ] = status_code;
4949
4950         //      Configuration Timeout
4951         //      Type:
4952         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
4953
4954         //      Length:
4955         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
4956         p2pielen += 2;
4957
4958         //      Value:
4959         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
4960         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
4961
4962         if( status_code == P2P_STATUS_SUCCESS )
4963         {
4964                 if( rtw_p2p_chk_role( pwdinfo, P2P_ROLE_GO ) )
4965                 {
4966                         //      The P2P Invitation request frame asks this Wi-Fi device to be the P2P GO
4967                         //      In this case, the P2P Invitation response frame should carry the two more P2P attributes.
4968                         //      First one is operating channel attribute.
4969                         //      Second one is P2P Group BSSID attribute.
4970
4971                         //      Operating Channel
4972                         //      Type:
4973                         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4974
4975                         //      Length:
4976                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4977                         p2pielen += 2;
4978
4979                         //      Value:
4980                         //      Country String
4981                         p2pie[ p2pielen++ ] = 'X';
4982                         p2pie[ p2pielen++ ] = 'X';
4983
4984                         //      The third byte should be set to 0x04.
4985                         //      Described in the "Operating Channel Attribute" section.
4986                         p2pie[ p2pielen++ ] = 0x04;
4987
4988                         //      Operating Class
4989                         p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
4990
4991                         //      Channel Number
4992                         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
4993
4994
4995                         //      P2P Group BSSID
4996                         //      Type:
4997                         p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_BSSID;
4998
4999                         //      Length:
5000                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
5001                         p2pielen += 2;
5002
5003                         //      Value:
5004                         //      P2P Device Address for GO
5005                         _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
5006                         p2pielen += ETH_ALEN;
5007
5008                 }
5009
5010                 //      Channel List
5011                 //      Type:
5012                 p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
5013
5014                 //      Length:
5015                 // Country String(3)
5016                 // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
5017                 // + number of channels in all classes
5018                 len_channellist_attr = 3
5019                         + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
5020                         + get_reg_classes_full_count(pmlmeext->channel_list);
5021
5022 #ifdef CONFIG_CONCURRENT_MODE
5023                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
5024                 {
5025                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
5026                 }
5027                 else
5028                 {
5029                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
5030                 }
5031 #else
5032
5033                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
5034
5035 #endif
5036                 p2pielen += 2;
5037
5038                 //      Value:
5039                 //      Country String
5040                 p2pie[ p2pielen++ ] = 'X';
5041                 p2pie[ p2pielen++ ] = 'X';
5042
5043                 //      The third byte should be set to 0x04.
5044                 //      Described in the "Operating Channel Attribute" section.
5045                 p2pie[ p2pielen++ ] = 0x04;
5046
5047                 //      Channel Entry List
5048 #ifdef CONFIG_CONCURRENT_MODE
5049                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
5050                 {
5051                         _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
5052                         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
5053
5054                         //      Operating Class
5055                         if ( pbuddy_mlmeext->cur_channel > 14 )
5056                         {
5057                                 if ( pbuddy_mlmeext->cur_channel >= 149 )
5058                                 {
5059                                         p2pie[ p2pielen++ ] = 0x7c;
5060                                 }
5061                                 else
5062                                 {
5063                                         p2pie[ p2pielen++ ] = 0x73;
5064                                 }
5065                         }
5066                         else
5067                         {
5068                                 p2pie[ p2pielen++ ] = 0x51;
5069                         }
5070
5071                         //      Number of Channels
5072                         //      Just support 1 channel and this channel is AP's channel
5073                         p2pie[ p2pielen++ ] = 1;
5074
5075                         //      Channel List
5076                         p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
5077                 }
5078                 else
5079                 {
5080                         int i, j;
5081                         for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
5082                                 //      Operating Class
5083                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
5084
5085                                 //      Number of Channels
5086                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
5087
5088                                 //      Channel List
5089                                 for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
5090                                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
5091                                 }
5092                         }
5093                 }
5094 #else // CONFIG_CONCURRENT_MODE
5095                 {
5096                         int i, j;
5097                         for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
5098                                 //      Operating Class
5099                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
5100
5101                                 //      Number of Channels
5102                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
5103
5104                                 //      Channel List
5105                                 for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
5106                                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
5107                                 }
5108                         }
5109                 }
5110 #endif // CONFIG_CONCURRENT_MODE
5111         }
5112
5113         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
5114
5115 #ifdef CONFIG_WFD
5116         wfdielen = build_invitation_resp_wfd_ie(pwdinfo, pframe);
5117         pframe += wfdielen;
5118         pattrib->pktlen += wfdielen;
5119 #endif //CONFIG_WFD
5120
5121         pattrib->last_txcmdsz = pattrib->pktlen;
5122
5123         dump_mgntframe(padapter, pmgntframe);
5124
5125         return;
5126
5127 }
5128
5129 void issue_p2p_provision_request(_adapter *padapter, u8* pssid, u8 ussidlen, u8* pdev_raddr )
5130 {
5131         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
5132         u8                      action = P2P_PUB_ACTION_ACTION;
5133         u8                      dialogToken = 1;
5134         u32                     p2poui = cpu_to_be32(P2POUI);
5135         u8                      oui_subtype = P2P_PROVISION_DISC_REQ;
5136         u8                      wpsie[ 100 ] = { 0x00 };
5137         u8                      wpsielen = 0;
5138         u32                     p2pielen = 0;
5139 #ifdef CONFIG_WFD
5140         u32                                     wfdielen = 0;
5141 #endif //CONFIG_WFD
5142
5143         struct xmit_frame                       *pmgntframe;
5144         struct pkt_attrib                       *pattrib;
5145         unsigned char                                   *pframe;
5146         struct rtw_ieee80211_hdr        *pwlanhdr;
5147         unsigned short                          *fctrl;
5148         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
5149         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5150         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5151         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
5152
5153
5154         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
5155         {
5156                 return;
5157         }
5158
5159         DBG_871X( "[%s] In\n", __FUNCTION__ );
5160         //update attribute
5161         pattrib = &pmgntframe->attrib;
5162         update_mgntframe_attrib(padapter, pattrib);
5163
5164         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5165
5166         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5167         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5168
5169         fctrl = &(pwlanhdr->frame_ctl);
5170         *(fctrl) = 0;
5171
5172         _rtw_memcpy(pwlanhdr->addr1, pdev_raddr, ETH_ALEN);
5173         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
5174         _rtw_memcpy(pwlanhdr->addr3, pdev_raddr, ETH_ALEN);
5175
5176         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5177         pmlmeext->mgnt_seq++;
5178         SetFrameSubType(pframe, WIFI_ACTION);
5179
5180         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
5181         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
5182
5183         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
5184         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
5185         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
5186         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
5187         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
5188
5189         p2pielen = build_prov_disc_request_p2p_ie( pwdinfo, pframe, pssid, ussidlen, pdev_raddr );
5190
5191         pframe += p2pielen;
5192         pattrib->pktlen += p2pielen;
5193
5194         wpsielen = 0;
5195         //      WPS OUI
5196         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
5197         wpsielen += 4;
5198
5199         //      WPS version
5200         //      Type:
5201         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
5202         wpsielen += 2;
5203
5204         //      Length:
5205         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5206         wpsielen += 2;
5207
5208         //      Value:
5209         wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
5210
5211         //      Config Method
5212         //      Type:
5213         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
5214         wpsielen += 2;
5215
5216         //      Length:
5217         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
5218         wpsielen += 2;
5219
5220         //      Value:
5221         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->tx_prov_disc_info.wps_config_method_request );
5222         wpsielen += 2;
5223
5224         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
5225
5226
5227 #ifdef CONFIG_WFD
5228         wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe);
5229         pframe += wfdielen;
5230         pattrib->pktlen += wfdielen;
5231 #endif //CONFIG_WFD
5232
5233         pattrib->last_txcmdsz = pattrib->pktlen;
5234
5235         dump_mgntframe(padapter, pmgntframe);
5236
5237         return;
5238
5239 }
5240
5241
5242 u8 is_matched_in_profilelist( u8* peermacaddr, struct profile_info* profileinfo )
5243 {
5244         u8 i, match_result = 0;
5245
5246         DBG_871X( "[%s] peermac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
5247                     peermacaddr[0], peermacaddr[1],peermacaddr[2],peermacaddr[3],peermacaddr[4],peermacaddr[5]);
5248
5249         for( i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++ )
5250         {
5251                DBG_871X( "[%s] profileinfo_mac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
5252                             profileinfo->peermac[0], profileinfo->peermac[1],profileinfo->peermac[2],profileinfo->peermac[3],profileinfo->peermac[4],profileinfo->peermac[5]);
5253                 if ( _rtw_memcmp( peermacaddr, profileinfo->peermac, ETH_ALEN ) )
5254                 {
5255                         match_result = 1;
5256                         DBG_871X( "[%s] Match!\n", __FUNCTION__ );
5257                         break;
5258                 }
5259         }
5260
5261         return (match_result );
5262 }
5263
5264 void issue_probersp_p2p(_adapter *padapter, unsigned char *da)
5265 {
5266         struct xmit_frame                       *pmgntframe;
5267         struct pkt_attrib                       *pattrib;
5268         unsigned char                                   *pframe;
5269         struct rtw_ieee80211_hdr        *pwlanhdr;
5270         unsigned short                          *fctrl;
5271         unsigned char                                   *mac;
5272         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
5273         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5274         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5275         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
5276         //WLAN_BSSID_EX                 *cur_network = &(pmlmeinfo->network);
5277         u16                                     beacon_interval = 100;
5278         u16                                     capInfo = 0;
5279         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
5280         u8                                      wpsie[255] = { 0x00 };
5281         u32                                     wpsielen = 0, p2pielen = 0;
5282 #ifdef CONFIG_WFD
5283         u32                                     wfdielen = 0;
5284 #endif //CONFIG_WFD
5285 #ifdef CONFIG_INTEL_WIDI
5286         u8 zero_array_check[L2SDTA_SERVICE_VE_LEN] = { 0x00 };
5287 #endif //CONFIG_INTEL_WIDI
5288
5289         //DBG_871X("%s\n", __FUNCTION__);
5290
5291         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
5292         {
5293                 return;
5294         }
5295
5296         //update attribute
5297         pattrib = &pmgntframe->attrib;
5298         update_mgntframe_attrib(padapter, pattrib);
5299
5300         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5301
5302         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5303         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5304
5305         mac = adapter_mac_addr(padapter);
5306
5307         fctrl = &(pwlanhdr->frame_ctl);
5308         *(fctrl) = 0;
5309         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
5310         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
5311
5312         //      Use the device address for BSSID field.
5313         _rtw_memcpy(pwlanhdr->addr3, mac, ETH_ALEN);
5314
5315         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5316         pmlmeext->mgnt_seq++;
5317         SetFrameSubType(fctrl, WIFI_PROBERSP);
5318
5319         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
5320         pattrib->pktlen = pattrib->hdrlen;
5321         pframe += pattrib->hdrlen;
5322
5323         //timestamp will be inserted by hardware
5324         pframe += 8;
5325         pattrib->pktlen += 8;
5326
5327         // beacon interval: 2 bytes
5328         _rtw_memcpy(pframe, (unsigned char *) &beacon_interval, 2);
5329         pframe += 2;
5330         pattrib->pktlen += 2;
5331
5332         //      capability info: 2 bytes
5333         //      ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of WiFi Direct Spec)
5334         capInfo |= cap_ShortPremble;
5335         capInfo |= cap_ShortSlot;
5336
5337         _rtw_memcpy(pframe, (unsigned char *) &capInfo, 2);
5338         pframe += 2;
5339         pattrib->pktlen += 2;
5340
5341
5342         // SSID
5343         pframe = rtw_set_ie(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid, &pattrib->pktlen);
5344
5345         // supported rates...
5346         //      Use the OFDM rate in the P2P probe response frame. ( 6(B), 9(B), 12, 18, 24, 36, 48, 54 )
5347         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
5348
5349         // DS parameter set
5350         pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&pwdinfo->listen_channel, &pattrib->pktlen);
5351
5352 #ifdef CONFIG_IOCTL_CFG80211
5353         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
5354         {
5355                 if( pmlmepriv->wps_probe_resp_ie != NULL && pmlmepriv->p2p_probe_resp_ie != NULL )
5356                 {
5357                         //WPS IE
5358                         _rtw_memcpy(pframe, pmlmepriv->wps_probe_resp_ie, pmlmepriv->wps_probe_resp_ie_len);
5359                         pattrib->pktlen += pmlmepriv->wps_probe_resp_ie_len;
5360                         pframe += pmlmepriv->wps_probe_resp_ie_len;
5361
5362                         //P2P IE
5363                         _rtw_memcpy(pframe, pmlmepriv->p2p_probe_resp_ie, pmlmepriv->p2p_probe_resp_ie_len);
5364                         pattrib->pktlen += pmlmepriv->p2p_probe_resp_ie_len;
5365                         pframe += pmlmepriv->p2p_probe_resp_ie_len;
5366                 }
5367         }
5368         else
5369 #endif //CONFIG_IOCTL_CFG80211
5370         {
5371
5372                 //      Todo: WPS IE
5373                 //      Noted by Albert 20100907
5374                 //      According to the WPS specification, all the WPS attribute is presented by Big Endian.
5375
5376                 wpsielen = 0;
5377                 //      WPS OUI
5378                 *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
5379                 wpsielen += 4;
5380
5381                 //      WPS version
5382                 //      Type:
5383                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
5384                 wpsielen += 2;
5385
5386                 //      Length:
5387                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5388                 wpsielen += 2;
5389
5390                 //      Value:
5391                 wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
5392
5393 #ifdef CONFIG_INTEL_WIDI
5394                 //      Commented by Kurt
5395                 //      Appended WiDi info. only if we did issued_probereq_widi(), and then we saved ven. ext. in pmlmepriv->sa_ext.
5396                 if(  _rtw_memcmp(pmlmepriv->sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN) == _FALSE
5397                         || pmlmepriv->num_p2p_sdt != 0 )
5398                 {
5399                         //Sec dev type
5400                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SEC_DEV_TYPE_LIST );
5401                         wpsielen += 2;
5402
5403                         //      Length:
5404                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
5405                         wpsielen += 2;
5406
5407                         //      Value:
5408                         //      Category ID
5409                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_DISPLAYS );
5410                         wpsielen += 2;
5411
5412                         //      OUI
5413                         *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( INTEL_DEV_TYPE_OUI );
5414                         wpsielen += 4;
5415
5416                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_WIDI_CONSUMER_SINK );
5417                         wpsielen += 2;
5418
5419                         if(  _rtw_memcmp(pmlmepriv->sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN) == _FALSE )
5420                         {
5421                                 //      Vendor Extension
5422                                 _rtw_memcpy( wpsie + wpsielen, pmlmepriv->sa_ext, L2SDTA_SERVICE_VE_LEN );
5423                                 wpsielen += L2SDTA_SERVICE_VE_LEN;
5424                         }
5425                 }
5426 #endif //CONFIG_INTEL_WIDI
5427
5428                 //      WiFi Simple Config State
5429                 //      Type:
5430                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SIMPLE_CONF_STATE );
5431                 wpsielen += 2;
5432
5433                 //      Length:
5434                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5435                 wpsielen += 2;
5436
5437                 //      Value:
5438                 wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG;   //      Not Configured.
5439
5440                 //      Response Type
5441                 //      Type:
5442                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_RESP_TYPE );
5443                 wpsielen += 2;
5444
5445                 //      Length:
5446                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5447                 wpsielen += 2;
5448
5449                 //      Value:
5450                 wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
5451
5452                 //      UUID-E
5453                 //      Type:
5454                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_UUID_E );
5455                 wpsielen += 2;
5456
5457                 //      Length:
5458                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0010 );
5459                 wpsielen += 2;
5460
5461                 //      Value:
5462                 if (pwdinfo->external_uuid == 0) {
5463                         _rtw_memset( wpsie + wpsielen, 0x0, 16 );
5464                         _rtw_memcpy(wpsie + wpsielen, mac, ETH_ALEN);
5465                 } else {
5466                         _rtw_memcpy( wpsie + wpsielen, pwdinfo->uuid, 0x10 );
5467                 }
5468                 wpsielen += 0x10;
5469
5470                 //      Manufacturer
5471                 //      Type:
5472                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MANUFACTURER );
5473                 wpsielen += 2;
5474
5475                 //      Length:
5476                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0007 );
5477                 wpsielen += 2;
5478
5479                 //      Value:
5480                 _rtw_memcpy( wpsie + wpsielen, "Realtek", 7 );
5481                 wpsielen += 7;
5482
5483                 //      Model Name
5484                 //      Type:
5485                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MODEL_NAME );
5486                 wpsielen += 2;
5487
5488                 //      Length:
5489                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0006 );
5490                 wpsielen += 2;
5491
5492                 //      Value:
5493                 _rtw_memcpy( wpsie + wpsielen, "8192CU", 6 );
5494                 wpsielen += 6;
5495
5496                 //      Model Number
5497                 //      Type:
5498                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MODEL_NUMBER );
5499                 wpsielen += 2;
5500
5501                 //      Length:
5502                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5503                 wpsielen += 2;
5504
5505                 //      Value:
5506                 wpsie[ wpsielen++ ] = 0x31;             //      character 1
5507
5508                 //      Serial Number
5509                 //      Type:
5510                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SERIAL_NUMBER );
5511                 wpsielen += 2;
5512
5513                 //      Length:
5514                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( ETH_ALEN );
5515                 wpsielen += 2;
5516
5517                 //      Value:
5518                 _rtw_memcpy( wpsie + wpsielen, "123456" , ETH_ALEN );
5519                 wpsielen += ETH_ALEN;
5520
5521                 //      Primary Device Type
5522                 //      Type:
5523                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
5524                 wpsielen += 2;
5525
5526                 //      Length:
5527                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
5528                 wpsielen += 2;
5529
5530                 //      Value:
5531                 //      Category ID
5532                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
5533                 wpsielen += 2;
5534
5535                 //      OUI
5536                 *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( WPSOUI );
5537                 wpsielen += 4;
5538
5539                 //      Sub Category ID
5540                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
5541                 wpsielen += 2;
5542
5543                 //      Device Name
5544                 //      Type:
5545                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
5546                 wpsielen += 2;
5547
5548                 //      Length:
5549                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->device_name_len );
5550                 wpsielen += 2;
5551
5552                 //      Value:
5553                 _rtw_memcpy( wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len );
5554                 wpsielen += pwdinfo->device_name_len;
5555
5556                 //      Config Method
5557                 //      Type:
5558                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
5559                 wpsielen += 2;
5560
5561                 //      Length:
5562                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
5563                 wpsielen += 2;
5564
5565                 //      Value:
5566                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
5567                 wpsielen += 2;
5568
5569
5570                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
5571
5572
5573                 p2pielen = build_probe_resp_p2p_ie(pwdinfo, pframe);
5574                 pframe += p2pielen;
5575                 pattrib->pktlen += p2pielen;
5576         }
5577
5578 #ifdef CONFIG_WFD
5579 #ifdef CONFIG_IOCTL_CFG80211
5580         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
5581 #endif //CONFIG_IOCTL_CFG80211
5582         {
5583                 wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
5584                 pframe += wfdielen;
5585                 pattrib->pktlen += wfdielen;
5586         }
5587 #ifdef CONFIG_IOCTL_CFG80211
5588         else if (pmlmepriv->wfd_probe_resp_ie != NULL && pmlmepriv->wfd_probe_resp_ie_len>0)
5589         {
5590                 //WFD IE
5591                 _rtw_memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, pmlmepriv->wfd_probe_resp_ie_len);
5592                 pattrib->pktlen += pmlmepriv->wfd_probe_resp_ie_len;
5593                 pframe += pmlmepriv->wfd_probe_resp_ie_len;
5594         }
5595 #endif //CONFIG_IOCTL_CFG80211
5596 #endif //CONFIG_WFD
5597
5598         pattrib->last_txcmdsz = pattrib->pktlen;
5599
5600
5601         dump_mgntframe(padapter, pmgntframe);
5602
5603         return;
5604
5605 }
5606
5607 int _issue_probereq_p2p(_adapter *padapter, u8 *da, int wait_ack)
5608 {
5609         int ret = _FAIL;
5610         struct xmit_frame               *pmgntframe;
5611         struct pkt_attrib               *pattrib;
5612         unsigned char                   *pframe;
5613         struct rtw_ieee80211_hdr        *pwlanhdr;
5614         unsigned short          *fctrl;
5615         unsigned char                   *mac;
5616         unsigned char                   bssrate[NumRates];
5617         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
5618         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5619         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5620         int     bssrate_len = 0;
5621         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
5622         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
5623         u8                                      wpsie[255] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
5624         u16                                     wpsielen = 0, p2pielen = 0;
5625 #ifdef CONFIG_WFD
5626         u32                                     wfdielen = 0;
5627 #endif //CONFIG_WFD
5628
5629         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
5630
5631
5632         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
5633         {
5634                 goto exit;
5635         }
5636
5637         //update attribute
5638         pattrib = &pmgntframe->attrib;
5639         update_mgntframe_attrib(padapter, pattrib);
5640
5641
5642         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5643
5644         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5645         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5646
5647         mac = adapter_mac_addr(padapter);
5648
5649         fctrl = &(pwlanhdr->frame_ctl);
5650         *(fctrl) = 0;
5651
5652         if (da) {
5653                 _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
5654                 _rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN);
5655         } else {
5656                 if ( ( pwdinfo->p2p_info.scan_op_ch_only ) || ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) )
5657                 {
5658                         //      This two flags will be set when this is only the P2P client mode.
5659                         _rtw_memcpy(pwlanhdr->addr1, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
5660                         _rtw_memcpy(pwlanhdr->addr3, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
5661                 }
5662                 else
5663                 {
5664                         //      broadcast probe request frame
5665                         _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
5666                         _rtw_memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
5667                 }
5668         }
5669         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
5670
5671         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5672         pmlmeext->mgnt_seq++;
5673         SetFrameSubType(pframe, WIFI_PROBEREQ);
5674
5675         pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
5676         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
5677
5678         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
5679         {
5680                 pframe = rtw_set_ie(pframe, _SSID_IE_, pwdinfo->tx_prov_disc_info.ssid.SsidLength, pwdinfo->tx_prov_disc_info.ssid.Ssid, &(pattrib->pktlen));
5681         }
5682         else
5683         {
5684                 pframe = rtw_set_ie(pframe, _SSID_IE_, P2P_WILDCARD_SSID_LEN, pwdinfo->p2p_wildcard_ssid, &(pattrib->pktlen));
5685         }
5686         //      Use the OFDM rate in the P2P probe request frame. ( 6(B), 9(B), 12(B), 24(B), 36, 48, 54 )
5687         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
5688
5689 #ifdef CONFIG_IOCTL_CFG80211
5690         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
5691         {
5692                 if( pmlmepriv->wps_probe_req_ie != NULL && pmlmepriv->p2p_probe_req_ie != NULL )
5693                 {
5694                         //WPS IE
5695                         _rtw_memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
5696                         pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
5697                         pframe += pmlmepriv->wps_probe_req_ie_len;
5698
5699                         //P2P IE
5700                         _rtw_memcpy(pframe, pmlmepriv->p2p_probe_req_ie, pmlmepriv->p2p_probe_req_ie_len);
5701                         pattrib->pktlen += pmlmepriv->p2p_probe_req_ie_len;
5702                         pframe += pmlmepriv->p2p_probe_req_ie_len;
5703                 }
5704         }
5705         else
5706 #endif //CONFIG_IOCTL_CFG80211
5707         {
5708
5709                 //      WPS IE
5710                 //      Noted by Albert 20110221
5711                 //      According to the WPS specification, all the WPS attribute is presented by Big Endian.
5712
5713                 wpsielen = 0;
5714                 //      WPS OUI
5715                 *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
5716                 wpsielen += 4;
5717
5718                 //      WPS version
5719                 //      Type:
5720                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
5721                 wpsielen += 2;
5722
5723                 //      Length:
5724                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5725                 wpsielen += 2;
5726
5727                 //      Value:
5728                 wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
5729
5730                 if( pmlmepriv->wps_probe_req_ie == NULL )
5731                 {
5732                         //      UUID-E
5733                         //      Type:
5734                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_UUID_E );
5735                         wpsielen += 2;
5736
5737                         //      Length:
5738                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0010 );
5739                         wpsielen += 2;
5740
5741                         //      Value:
5742                         if (pwdinfo->external_uuid == 0) {
5743                                 _rtw_memset( wpsie + wpsielen, 0x0, 16 );
5744                                 _rtw_memcpy(wpsie + wpsielen, mac, ETH_ALEN);
5745                         } else {
5746                                 _rtw_memcpy( wpsie + wpsielen, pwdinfo->uuid, 0x10 );
5747                         }
5748                         wpsielen += 0x10;
5749
5750                         //      Config Method
5751                         //      Type:
5752                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
5753                         wpsielen += 2;
5754
5755                         //      Length:
5756                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
5757                         wpsielen += 2;
5758
5759                         //      Value:
5760                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
5761                         wpsielen += 2;
5762                 }
5763
5764                 //      Device Name
5765                 //      Type:
5766                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
5767                 wpsielen += 2;
5768
5769                 //      Length:
5770                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->device_name_len );
5771                 wpsielen += 2;
5772
5773                 //      Value:
5774                 _rtw_memcpy( wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len );
5775                 wpsielen += pwdinfo->device_name_len;
5776
5777                 //      Primary Device Type
5778                 //      Type:
5779                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
5780                 wpsielen += 2;
5781
5782                 //      Length:
5783                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
5784                 wpsielen += 2;
5785
5786                 //      Value:
5787                 //      Category ID
5788                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_RTK_WIDI );
5789                 wpsielen += 2;
5790
5791                 //      OUI
5792                 *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( WPSOUI );
5793                 wpsielen += 4;
5794
5795                 //      Sub Category ID
5796                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_RTK_DMP );
5797                 wpsielen += 2;
5798
5799                 //      Device Password ID
5800                 //      Type:
5801                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
5802                 wpsielen += 2;
5803
5804                 //      Length:
5805                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
5806                 wpsielen += 2;
5807
5808                 //      Value:
5809                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );  //      Registrar-specified
5810                 wpsielen += 2;
5811
5812                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
5813
5814                 //      P2P OUI
5815                 p2pielen = 0;
5816                 p2pie[ p2pielen++ ] = 0x50;
5817                 p2pie[ p2pielen++ ] = 0x6F;
5818                 p2pie[ p2pielen++ ] = 0x9A;
5819                 p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
5820
5821                 //      Commented by Albert 20110221
5822                 //      According to the P2P Specification, the probe request frame should contain 5 P2P attributes
5823                 //      1. P2P Capability
5824                 //      2. P2P Device ID if this probe request wants to find the specific P2P device
5825                 //      3. Listen Channel
5826                 //      4. Extended Listen Timing
5827                 //      5. Operating Channel if this WiFi is working as the group owner now
5828
5829                 //      P2P Capability
5830                 //      Type:
5831                 p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
5832
5833                 //      Length:
5834                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
5835                 p2pielen += 2;
5836
5837                 //      Value:
5838                 //      Device Capability Bitmap, 1 byte
5839                 p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
5840
5841                 //      Group Capability Bitmap, 1 byte
5842                 if ( pwdinfo->persistent_supported )
5843                         p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
5844                 else
5845                         p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
5846
5847                 //      Listen Channel
5848                 //      Type:
5849                 p2pie[ p2pielen++ ] = P2P_ATTR_LISTEN_CH;
5850
5851                 //      Length:
5852                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
5853                 p2pielen += 2;
5854
5855                 //      Value:
5856                 //      Country String
5857                 p2pie[ p2pielen++ ] = 'X';
5858                 p2pie[ p2pielen++ ] = 'X';
5859
5860                 //      The third byte should be set to 0x04.
5861                 //      Described in the "Operating Channel Attribute" section.
5862                 p2pie[ p2pielen++ ] = 0x04;
5863
5864                 //      Operating Class
5865                 p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
5866
5867                 //      Channel Number
5868                 p2pie[ p2pielen++ ] = pwdinfo->listen_channel;  //      listen channel
5869
5870
5871                 //      Extended Listen Timing
5872                 //      Type:
5873                 p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
5874
5875                 //      Length:
5876                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
5877                 p2pielen += 2;
5878
5879                 //      Value:
5880                 //      Availability Period
5881                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
5882                 p2pielen += 2;
5883
5884                 //      Availability Interval
5885                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
5886                 p2pielen += 2;
5887
5888                 if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
5889                 {
5890                         //      Operating Channel (if this WiFi is working as the group owner now)
5891                         //      Type:
5892                         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
5893
5894                         //      Length:
5895                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
5896                         p2pielen += 2;
5897
5898                         //      Value:
5899                         //      Country String
5900                         p2pie[ p2pielen++ ] = 'X';
5901                         p2pie[ p2pielen++ ] = 'X';
5902
5903                         //      The third byte should be set to 0x04.
5904                         //      Described in the "Operating Channel Attribute" section.
5905                         p2pie[ p2pielen++ ] = 0x04;
5906
5907                         //      Operating Class
5908                         p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
5909
5910                         //      Channel Number
5911                         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
5912
5913                 }
5914
5915                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
5916
5917         }
5918
5919 #ifdef CONFIG_WFD
5920 #ifdef CONFIG_IOCTL_CFG80211
5921         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
5922 #endif
5923         {
5924                 wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe);
5925                 pframe += wfdielen;
5926                 pattrib->pktlen += wfdielen;
5927         }
5928 #ifdef CONFIG_IOCTL_CFG80211
5929         else if (pmlmepriv->wfd_probe_req_ie != NULL && pmlmepriv->wfd_probe_req_ie_len>0)
5930         {
5931                 //WFD IE
5932                 _rtw_memcpy(pframe, pmlmepriv->wfd_probe_req_ie, pmlmepriv->wfd_probe_req_ie_len);
5933                 pattrib->pktlen += pmlmepriv->wfd_probe_req_ie_len;
5934                 pframe += pmlmepriv->wfd_probe_req_ie_len;
5935         }
5936 #endif //CONFIG_IOCTL_CFG80211
5937 #endif //CONFIG_WFD
5938
5939         pattrib->last_txcmdsz = pattrib->pktlen;
5940
5941         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
5942
5943         if (wait_ack) {
5944                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
5945         } else {
5946                 dump_mgntframe(padapter, pmgntframe);
5947                 ret = _SUCCESS;
5948         }
5949
5950 exit:
5951         return ret;
5952 }
5953
5954 inline void issue_probereq_p2p(_adapter *adapter, u8 *da)
5955 {
5956         _issue_probereq_p2p(adapter, da, _FALSE);
5957 }
5958
5959 int issue_probereq_p2p_ex(_adapter *adapter, u8 *da, int try_cnt, int wait_ms)
5960 {
5961         int ret;
5962         int i = 0;
5963         u32 start = rtw_get_current_time();
5964
5965         do
5966         {
5967                 ret = _issue_probereq_p2p(adapter, da, wait_ms>0?_TRUE:_FALSE);
5968
5969                 i++;
5970
5971                 if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
5972                         break;
5973
5974                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
5975                         rtw_msleep_os(wait_ms);
5976
5977         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
5978
5979         if (ret != _FAIL) {
5980                 ret = _SUCCESS;
5981                 #ifndef DBG_XMIT_ACK
5982                 goto exit;
5983                 #endif
5984         }
5985
5986         if (try_cnt && wait_ms) {
5987                 if (da)
5988                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
5989                                 FUNC_ADPT_ARG(adapter), MAC_ARG(da), rtw_get_oper_ch(adapter),
5990                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
5991                 else
5992                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
5993                                 FUNC_ADPT_ARG(adapter), rtw_get_oper_ch(adapter),
5994                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
5995         }
5996 exit:
5997         return ret;
5998 }
5999
6000 #endif //CONFIG_P2P
6001
6002 s32 rtw_action_public_decache(union recv_frame *recv_frame, s32 token)
6003 {
6004         _adapter *adapter = recv_frame->u.hdr.adapter;
6005         struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
6006         u8 *frame = recv_frame->u.hdr.rx_data;
6007         u16 seq_ctrl = ( (recv_frame->u.hdr.attrib.seq_num&0xffff) << 4) |
6008                 (recv_frame->u.hdr.attrib.frag_num & 0xf);
6009
6010         if (GetRetry(frame)) {
6011                 if (token >= 0) {
6012                         if ((seq_ctrl == mlmeext->action_public_rxseq)
6013                                 && (token == mlmeext->action_public_dialog_token))
6014                         {
6015                                 DBG_871X(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x, token:%d\n",
6016                                         FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
6017                                 return _FAIL;
6018                         }
6019                 } else {
6020                         if (seq_ctrl == mlmeext->action_public_rxseq) {
6021                                 DBG_871X(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x\n",
6022                                         FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq);
6023                                 return _FAIL;
6024                         }
6025                 }
6026         }
6027
6028         mlmeext->action_public_rxseq = seq_ctrl;
6029
6030         if (token >= 0)
6031                 mlmeext->action_public_dialog_token = token;
6032
6033         return _SUCCESS;
6034 }
6035
6036 unsigned int on_action_public_p2p(union recv_frame *precv_frame)
6037 {
6038         _adapter *padapter = precv_frame->u.hdr.adapter;
6039         u8 *pframe = precv_frame->u.hdr.rx_data;
6040         uint len = precv_frame->u.hdr.len;
6041         u8 *frame_body;
6042         u8 dialogToken=0;
6043 #ifdef CONFIG_P2P
6044         u8 *p2p_ie;
6045         u32     p2p_ielen, wps_ielen;
6046         struct  wifidirect_info *pwdinfo = &( padapter->wdinfo );
6047         u8      result = P2P_STATUS_SUCCESS;
6048         u8      empty_addr[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
6049         u8 *merged_p2pie = NULL;
6050         u32 merged_p2p_ielen= 0;
6051 #endif //CONFIG_P2P
6052
6053         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
6054
6055         dialogToken = frame_body[7];
6056
6057         if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
6058                 return _FAIL;
6059
6060 #ifdef CONFIG_P2P
6061         _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
6062 #ifdef CONFIG_IOCTL_CFG80211
6063         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211)
6064         {
6065                 rtw_cfg80211_rx_p2p_action_public(padapter, pframe, len);
6066         }
6067         else
6068 #endif //CONFIG_IOCTL_CFG80211
6069         {
6070                 //      Do nothing if the driver doesn't enable the P2P function.
6071                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
6072                         return _SUCCESS;
6073
6074                 len -= sizeof(struct rtw_ieee80211_hdr_3addr);
6075
6076                 switch( frame_body[ 6 ] )//OUI Subtype
6077                 {
6078                         case P2P_GO_NEGO_REQ:
6079                         {
6080                                 DBG_871X( "[%s] Got GO Nego Req Frame\n", __FUNCTION__);
6081                                 _rtw_memset( &pwdinfo->groupid_info, 0x00, sizeof( struct group_id_info ) );
6082
6083                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
6084                                 {
6085                                         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
6086                                 }
6087
6088                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
6089                                 {
6090                                         //      Commented by Albert 20110526
6091                                         //      In this case, this means the previous nego fail doesn't be reset yet.
6092                                         _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
6093                                         //      Restore the previous p2p state
6094                                         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
6095                                         DBG_871X( "[%s] Restore the previous p2p state to %d\n", __FUNCTION__, rtw_p2p_state(pwdinfo) );
6096                                 }
6097 #ifdef CONFIG_CONCURRENT_MODE
6098                                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
6099                                 {
6100                                         _cancel_timer_ex( &pwdinfo->ap_p2p_switch_timer );
6101                                 }
6102 #endif // CONFIG_CONCURRENT_MODE
6103
6104                                 //      Commented by Kurt 20110902
6105                                 //Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered.
6106                                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
6107                                         rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
6108
6109                                 //      Commented by Kurt 20120113
6110                                 //      Get peer_dev_addr here if peer doesn't issue prov_disc frame.
6111                                 if( _rtw_memcmp(pwdinfo->rx_prov_disc_info.peerDevAddr, empty_addr, ETH_ALEN) )
6112                                         _rtw_memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
6113
6114                                 result = process_p2p_group_negotation_req( pwdinfo, frame_body, len );
6115                                 issue_p2p_GO_response( padapter, GetAddr2Ptr(pframe), frame_body, len, result );
6116 #ifdef CONFIG_INTEL_WIDI
6117                                 if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) {
6118                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
6119                                         _cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
6120                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
6121                                 }
6122 #endif //CONFIG_INTEL_WIDI
6123
6124                                 //      Commented by Albert 20110718
6125                                 //      No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer.
6126 #ifdef CONFIG_CONCURRENT_MODE
6127                                 //      Commented by Albert 20120107
6128                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
6129 #else // CONFIG_CONCURRENT_MODE
6130                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
6131 #endif // CONFIG_CONCURRENT_MODE
6132                                 break;
6133                         }
6134                         case P2P_GO_NEGO_RESP:
6135                         {
6136                                 DBG_871X( "[%s] Got GO Nego Resp Frame\n", __FUNCTION__);
6137
6138                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
6139                                 {
6140                                         //      Commented by Albert 20110425
6141                                         //      The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function.
6142                                         _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
6143                                         pwdinfo->nego_req_info.benable = _FALSE;
6144                                         result = process_p2p_group_negotation_resp( pwdinfo, frame_body, len);
6145                                         issue_p2p_GO_confirm( pwdinfo->padapter, GetAddr2Ptr(pframe), result);
6146                                         if ( P2P_STATUS_SUCCESS == result )
6147                                         {
6148                                                 if ( rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT )
6149                                                 {
6150                                                         pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
6151                                                         #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
6152                                                         pwdinfo->p2p_info.operation_ch[ 1 ] = 1;        //Check whether GO is operating in channel 1;
6153                                                         pwdinfo->p2p_info.operation_ch[ 2 ] = 6;        //Check whether GO is operating in channel 6;
6154                                                         pwdinfo->p2p_info.operation_ch[ 3 ] = 11;       //Check whether GO is operating in channel 11;
6155                                                         #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
6156                                                         pwdinfo->p2p_info.scan_op_ch_only = 1;
6157                                                         _set_timer( &pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH );
6158                                                 }
6159                                         }
6160
6161                                         //      Reset the dialog token for group negotiation frames.
6162                                         pwdinfo->negotiation_dialog_token = 1;
6163
6164                                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
6165                                         {
6166                                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
6167                                         }
6168                                 }
6169                                 else
6170                                 {
6171                                         DBG_871X( "[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __FUNCTION__);
6172                                 }
6173
6174                                 break;
6175                         }
6176                         case P2P_GO_NEGO_CONF:
6177                         {
6178                                 DBG_871X( "[%s] Got GO Nego Confirm Frame\n", __FUNCTION__);
6179                                 result = process_p2p_group_negotation_confirm( pwdinfo, frame_body, len);
6180                                 if ( P2P_STATUS_SUCCESS == result )
6181                                 {
6182                                         if ( rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT )
6183                                         {
6184                                                 pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
6185                                                 #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
6186                                                 pwdinfo->p2p_info.operation_ch[ 1 ] = 1;        //Check whether GO is operating in channel 1;
6187                                                 pwdinfo->p2p_info.operation_ch[ 2 ] = 6;        //Check whether GO is operating in channel 6;
6188                                                 pwdinfo->p2p_info.operation_ch[ 3 ] = 11;       //Check whether GO is operating in channel 11;
6189                                                 #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
6190                                                 pwdinfo->p2p_info.scan_op_ch_only = 1;
6191                                                 _set_timer( &pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH );
6192                                         }
6193                                 }
6194                                 break;
6195                         }
6196                         case P2P_INVIT_REQ:
6197                         {
6198                                 //      Added by Albert 2010/10/05
6199                                 //      Received the P2P Invite Request frame.
6200
6201                                 DBG_871X( "[%s] Got invite request frame!\n", __FUNCTION__ );
6202                                 if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
6203                                 {
6204                                         //      Parse the necessary information from the P2P Invitation Request frame.
6205                                         //      For example: The MAC address of sending this P2P Invitation Request frame.
6206                                         u32     attr_contentlen = 0;
6207                                         u8      status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6208                                         struct group_id_info group_id;
6209                                         u8      invitation_flag = 0;
6210                                         int j=0;
6211
6212                                         merged_p2p_ielen = rtw_get_p2p_merged_ies_len(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_);
6213
6214                                         merged_p2pie = rtw_zmalloc(merged_p2p_ielen + 2);       // 2 is for EID and Length
6215                                         if (merged_p2pie == NULL)
6216                                         {
6217                                                 DBG_871X( "[%s] Malloc p2p ie fail\n", __FUNCTION__);
6218                                                 goto exit;
6219                                         }
6220                                         _rtw_memset(merged_p2pie, 0x00, merged_p2p_ielen);
6221
6222                                         merged_p2p_ielen = rtw_p2p_merge_ies(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, merged_p2pie);
6223
6224                                         rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen);
6225                                         if ( attr_contentlen )
6226                                         {
6227
6228                                                 rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen);
6229                                                 //      Commented by Albert 20120510
6230                                                 //      Copy to the pwdinfo->p2p_peer_interface_addr.
6231                                                 //      So that the WFD UI ( or Sigma ) can get the peer interface address by using the following command.
6232                                                 //      #> iwpriv wlan0 p2p_get peer_ifa
6233                                                 //      After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant.
6234
6235                                                 if ( attr_contentlen )
6236                                                 {
6237                                                         DBG_871X( "[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
6238                                                                         pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1],
6239                                                                         pwdinfo->p2p_peer_interface_addr[2], pwdinfo->p2p_peer_interface_addr[3],
6240                                                                         pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5] );
6241                                                 }
6242
6243                                                 if ( invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT )
6244                                                 {
6245                                                         //      Re-invoke the persistent group.
6246
6247                                                         _rtw_memset( &group_id, 0x00, sizeof( struct group_id_info ) );
6248                                                         rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_ID, ( u8* ) &group_id, &attr_contentlen);
6249                                                         if ( attr_contentlen )
6250                                                         {
6251                                                                 if (_rtw_memcmp(group_id.go_device_addr, adapter_mac_addr(padapter), ETH_ALEN))
6252                                                                 {
6253                                                                         //      The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO.
6254                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO );
6255                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_GO );
6256                                                                         status_code = P2P_STATUS_SUCCESS;
6257                                                                 }
6258                                                                 else
6259                                                                 {
6260                                                                         //      The p2p device sending this p2p invitation request wants to be the persistent GO.
6261                                                                         if ( is_matched_in_profilelist( pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[ 0 ] ) )
6262                                                                         {
6263                                                                                 u8 operatingch_info[5] = { 0x00 };
6264                                                                                 if ( rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen) )
6265                                                                                 {
6266                                                                                         if( rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, (u32)operatingch_info[4] ) >= 0 )
6267                                                                                         {
6268                                                                                                 //      The operating channel is acceptable for this device.
6269                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[0]= operatingch_info[4];
6270                                                                                                 #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
6271                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[1]= 1;          //Check whether GO is operating in channel 1;
6272                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[2]= 6;          //Check whether GO is operating in channel 6;
6273                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[3]= 11;         //Check whether GO is operating in channel 11;
6274                                                                                                 #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
6275                                                                                                 pwdinfo->rx_invitereq_info.scan_op_ch_only = 1;
6276                                                                                                 _set_timer( &pwdinfo->reset_ch_sitesurvey, P2P_RESET_SCAN_CH );
6277                                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH );
6278                                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
6279                                                                                                 status_code = P2P_STATUS_SUCCESS;
6280                                                                                         }
6281                                                                                         else
6282                                                                                         {
6283                                                                                                 //      The operating channel isn't supported by this device.
6284                                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
6285                                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_DEVICE );
6286                                                                                                 status_code = P2P_STATUS_FAIL_NO_COMMON_CH;
6287                                                                                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
6288                                                                                         }
6289                                                                                 }
6290                                                                                 else
6291                                                                                 {
6292                                                                                         //      Commented by Albert 20121130
6293                                                                                         //      Intel will use the different P2P IE to store the operating channel information
6294                                                                                         //      Workaround for Intel WiDi 3.5
6295                                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH );
6296                                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
6297                                                                                         status_code = P2P_STATUS_SUCCESS;
6298                                                                                 }
6299                                                                         }
6300                                                                         else
6301                                                                         {
6302                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
6303                                                                                 #ifdef CONFIG_INTEL_WIDI
6304                                                                                 _rtw_memcpy( pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN );
6305                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
6306                                                                                 #endif //CONFIG_INTEL_WIDI
6307
6308                                                                                 status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
6309                                                                         }
6310                                                                 }
6311                                                         }
6312                                                         else
6313                                                         {
6314                                                                 DBG_871X( "[%s] P2P Group ID Attribute NOT FOUND!\n", __FUNCTION__ );
6315                                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6316                                                         }
6317                                                 }
6318                                                 else
6319                                                 {
6320                                                         //      Received the invitation to join a P2P group.
6321
6322                                                         _rtw_memset( &group_id, 0x00, sizeof( struct group_id_info ) );
6323                                                         rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_ID, ( u8* ) &group_id, &attr_contentlen);
6324                                                         if ( attr_contentlen )
6325                                                         {
6326                                                                 if (_rtw_memcmp(group_id.go_device_addr, adapter_mac_addr(padapter), ETH_ALEN))
6327                                                                 {
6328                                                                         //      In this case, the GO can't be myself.
6329                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
6330                                                                         status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6331                                                                 }
6332                                                                 else
6333                                                                 {
6334                                                                         //      The p2p device sending this p2p invitation request wants to join an existing P2P group
6335                                                                         //      Commented by Albert 2012/06/28
6336                                                                         //      In this case, this Wi-Fi device should use the iwpriv command to get the peer device address.
6337                                                                         //      The peer device address should be the destination address for the provisioning discovery request.
6338                                                                         //      Then, this Wi-Fi device should use the iwpriv command to get the peer interface address.
6339                                                                         //      The peer interface address should be the address for WPS mac address
6340                                                                         _rtw_memcpy( pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN );
6341                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
6342                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN );
6343                                                                         status_code = P2P_STATUS_SUCCESS;
6344                                                                 }
6345                                                         }
6346                                                         else
6347                                                         {
6348                                                                 DBG_871X( "[%s] P2P Group ID Attribute NOT FOUND!\n", __FUNCTION__ );
6349                                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6350                                                         }
6351                                                 }
6352                                         }
6353                                         else
6354                                         {
6355                                                 DBG_871X( "[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __FUNCTION__ );
6356                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6357                                         }
6358
6359                                         DBG_871X( "[%s] status_code = %d\n", __FUNCTION__, status_code );
6360
6361                                         pwdinfo->inviteresp_info.token = frame_body[ 7 ];
6362                                         issue_p2p_invitation_response( padapter, GetAddr2Ptr(pframe), pwdinfo->inviteresp_info.token, status_code );
6363                                         _set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
6364                                 }
6365 #ifdef CONFIG_INTEL_WIDI
6366                                 if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) {
6367                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
6368                                         _cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
6369                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
6370                                 }
6371 #endif //CONFIG_INTEL_WIDI
6372                                 break;
6373                         }
6374                         case P2P_INVIT_RESP:
6375                         {
6376                                 u8      attr_content = 0x00;
6377                                 u32     attr_contentlen = 0;
6378
6379                                 DBG_871X( "[%s] Got invite response frame!\n", __FUNCTION__ );
6380                                 _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
6381                                 if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
6382                                 {
6383                                         rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
6384
6385                                         if ( attr_contentlen == 1 )
6386                                         {
6387                                                 DBG_871X( "[%s] Status = %d\n", __FUNCTION__, attr_content );
6388                                                 pwdinfo->invitereq_info.benable = _FALSE;
6389
6390                                                 if ( attr_content == P2P_STATUS_SUCCESS )
6391                                                 {
6392                                                         if (_rtw_memcmp(pwdinfo->invitereq_info.go_bssid, adapter_mac_addr(padapter), ETH_ALEN))
6393                                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO );
6394                                                         else
6395                                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
6396
6397                                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_OK );
6398                                                 }
6399                                                 else
6400                                                 {
6401                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
6402                                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
6403                                                 }
6404                                         }
6405                                         else
6406                                         {
6407                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
6408                                                 rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
6409                                         }
6410                                 }
6411                                 else
6412                                 {
6413                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
6414                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
6415                                 }
6416
6417                                 if ( rtw_p2p_chk_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL ) )
6418                                 {
6419                                         _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
6420                                 }
6421                                 break;
6422                         }
6423                         case P2P_DEVDISC_REQ:
6424
6425                                 process_p2p_devdisc_req(pwdinfo, pframe, len);
6426
6427                                 break;
6428
6429                         case P2P_DEVDISC_RESP:
6430
6431                                 process_p2p_devdisc_resp(pwdinfo, pframe, len);
6432
6433                                 break;
6434
6435                         case P2P_PROVISION_DISC_REQ:
6436                                 DBG_871X( "[%s] Got Provisioning Discovery Request Frame\n", __FUNCTION__ );
6437                                 process_p2p_provdisc_req(pwdinfo, pframe, len);
6438                                 _rtw_memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
6439
6440                                 //20110902 Kurt
6441                                 //Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered.
6442                                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
6443                                         rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
6444
6445                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ);
6446                                 _set_timer( &pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT );
6447 #ifdef CONFIG_INTEL_WIDI
6448                                 if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) {
6449                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
6450                                         _cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
6451                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
6452                                 }
6453 #endif //CONFIG_INTEL_WIDI
6454                                 break;
6455
6456                         case P2P_PROVISION_DISC_RESP:
6457                                 //      Commented by Albert 20110707
6458                                 //      Should we check the pwdinfo->tx_prov_disc_info.bsent flag here??
6459                                 DBG_871X( "[%s] Got Provisioning Discovery Response Frame\n", __FUNCTION__ );
6460                                 //      Commented by Albert 20110426
6461                                 //      The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function.
6462                                 _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
6463                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP);
6464                                 process_p2p_provdisc_resp(pwdinfo, pframe);
6465                                 _set_timer( &pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT );
6466                                 break;
6467
6468                 }
6469         }
6470
6471
6472 exit:
6473
6474         if(merged_p2pie)
6475         {
6476                 rtw_mfree(merged_p2pie, merged_p2p_ielen + 2);
6477         }
6478 #endif //CONFIG_P2P
6479         return _SUCCESS;
6480 }
6481
6482 unsigned int on_action_public_vendor(union recv_frame *precv_frame)
6483 {
6484         unsigned int ret = _FAIL;
6485         u8 *pframe = precv_frame->u.hdr.rx_data;
6486         uint frame_len = precv_frame->u.hdr.len;
6487         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6488
6489         if (_rtw_memcmp(frame_body + 2, P2P_OUI, 4) == _TRUE) {
6490                 ret = on_action_public_p2p(precv_frame);
6491         }
6492
6493         return ret;
6494 }
6495
6496 unsigned int on_action_public_default(union recv_frame *precv_frame, u8 action)
6497 {
6498         unsigned int ret = _FAIL;
6499         u8 *pframe = precv_frame->u.hdr.rx_data;
6500         uint frame_len = precv_frame->u.hdr.len;
6501         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6502         u8 token;
6503         _adapter *adapter = precv_frame->u.hdr.adapter;
6504         int cnt = 0;
6505         char msg[64];
6506
6507         token = frame_body[2];
6508
6509         if (rtw_action_public_decache(precv_frame, token) == _FAIL)
6510                 goto exit;
6511
6512         #ifdef CONFIG_IOCTL_CFG80211
6513         cnt += sprintf((msg+cnt), "%s(token:%u)", action_public_str(action), token);
6514         rtw_cfg80211_rx_action(adapter, pframe, frame_len, msg);
6515         #endif
6516
6517         ret = _SUCCESS;
6518
6519 exit:
6520         return ret;
6521 }
6522
6523 unsigned int on_action_public(_adapter *padapter, union recv_frame *precv_frame)
6524 {
6525         unsigned int ret = _FAIL;
6526         u8 *pframe = precv_frame->u.hdr.rx_data;
6527         uint frame_len = precv_frame->u.hdr.len;
6528         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6529         u8 category, action;
6530
6531         /* check RA matches or not */
6532         if (!_rtw_memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN))
6533                 goto exit;
6534
6535         category = frame_body[0];
6536         if(category != RTW_WLAN_CATEGORY_PUBLIC)
6537                 goto exit;
6538
6539         action = frame_body[1];
6540         switch (action) {
6541         case ACT_PUBLIC_VENDOR:
6542                 ret = on_action_public_vendor(precv_frame);
6543                 break;
6544         default:
6545                 ret = on_action_public_default(precv_frame, action);
6546                 break;
6547         }
6548
6549 exit:
6550         return ret;
6551 }
6552
6553 unsigned int OnAction_ht(_adapter *padapter, union recv_frame *precv_frame)
6554 {
6555         u8 *pframe = precv_frame->u.hdr.rx_data;
6556         uint frame_len = precv_frame->u.hdr.len;
6557         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6558         u8 category, action;
6559
6560         /* check RA matches or not */
6561         if (!_rtw_memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN))
6562                 goto exit;
6563
6564         category = frame_body[0];
6565         if(category != RTW_WLAN_CATEGORY_HT)
6566                 goto exit;
6567
6568         action = frame_body[1];
6569         switch (action) {
6570         case RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING:
6571 #ifdef CONFIG_BEAMFORMING
6572                 //DBG_871X("RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING\n");
6573                 beamforming_get_report_frame(padapter, precv_frame);
6574 #endif //CONFIG_BEAMFORMING
6575                 break;
6576         default:
6577                 break;
6578         }
6579
6580 exit:
6581
6582         return _SUCCESS;
6583 }
6584
6585 #ifdef CONFIG_IEEE80211W
6586 unsigned int OnAction_sa_query(_adapter *padapter, union recv_frame *precv_frame)
6587 {
6588         u8 *pframe = precv_frame->u.hdr.rx_data;
6589         struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
6590         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6591         unsigned short tid;
6592         //Baron
6593
6594         DBG_871X("OnAction_sa_query\n");
6595
6596         switch (pframe[WLAN_HDR_A3_LEN+1])
6597         {
6598                 case 0: //SA Query req
6599                         _rtw_memcpy(&tid, &pframe[WLAN_HDR_A3_LEN+2], sizeof(unsigned short));
6600                         DBG_871X("OnAction_sa_query request,action=%d, tid=%04x\n", pframe[WLAN_HDR_A3_LEN+1], tid);
6601                         issue_action_SA_Query(padapter, GetAddr2Ptr(pframe), 1, tid);
6602                         break;
6603
6604                 case 1: //SA Query rsp
6605                         _cancel_timer_ex(&pmlmeext->sa_query_timer);
6606                         DBG_871X("OnAction_sa_query response,action=%d, tid=%04x, cancel timer\n", pframe[WLAN_HDR_A3_LEN+1], pframe[WLAN_HDR_A3_LEN+2]);
6607                         break;
6608                 default:
6609                         break;
6610         }
6611         if(0)
6612         {
6613                 int pp;
6614                 printk("pattrib->pktlen = %d =>", pattrib->pkt_len);
6615                 for(pp=0;pp< pattrib->pkt_len; pp++)
6616                         printk(" %02x ", pframe[pp]);
6617                 printk("\n");
6618         }
6619
6620         return _SUCCESS;
6621 }
6622 #endif //CONFIG_IEEE80211W
6623
6624 unsigned int OnAction_wmm(_adapter *padapter, union recv_frame *precv_frame)
6625 {
6626         return _SUCCESS;
6627 }
6628
6629 unsigned int OnAction_vht(_adapter *padapter, union recv_frame *precv_frame)
6630 {
6631 #ifdef CONFIG_80211AC_VHT
6632         struct rx_pkt_attrib *prxattrib = &precv_frame->u.hdr.attrib;
6633         u8 *pframe = precv_frame->u.hdr.rx_data;
6634         uint frame_len = precv_frame->u.hdr.len;
6635         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6636         u8 category, action;
6637         struct sta_info *psta = NULL;
6638
6639         /* check RA matches or not */
6640         if (!_rtw_memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN))
6641                 goto exit;
6642
6643         category = frame_body[0];
6644         if(category != RTW_WLAN_CATEGORY_VHT)
6645                 goto exit;
6646
6647         action = frame_body[1];
6648         switch (action) {
6649         case RTW_WLAN_ACTION_VHT_COMPRESSED_BEAMFORMING:
6650 #ifdef CONFIG_BEAMFORMING
6651                 //DBG_871X("RTW_WLAN_ACTION_VHT_COMPRESSED_BEAMFORMING\n");
6652                 beamforming_get_report_frame(padapter, precv_frame);
6653 #endif //CONFIG_BEAMFORMING
6654                 break;
6655         case RTW_WLAN_ACTION_VHT_OPMODE_NOTIFICATION:
6656                 // CategoryCode(1) + ActionCode(1) + OpModeNotification(1)
6657                 //DBG_871X("RTW_WLAN_ACTION_VHT_OPMODE_NOTIFICATION\n");
6658                 psta = rtw_get_stainfo(&padapter->stapriv, prxattrib->ta);
6659                 if (psta)
6660                         rtw_process_vht_op_mode_notify(padapter, &frame_body[2], psta);
6661                 break;
6662         default:
6663                 break;
6664         }
6665
6666 exit:
6667 #endif //CONFIG_80211AC_VHT
6668
6669         return _SUCCESS;
6670 }
6671
6672 unsigned int OnAction_p2p(_adapter *padapter, union recv_frame *precv_frame)
6673 {
6674 #ifdef CONFIG_P2P
6675         u8 *frame_body;
6676         u8 category, OUI_Subtype, dialogToken=0;
6677         u8 *pframe = precv_frame->u.hdr.rx_data;
6678         uint len = precv_frame->u.hdr.len;
6679         struct  wifidirect_info *pwdinfo = &( padapter->wdinfo );
6680
6681         //check RA matches or not
6682         if (!_rtw_memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN))
6683                 return _SUCCESS;
6684
6685         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
6686
6687         category = frame_body[0];
6688         if(category != RTW_WLAN_CATEGORY_P2P)
6689                 return _SUCCESS;
6690
6691         if ( cpu_to_be32( *( ( u32* ) ( frame_body + 1 ) ) ) != P2POUI )
6692                 return _SUCCESS;
6693
6694 #ifdef CONFIG_IOCTL_CFG80211
6695         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
6696         {
6697                 rtw_cfg80211_rx_action_p2p(padapter, pframe, len);
6698                 return _SUCCESS;
6699         }
6700         else
6701 #endif //CONFIG_IOCTL_CFG80211
6702         {
6703                 len -= sizeof(struct rtw_ieee80211_hdr_3addr);
6704                 OUI_Subtype = frame_body[5];
6705                 dialogToken = frame_body[6];
6706
6707                 switch(OUI_Subtype)
6708                 {
6709                         case P2P_NOTICE_OF_ABSENCE:
6710
6711                                 break;
6712
6713                         case P2P_PRESENCE_REQUEST:
6714
6715                                 process_p2p_presence_req(pwdinfo, pframe, len);
6716
6717                                 break;
6718
6719                         case P2P_PRESENCE_RESPONSE:
6720
6721                                 break;
6722
6723                         case P2P_GO_DISC_REQUEST:
6724
6725                                 break;
6726
6727                         default:
6728                                 break;
6729
6730                 }
6731         }
6732 #endif //CONFIG_P2P
6733
6734         return _SUCCESS;
6735
6736 }
6737
6738 unsigned int OnAction(_adapter *padapter, union recv_frame *precv_frame)
6739 {
6740         int i;
6741         unsigned char   category;
6742         struct action_handler *ptable;
6743         unsigned char   *frame_body;
6744         u8 *pframe = precv_frame->u.hdr.rx_data;
6745
6746         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
6747
6748         category = frame_body[0];
6749
6750         for(i = 0; i < sizeof(OnAction_tbl)/sizeof(struct action_handler); i++)
6751         {
6752                 ptable = &OnAction_tbl[i];
6753
6754                 if(category == ptable->num)
6755                         ptable->func(padapter, precv_frame);
6756
6757         }
6758
6759         return _SUCCESS;
6760
6761 }
6762
6763 unsigned int DoReserved(_adapter *padapter, union recv_frame *precv_frame)
6764 {
6765
6766         //DBG_871X("rcvd mgt frame(%x, %x)\n", (GetFrameSubType(pframe) >> 4), *(unsigned int *)GetAddr1Ptr(pframe));
6767         return _SUCCESS;
6768 }
6769
6770 struct xmit_frame *_alloc_mgtxmitframe(struct xmit_priv *pxmitpriv, bool once)
6771 {
6772         struct xmit_frame *pmgntframe;
6773         struct xmit_buf *pxmitbuf;
6774
6775         if (once)
6776                 pmgntframe = rtw_alloc_xmitframe_once(pxmitpriv);
6777         else
6778                 pmgntframe = rtw_alloc_xmitframe_ext(pxmitpriv);
6779
6780         if (pmgntframe == NULL) {
6781                 DBG_871X(FUNC_ADPT_FMT" alloc xmitframe fail, once:%d\n", FUNC_ADPT_ARG(pxmitpriv->adapter), once);
6782                 goto exit;
6783         }
6784
6785         if ((pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv)) == NULL) {
6786                 DBG_871X(FUNC_ADPT_FMT" alloc xmitbuf fail\n", FUNC_ADPT_ARG(pxmitpriv->adapter));
6787                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
6788                 pmgntframe = NULL;
6789                 goto exit;
6790         }
6791
6792         pmgntframe->frame_tag = MGNT_FRAMETAG;
6793         pmgntframe->pxmitbuf = pxmitbuf;
6794         pmgntframe->buf_addr = pxmitbuf->pbuf;
6795         pxmitbuf->priv_data = pmgntframe;
6796
6797 exit:
6798         return pmgntframe;
6799
6800 }
6801
6802 inline struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
6803 {
6804         return _alloc_mgtxmitframe(pxmitpriv, _FALSE);
6805 }
6806
6807 inline struct xmit_frame *alloc_mgtxmitframe_once(struct xmit_priv *pxmitpriv)
6808 {
6809         return _alloc_mgtxmitframe(pxmitpriv, _TRUE);
6810 }
6811
6812
6813 /****************************************************************************
6814
6815 Following are some TX fuctions for WiFi MLME
6816
6817 *****************************************************************************/
6818
6819 void update_mgnt_tx_rate(_adapter *padapter, u8 rate)
6820 {
6821         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6822
6823         pmlmeext->tx_rate = rate;
6824         //DBG_871X("%s(): rate = %x\n",__FUNCTION__, rate);
6825 }
6826
6827 void update_mgntframe_attrib(_adapter *padapter, struct pkt_attrib *pattrib)
6828 {
6829         u8      wireless_mode;
6830         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6831         struct xmit_priv                *pxmitpriv = &padapter->xmitpriv;
6832         //_rtw_memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib));
6833
6834         pattrib->hdrlen = 24;
6835         pattrib->nr_frags = 1;
6836         pattrib->priority = 7;
6837         pattrib->mac_id = 0;
6838         pattrib->qsel = QSLT_MGNT;
6839
6840         pattrib->pktlen = 0;
6841
6842         if (pmlmeext->tx_rate == IEEE80211_CCK_RATE_1MB)
6843                 wireless_mode = WIRELESS_11B;
6844         else
6845                 wireless_mode = WIRELESS_11G;
6846         pattrib->raid =  rtw_get_mgntframe_raid(padapter, wireless_mode);
6847         pattrib->rate = pmlmeext->tx_rate;
6848
6849         pattrib->encrypt = _NO_PRIVACY_;
6850         pattrib->bswenc = _FALSE;
6851
6852         pattrib->qos_en = _FALSE;
6853         pattrib->ht_en = _FALSE;
6854         pattrib->bwmode = CHANNEL_WIDTH_20;
6855         pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
6856         pattrib->sgi = _FALSE;
6857
6858         pattrib->seqnum = pmlmeext->mgnt_seq;
6859
6860         pattrib->retry_ctrl = _TRUE;
6861
6862         pattrib->mbssid = 0;
6863         pattrib->hw_ssn_sel = pxmitpriv->hw_ssn_seq_no;
6864
6865 }
6866
6867 void update_mgntframe_attrib_addr(_adapter *padapter, struct xmit_frame *pmgntframe)
6868 {
6869         u8      *pframe;
6870         struct pkt_attrib       *pattrib = &pmgntframe->attrib;
6871
6872         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6873
6874         _rtw_memcpy(pattrib->ra, GetAddr1Ptr(pframe), ETH_ALEN);
6875         _rtw_memcpy(pattrib->ta, GetAddr2Ptr(pframe), ETH_ALEN);
6876 }
6877
6878 void dump_mgntframe(_adapter *padapter, struct xmit_frame *pmgntframe)
6879 {
6880         if(padapter->bSurpriseRemoved == _TRUE ||
6881                 padapter->bDriverStopped == _TRUE)
6882         {
6883                 rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
6884                 rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
6885                 return;
6886         }
6887
6888         rtw_hal_mgnt_xmit(padapter, pmgntframe);
6889 }
6890
6891 s32 dump_mgntframe_and_wait(_adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
6892 {
6893         s32 ret = _FAIL;
6894         _irqL irqL;
6895         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
6896         struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
6897         struct submit_ctx sctx;
6898
6899         if(padapter->bSurpriseRemoved == _TRUE ||
6900                 padapter->bDriverStopped == _TRUE)
6901         {
6902                 rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
6903                 rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
6904                 return ret;
6905         }
6906
6907         rtw_sctx_init(&sctx, timeout_ms);
6908         pxmitbuf->sctx = &sctx;
6909
6910         ret = rtw_hal_mgnt_xmit(padapter, pmgntframe);
6911
6912         if (ret == _SUCCESS)
6913                 ret = rtw_sctx_wait(&sctx, __func__);
6914
6915         _enter_critical(&pxmitpriv->lock_sctx, &irqL);
6916         pxmitbuf->sctx = NULL;
6917         _exit_critical(&pxmitpriv->lock_sctx, &irqL);
6918
6919          return ret;
6920 }
6921
6922 s32 dump_mgntframe_and_wait_ack(_adapter *padapter, struct xmit_frame *pmgntframe)
6923 {
6924 #ifdef CONFIG_XMIT_ACK
6925         static u8 seq_no = 0;
6926         s32 ret = _FAIL;
6927         u32 timeout_ms = 500;//  500ms
6928         struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
6929         #ifdef CONFIG_CONCURRENT_MODE
6930         if (padapter->pbuddy_adapter && !padapter->isprimary)
6931                 pxmitpriv = &(padapter->pbuddy_adapter->xmitpriv);
6932         #endif
6933
6934         if(padapter->bSurpriseRemoved == _TRUE ||
6935                 padapter->bDriverStopped == _TRUE)
6936         {
6937                 rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
6938                 rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
6939                 return -1;
6940         }
6941
6942         _enter_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
6943         pxmitpriv->ack_tx = _TRUE;
6944         pxmitpriv->seq_no = seq_no++;
6945         pmgntframe->ack_report = 1;
6946         if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS) {
6947                 ret = rtw_ack_tx_wait(pxmitpriv, timeout_ms);
6948         }
6949
6950         pxmitpriv->ack_tx = _FALSE;
6951         _exit_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
6952
6953          return ret;
6954 #else //!CONFIG_XMIT_ACK
6955         dump_mgntframe(padapter, pmgntframe);
6956         rtw_msleep_os(50);
6957         return _SUCCESS;
6958 #endif //!CONFIG_XMIT_ACK
6959 }
6960
6961 int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
6962 {
6963         u8 *ssid_ie;
6964         sint ssid_len_ori;
6965         int len_diff = 0;
6966
6967         ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
6968
6969         //DBG_871X("%s hidden_ssid_mode:%u, ssid_ie:%p, ssid_len_ori:%d\n", __FUNCTION__, hidden_ssid_mode, ssid_ie, ssid_len_ori);
6970
6971         if(ssid_ie && ssid_len_ori>0)
6972         {
6973                 switch(hidden_ssid_mode)
6974                 {
6975                         case 1:
6976                         {
6977                                 u8 *next_ie = ssid_ie + 2 + ssid_len_ori;
6978                                 u32 remain_len = 0;
6979
6980                                 remain_len = ies_len -(next_ie-ies);
6981
6982                                 ssid_ie[1] = 0;
6983                                 _rtw_memcpy(ssid_ie+2, next_ie, remain_len);
6984                                 len_diff -= ssid_len_ori;
6985
6986                                 break;
6987                         }
6988                         case 2:
6989                                 _rtw_memset(&ssid_ie[2], 0, ssid_len_ori);
6990                                 break;
6991                         default:
6992                                 break;
6993                 }
6994         }
6995
6996         return len_diff;
6997 }
6998
6999 void issue_beacon(_adapter *padapter, int timeout_ms)
7000 {
7001         struct xmit_frame       *pmgntframe;
7002         struct pkt_attrib       *pattrib;
7003         unsigned char   *pframe;
7004         struct rtw_ieee80211_hdr *pwlanhdr;
7005         unsigned short *fctrl;
7006         unsigned int    rate_len;
7007         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
7008 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7009         _irqL irqL;
7010         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
7011 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7012         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7013         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7014         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
7015         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
7016 #ifdef CONFIG_P2P
7017         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
7018 #endif //CONFIG_P2P
7019
7020
7021         //DBG_871X("%s\n", __FUNCTION__);
7022
7023 #ifdef CONFIG_BCN_ICF
7024         if ((pmgntframe = rtw_alloc_bcnxmitframe(pxmitpriv)) == NULL)
7025 #else
7026         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7027 #endif
7028         {
7029                 DBG_871X("%s, alloc mgnt frame fail\n", __FUNCTION__);
7030                 return;
7031         }
7032 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7033         _enter_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
7034 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7035
7036         //update attribute
7037         pattrib = &pmgntframe->attrib;
7038         update_mgntframe_attrib(padapter, pattrib);
7039         pattrib->qsel = QSLT_BEACON;
7040         #ifdef CONFIG_CONCURRENT_MODE
7041         if(padapter->iface_type == IFACE_PORT1)
7042                 pattrib->mbssid = 1;
7043         #endif
7044
7045         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7046
7047         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7048         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7049
7050
7051         fctrl = &(pwlanhdr->frame_ctl);
7052         *(fctrl) = 0;
7053
7054         _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
7055         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
7056         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
7057
7058         SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
7059         //pmlmeext->mgnt_seq++;
7060         SetFrameSubType(pframe, WIFI_BEACON);
7061
7062         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
7063         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
7064
7065         if( (pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
7066         {
7067                 //DBG_871X("ie len=%d\n", cur_network->IELength);
7068 #ifdef CONFIG_P2P
7069                 // for P2P : Primary Device Type & Device Name
7070                 u32 wpsielen=0, insert_len=0;
7071                 u8 *wpsie=NULL;
7072                 wpsie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wpsielen);
7073
7074                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wpsie && wpsielen>0)
7075                 {
7076                         uint wps_offset, remainder_ielen;
7077                         u8 *premainder_ie, *pframe_wscie;
7078
7079                         wps_offset = (uint)(wpsie - cur_network->IEs);
7080
7081                         premainder_ie = wpsie + wpsielen;
7082
7083                         remainder_ielen = cur_network->IELength - wps_offset - wpsielen;
7084
7085 #ifdef CONFIG_IOCTL_CFG80211
7086                         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
7087                         {
7088                                 if(pmlmepriv->wps_beacon_ie && pmlmepriv->wps_beacon_ie_len>0)
7089                                 {
7090                                         _rtw_memcpy(pframe, cur_network->IEs, wps_offset);
7091                                         pframe += wps_offset;
7092                                         pattrib->pktlen += wps_offset;
7093
7094                                         _rtw_memcpy(pframe, pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len);
7095                                         pframe += pmlmepriv->wps_beacon_ie_len;
7096                                         pattrib->pktlen += pmlmepriv->wps_beacon_ie_len;
7097
7098                                         //copy remainder_ie to pframe
7099                                         _rtw_memcpy(pframe, premainder_ie, remainder_ielen);
7100                                         pframe += remainder_ielen;
7101                                         pattrib->pktlen += remainder_ielen;
7102                                 }
7103                                 else
7104                                 {
7105                                         _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
7106                                         pframe += cur_network->IELength;
7107                                         pattrib->pktlen += cur_network->IELength;
7108                                 }
7109                         }
7110                         else
7111 #endif //CONFIG_IOCTL_CFG80211
7112                         {
7113                                 pframe_wscie = pframe + wps_offset;
7114                                 _rtw_memcpy(pframe, cur_network->IEs, wps_offset+wpsielen);
7115                                 pframe += (wps_offset + wpsielen);
7116                                 pattrib->pktlen += (wps_offset + wpsielen);
7117
7118                                 //now pframe is end of wsc ie, insert Primary Device Type & Device Name
7119                                 //      Primary Device Type
7120                                 //      Type:
7121                                 *(u16*) ( pframe + insert_len) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
7122                                 insert_len += 2;
7123
7124                                 //      Length:
7125                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( 0x0008 );
7126                                 insert_len += 2;
7127
7128                                 //      Value:
7129                                 //      Category ID
7130                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
7131                                 insert_len += 2;
7132
7133                                 //      OUI
7134                                 *(u32*) ( pframe + insert_len ) = cpu_to_be32( WPSOUI );
7135                                 insert_len += 4;
7136
7137                                 //      Sub Category ID
7138                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
7139                                 insert_len += 2;
7140
7141
7142                                 //      Device Name
7143                                 //      Type:
7144                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
7145                                 insert_len += 2;
7146
7147                                 //      Length:
7148                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( pwdinfo->device_name_len );
7149                                 insert_len += 2;
7150
7151                                 //      Value:
7152                                 _rtw_memcpy( pframe + insert_len, pwdinfo->device_name, pwdinfo->device_name_len );
7153                                 insert_len += pwdinfo->device_name_len;
7154
7155
7156                                 //update wsc ie length
7157                                 *(pframe_wscie+1) = (wpsielen -2) + insert_len;
7158
7159                                 //pframe move to end
7160                                 pframe+=insert_len;
7161                                 pattrib->pktlen += insert_len;
7162
7163                                 //copy remainder_ie to pframe
7164                                 _rtw_memcpy(pframe, premainder_ie, remainder_ielen);
7165                                 pframe += remainder_ielen;
7166                                 pattrib->pktlen += remainder_ielen;
7167                         }
7168                 }
7169                 else
7170 #endif //CONFIG_P2P
7171                 {
7172                         int len_diff;
7173                         _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
7174                         len_diff = update_hidden_ssid(
7175                                 pframe+_BEACON_IE_OFFSET_
7176                                 , cur_network->IELength-_BEACON_IE_OFFSET_
7177                                 , pmlmeinfo->hidden_ssid_mode
7178                         );
7179                         pframe += (cur_network->IELength+len_diff);
7180                         pattrib->pktlen += (cur_network->IELength+len_diff);
7181                 }
7182
7183                 {
7184                         u8 *wps_ie;
7185                         uint wps_ielen;
7186                         u8 sr = 0;
7187                         wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr+TXDESC_OFFSET+sizeof (struct rtw_ieee80211_hdr_3addr)+_BEACON_IE_OFFSET_,
7188                                 pattrib->pktlen-sizeof (struct rtw_ieee80211_hdr_3addr)-_BEACON_IE_OFFSET_, NULL, &wps_ielen);
7189                         if (wps_ie && wps_ielen>0) {
7190                                 rtw_get_wps_attr_content(wps_ie,  wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8*)(&sr), NULL);
7191                         }
7192                         if (sr != 0)
7193                                 set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
7194                         else
7195                                 _clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
7196                 }
7197
7198 #ifdef CONFIG_P2P
7199                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
7200                 {
7201                         u32 len;
7202 #ifdef CONFIG_IOCTL_CFG80211
7203                         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
7204                         {
7205                                 len = pmlmepriv->p2p_beacon_ie_len;
7206                                 if(pmlmepriv->p2p_beacon_ie && len>0)
7207                                         _rtw_memcpy(pframe, pmlmepriv->p2p_beacon_ie, len);
7208                         }
7209                         else
7210 #endif //CONFIG_IOCTL_CFG80211
7211                         {
7212                                 len = build_beacon_p2p_ie(pwdinfo, pframe);
7213                         }
7214
7215                         pframe += len;
7216                         pattrib->pktlen += len;
7217 #ifdef CONFIG_WFD
7218 #ifdef CONFIG_IOCTL_CFG80211
7219                         if(_TRUE == pwdinfo->wfd_info->wfd_enable)
7220 #endif //CONFIG_IOCTL_CFG80211
7221                         {
7222                         len = build_beacon_wfd_ie( pwdinfo, pframe );
7223                         }
7224 #ifdef CONFIG_IOCTL_CFG80211
7225                         else
7226                         {
7227                                 len = 0;
7228                                 if(pmlmepriv->wfd_beacon_ie && pmlmepriv->wfd_beacon_ie_len>0)
7229                                 {
7230                                         len = pmlmepriv->wfd_beacon_ie_len;
7231                                         _rtw_memcpy(pframe, pmlmepriv->wfd_beacon_ie, len);
7232                                 }
7233                         }
7234 #endif //CONFIG_IOCTL_CFG80211
7235                         pframe += len;
7236                         pattrib->pktlen += len;
7237 #endif //CONFIG_WFD
7238                 }
7239 #endif //CONFIG_P2P
7240
7241                 goto _issue_bcn;
7242
7243         }
7244
7245         //below for ad-hoc mode
7246
7247         //timestamp will be inserted by hardware
7248         pframe += 8;
7249         pattrib->pktlen += 8;
7250
7251         // beacon interval: 2 bytes
7252
7253         _rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
7254
7255         pframe += 2;
7256         pattrib->pktlen += 2;
7257
7258         // capability info: 2 bytes
7259
7260         _rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
7261
7262         pframe += 2;
7263         pattrib->pktlen += 2;
7264
7265         // SSID
7266         pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
7267
7268         // supported rates...
7269         rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
7270         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8)? 8: rate_len), cur_network->SupportedRates, &pattrib->pktlen);
7271
7272         // DS parameter set
7273         pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
7274
7275         //if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
7276         {
7277                 u8 erpinfo=0;
7278                 u32 ATIMWindow;
7279                 // IBSS Parameter Set...
7280                 //ATIMWindow = cur->Configuration.ATIMWindow;
7281                 ATIMWindow = 0;
7282                 pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
7283
7284                 //ERP IE
7285                 pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
7286         }
7287
7288
7289         // EXTERNDED SUPPORTED RATE
7290         if (rate_len > 8)
7291         {
7292                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
7293         }
7294
7295
7296         //todo:HT for adhoc
7297
7298 _issue_bcn:
7299
7300 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7301         pmlmepriv->update_bcn = _FALSE;
7302
7303         _exit_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
7304 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7305
7306         if ((pattrib->pktlen + TXDESC_SIZE) > 512)
7307         {
7308                 DBG_871X("beacon frame too large\n");
7309                 return;
7310         }
7311
7312         pattrib->last_txcmdsz = pattrib->pktlen;
7313
7314         //DBG_871X("issue bcn_sz=%d\n", pattrib->last_txcmdsz);
7315         if(timeout_ms > 0)
7316                 dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
7317         else
7318                 dump_mgntframe(padapter, pmgntframe);
7319
7320 }
7321
7322 void issue_probersp(_adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
7323 {
7324         struct xmit_frame                       *pmgntframe;
7325         struct pkt_attrib                       *pattrib;
7326         unsigned char                                   *pframe;
7327         struct rtw_ieee80211_hdr        *pwlanhdr;
7328         unsigned short                          *fctrl;
7329         unsigned char                                   *mac, *bssid;
7330         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
7331 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7332         u8 *pwps_ie;
7333         uint wps_ielen;
7334         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
7335 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7336         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7337         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7338         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
7339         unsigned int    rate_len;
7340 #ifdef CONFIG_P2P
7341         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
7342 #ifdef CONFIG_WFD
7343         u32                                     wfdielen = 0;
7344 #endif //CONFIG_WFD
7345 #endif //CONFIG_P2P
7346
7347         //DBG_871X("%s\n", __FUNCTION__);
7348
7349         if(da == NULL)
7350                 return;
7351
7352         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7353         {
7354                 DBG_871X("%s, alloc mgnt frame fail\n", __FUNCTION__);
7355                 return;
7356         }
7357
7358
7359         //update attribute
7360         pattrib = &pmgntframe->attrib;
7361         update_mgntframe_attrib(padapter, pattrib);
7362
7363         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7364
7365         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7366         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7367
7368         mac = adapter_mac_addr(padapter);
7369         bssid = cur_network->MacAddress;
7370
7371         fctrl = &(pwlanhdr->frame_ctl);
7372         *(fctrl) = 0;
7373         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
7374         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
7375         _rtw_memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
7376
7377         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7378         pmlmeext->mgnt_seq++;
7379         SetFrameSubType(fctrl, WIFI_PROBERSP);
7380
7381         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7382         pattrib->pktlen = pattrib->hdrlen;
7383         pframe += pattrib->hdrlen;
7384
7385
7386         if(cur_network->IELength>MAX_IE_SZ)
7387                 return;
7388
7389 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7390         if( (pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
7391         {
7392                 pwps_ie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
7393
7394                 //inerset & update wps_probe_resp_ie
7395                 if((pmlmepriv->wps_probe_resp_ie!=NULL) && pwps_ie && (wps_ielen>0))
7396                 {
7397                         uint wps_offset, remainder_ielen;
7398                         u8 *premainder_ie;
7399
7400                         wps_offset = (uint)(pwps_ie - cur_network->IEs);
7401
7402                         premainder_ie = pwps_ie + wps_ielen;
7403
7404                         remainder_ielen = cur_network->IELength - wps_offset - wps_ielen;
7405
7406                         _rtw_memcpy(pframe, cur_network->IEs, wps_offset);
7407                         pframe += wps_offset;
7408                         pattrib->pktlen += wps_offset;
7409
7410                         wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];//to get ie data len
7411                         if((wps_offset+wps_ielen+2)<=MAX_IE_SZ)
7412                         {
7413                                 _rtw_memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen+2);
7414                                 pframe += wps_ielen+2;
7415                                 pattrib->pktlen += wps_ielen+2;
7416                         }
7417
7418                         if((wps_offset+wps_ielen+2+remainder_ielen)<=MAX_IE_SZ)
7419                         {
7420                                 _rtw_memcpy(pframe, premainder_ie, remainder_ielen);
7421                                 pframe += remainder_ielen;
7422                                 pattrib->pktlen += remainder_ielen;
7423                         }
7424                 }
7425                 else
7426                 {
7427                         _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
7428                         pframe += cur_network->IELength;
7429                         pattrib->pktlen += cur_network->IELength;
7430                 }
7431
7432                 /* retrieve SSID IE from cur_network->Ssid */
7433                 {
7434                         u8 *ssid_ie;
7435                         sint ssid_ielen;
7436                         sint ssid_ielen_diff;
7437                         u8 buf[MAX_IE_SZ];
7438                         u8 *ies = pmgntframe->buf_addr+TXDESC_OFFSET+sizeof(struct rtw_ieee80211_hdr_3addr);
7439
7440                         ssid_ie = rtw_get_ie(ies+_FIXED_IE_LENGTH_, _SSID_IE_, &ssid_ielen,
7441                                 (pframe-ies)-_FIXED_IE_LENGTH_);
7442
7443                         ssid_ielen_diff = cur_network->Ssid.SsidLength - ssid_ielen;
7444
7445                         if (ssid_ie &&  cur_network->Ssid.SsidLength) {
7446                                 uint remainder_ielen;
7447                                 u8 *remainder_ie;
7448                                 remainder_ie = ssid_ie+2;
7449                                 remainder_ielen = (pframe-remainder_ie);
7450
7451                                 if (remainder_ielen > MAX_IE_SZ) {
7452                                         DBG_871X_LEVEL(_drv_warning_, FUNC_ADPT_FMT" remainder_ielen > MAX_IE_SZ\n", FUNC_ADPT_ARG(padapter));
7453                                         remainder_ielen = MAX_IE_SZ;
7454                                 }
7455
7456                                 _rtw_memcpy(buf, remainder_ie, remainder_ielen);
7457                                 _rtw_memcpy(remainder_ie+ssid_ielen_diff, buf, remainder_ielen);
7458                                 *(ssid_ie+1) = cur_network->Ssid.SsidLength;
7459                                 _rtw_memcpy(ssid_ie+2, cur_network->Ssid.Ssid, cur_network->Ssid.SsidLength);
7460
7461                                 pframe += ssid_ielen_diff;
7462                                 pattrib->pktlen += ssid_ielen_diff;
7463                         }
7464                 }
7465         }
7466         else
7467 #endif
7468         {
7469
7470                 //timestamp will be inserted by hardware
7471                 pframe += 8;
7472                 pattrib->pktlen += 8;
7473
7474                 // beacon interval: 2 bytes
7475
7476                 _rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
7477
7478                 pframe += 2;
7479                 pattrib->pktlen += 2;
7480
7481                 // capability info: 2 bytes
7482
7483                 _rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
7484
7485                 pframe += 2;
7486                 pattrib->pktlen += 2;
7487
7488                 //below for ad-hoc mode
7489
7490                 // SSID
7491                 pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
7492
7493                 // supported rates...
7494                 rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
7495                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8)? 8: rate_len), cur_network->SupportedRates, &pattrib->pktlen);
7496
7497                 // DS parameter set
7498                 pframe =rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
7499
7500                 if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
7501                 {
7502                         u8 erpinfo=0;
7503                         u32 ATIMWindow;
7504                         // IBSS Parameter Set...
7505                         //ATIMWindow = cur->Configuration.ATIMWindow;
7506                         ATIMWindow = 0;
7507                         pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
7508
7509                         //ERP IE
7510                         pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
7511                 }
7512
7513
7514                 // EXTERNDED SUPPORTED RATE
7515                 if (rate_len > 8)
7516                 {
7517                         pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
7518                 }
7519
7520
7521                 //todo:HT for adhoc
7522
7523         }
7524
7525 #ifdef CONFIG_P2P
7526         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)
7527                 /* IOT issue, When wifi_spec is not set, send probe_resp with P2P IE even if probe_req has no P2P IE */
7528                 && (is_valid_p2p_probereq || !padapter->registrypriv.wifi_spec))
7529         {
7530                 u32 len;
7531 #ifdef CONFIG_IOCTL_CFG80211
7532                 if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
7533                 {
7534                         //if pwdinfo->role == P2P_ROLE_DEVICE will call issue_probersp_p2p()
7535                         len = pmlmepriv->p2p_go_probe_resp_ie_len;
7536                         if(pmlmepriv->p2p_go_probe_resp_ie && len>0)
7537                                 _rtw_memcpy(pframe, pmlmepriv->p2p_go_probe_resp_ie, len);
7538                 }
7539                 else
7540 #endif //CONFIG_IOCTL_CFG80211
7541                 {
7542                         len = build_probe_resp_p2p_ie(pwdinfo, pframe);
7543                 }
7544
7545                 pframe += len;
7546                 pattrib->pktlen += len;
7547
7548 #ifdef CONFIG_WFD
7549 #ifdef CONFIG_IOCTL_CFG80211
7550                 if(_TRUE == pwdinfo->wfd_info->wfd_enable)
7551 #endif //CONFIG_IOCTL_CFG80211
7552                 {
7553                         len = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
7554                 }
7555 #ifdef CONFIG_IOCTL_CFG80211
7556                 else
7557                 {
7558                         len = 0;
7559                         if(pmlmepriv->wfd_probe_resp_ie && pmlmepriv->wfd_probe_resp_ie_len>0)
7560                         {
7561                                 len = pmlmepriv->wfd_probe_resp_ie_len;
7562                                 _rtw_memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, len);
7563                         }
7564                 }
7565 #endif //CONFIG_IOCTL_CFG80211
7566                 pframe += len;
7567                 pattrib->pktlen += len;
7568 #endif //CONFIG_WFD
7569
7570         }
7571 #endif //CONFIG_P2P
7572
7573
7574 #ifdef CONFIG_AUTO_AP_MODE
7575 {
7576         struct sta_info *psta;
7577         struct sta_priv *pstapriv = &padapter->stapriv;
7578
7579         DBG_871X("(%s)\n", __FUNCTION__);
7580
7581         //check rc station
7582         psta = rtw_get_stainfo(pstapriv, da);
7583         if (psta && psta->isrc && psta->pid>0)
7584         {
7585                 u8 RC_OUI[4]={0x00,0xE0,0x4C,0x0A};
7586                 u8 RC_INFO[14] = {0};
7587                 //EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2]
7588                 u16 cu_ch = (u16)cur_network->Configuration.DSConfig;
7589
7590                 DBG_871X("%s, reply rc(pid=0x%x) device "MAC_FMT" in ch=%d\n", __FUNCTION__,
7591                         psta->pid, MAC_ARG(psta->hwaddr), cu_ch);
7592
7593                 //append vendor specific ie
7594                 _rtw_memcpy(RC_INFO, RC_OUI, sizeof(RC_OUI));
7595                 _rtw_memcpy(&RC_INFO[4], mac, ETH_ALEN);
7596                 _rtw_memcpy(&RC_INFO[10], (u8*)&psta->pid, 2);
7597                 _rtw_memcpy(&RC_INFO[12], (u8*)&cu_ch, 2);
7598
7599                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, sizeof(RC_INFO), RC_INFO, &pattrib->pktlen);
7600         }
7601 }
7602 #endif //CONFIG_AUTO_AP_MODE
7603
7604
7605         pattrib->last_txcmdsz = pattrib->pktlen;
7606
7607
7608         dump_mgntframe(padapter, pmgntframe);
7609
7610         return;
7611
7612 }
7613
7614 int _issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da, u8 ch, bool append_wps, int wait_ack)
7615 {
7616         int ret = _FAIL;
7617         struct xmit_frame               *pmgntframe;
7618         struct pkt_attrib               *pattrib;
7619         unsigned char                   *pframe;
7620         struct rtw_ieee80211_hdr        *pwlanhdr;
7621         unsigned short          *fctrl;
7622         unsigned char                   *mac;
7623         unsigned char                   bssrate[NumRates];
7624         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
7625         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
7626         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7627         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7628         int     bssrate_len = 0;
7629         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
7630
7631         RT_TRACE(_module_rtl871x_mlme_c_,_drv_notice_,("+issue_probereq\n"));
7632
7633         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7634         {
7635                 goto exit;
7636         }
7637
7638         //update attribute
7639         pattrib = &pmgntframe->attrib;
7640         update_mgntframe_attrib(padapter, pattrib);
7641
7642
7643         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7644
7645         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7646         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7647
7648         mac = adapter_mac_addr(padapter);
7649
7650         fctrl = &(pwlanhdr->frame_ctl);
7651         *(fctrl) = 0;
7652
7653         if (da)
7654         {
7655                 //      unicast probe request frame
7656                 _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
7657                 _rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN);
7658         }
7659         else
7660         {
7661                 //      broadcast probe request frame
7662                 _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
7663                 _rtw_memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
7664         }
7665
7666         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
7667
7668         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7669         pmlmeext->mgnt_seq++;
7670         SetFrameSubType(pframe, WIFI_PROBEREQ);
7671
7672         pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
7673         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
7674
7675         if(pssid)
7676                 pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &(pattrib->pktlen));
7677         else
7678                 pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &(pattrib->pktlen));
7679
7680         get_rate_set(padapter, bssrate, &bssrate_len);
7681
7682         if (bssrate_len > 8)
7683         {
7684                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
7685                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
7686         }
7687         else
7688         {
7689                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
7690         }
7691
7692         if (ch)
7693                 pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, &ch, &pattrib->pktlen);
7694
7695         if (append_wps) {
7696                 //add wps_ie for wps2.0
7697                 if(pmlmepriv->wps_probe_req_ie_len>0 && pmlmepriv->wps_probe_req_ie)
7698                 {
7699                         _rtw_memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
7700                         pframe += pmlmepriv->wps_probe_req_ie_len;
7701                         pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
7702                         //pmlmepriv->wps_probe_req_ie_len = 0 ;//reset to zero
7703                 }
7704         }
7705
7706         pattrib->last_txcmdsz = pattrib->pktlen;
7707
7708         RT_TRACE(_module_rtl871x_mlme_c_,_drv_notice_,("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
7709
7710         if (wait_ack) {
7711                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
7712         } else {
7713                 dump_mgntframe(padapter, pmgntframe);
7714                 ret = _SUCCESS;
7715         }
7716
7717 exit:
7718         return ret;
7719 }
7720
7721 inline void issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da)
7722 {
7723         _issue_probereq(padapter, pssid, da, 0, 1, _FALSE);
7724 }
7725
7726 int issue_probereq_ex(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da, u8 ch, bool append_wps,
7727         int try_cnt, int wait_ms)
7728 {
7729         int ret;
7730         int i = 0;
7731         u32 start = rtw_get_current_time();
7732
7733         do
7734         {
7735                 ret = _issue_probereq(padapter, pssid, da, ch, append_wps, wait_ms>0?_TRUE:_FALSE);
7736
7737                 i++;
7738
7739                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
7740                         break;
7741
7742                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
7743                         rtw_msleep_os(wait_ms);
7744
7745         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
7746
7747         if (ret != _FAIL) {
7748                 ret = _SUCCESS;
7749                 #ifndef DBG_XMIT_ACK
7750                 goto exit;
7751                 #endif
7752         }
7753
7754         if (try_cnt && wait_ms) {
7755                 if (da)
7756                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
7757                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
7758                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7759                 else
7760                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
7761                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
7762                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7763         }
7764 exit:
7765         return ret;
7766 }
7767
7768 // if psta == NULL, indiate we are station(client) now...
7769 void issue_auth(_adapter *padapter, struct sta_info *psta, unsigned short status)
7770 {
7771         struct xmit_frame                       *pmgntframe;
7772         struct pkt_attrib                       *pattrib;
7773         unsigned char                                   *pframe;
7774         struct rtw_ieee80211_hdr        *pwlanhdr;
7775         unsigned short                          *fctrl;
7776         unsigned int                                    val32;
7777         unsigned short                          val16;
7778         int use_shared_key = 0;
7779         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
7780         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7781         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7782
7783         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7784         {
7785                 return;
7786         }
7787
7788         //update attribute
7789         pattrib = &pmgntframe->attrib;
7790         update_mgntframe_attrib(padapter, pattrib);
7791
7792         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7793
7794         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7795         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7796
7797         fctrl = &(pwlanhdr->frame_ctl);
7798         *(fctrl) = 0;
7799
7800         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7801         pmlmeext->mgnt_seq++;
7802         SetFrameSubType(pframe, WIFI_AUTH);
7803
7804         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
7805         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7806
7807
7808         if(psta)// for AP mode
7809         {
7810 #ifdef CONFIG_NATIVEAP_MLME
7811
7812                 _rtw_memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);
7813                 _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
7814                 _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN);
7815
7816
7817                 // setting auth algo number
7818                 val16 = (u16)psta->authalg;
7819
7820                 if(status != _STATS_SUCCESSFUL_)
7821                         val16 = 0;
7822
7823                 if (val16)      {
7824                         val16 = cpu_to_le16(val16);
7825                         use_shared_key = 1;
7826                 }
7827
7828                 pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7829
7830                 // setting auth seq number
7831                 val16 =(u16)psta->auth_seq;
7832                 val16 = cpu_to_le16(val16);
7833                 pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7834
7835                 // setting status code...
7836                 val16 = status;
7837                 val16 = cpu_to_le16(val16);
7838                 pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen));
7839
7840                 // added challenging text...
7841                 if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
7842                 {
7843                         pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, psta->chg_txt, &(pattrib->pktlen));
7844                 }
7845 #endif
7846         }
7847         else
7848         {
7849                 _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
7850                 _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
7851                 _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
7852
7853                 // setting auth algo number
7854                 val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)? 1: 0;// 0:OPEN System, 1:Shared key
7855                 if (val16)      {
7856                         val16 = cpu_to_le16(val16);
7857                         use_shared_key = 1;
7858                 }
7859                 //DBG_871X("%s auth_algo= %s auth_seq=%d\n",__FUNCTION__,(pmlmeinfo->auth_algo==0)?"OPEN":"SHARED",pmlmeinfo->auth_seq);
7860
7861                 //setting IV for auth seq #3
7862                 if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
7863                 {
7864                         //DBG_871X("==> iv(%d),key_index(%d)\n",pmlmeinfo->iv,pmlmeinfo->key_index);
7865                         val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
7866                         val32 = cpu_to_le32(val32);
7867                         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&val32, &(pattrib->pktlen));
7868
7869                         pattrib->iv_len = 4;
7870                 }
7871
7872                 pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7873
7874                 // setting auth seq number
7875                 val16 = pmlmeinfo->auth_seq;
7876                 val16 = cpu_to_le16(val16);
7877                 pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7878
7879
7880                 // setting status code...
7881                 val16 = status;
7882                 val16 = cpu_to_le16(val16);
7883                 pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen));
7884
7885                 // then checking to see if sending challenging text...
7886                 if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
7887                 {
7888                         pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen));
7889
7890                         SetPrivacy(fctrl);
7891
7892                         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7893
7894                         pattrib->encrypt = _WEP40_;
7895
7896                         pattrib->icv_len = 4;
7897
7898                         pattrib->pktlen += pattrib->icv_len;
7899
7900                 }
7901
7902         }
7903
7904         pattrib->last_txcmdsz = pattrib->pktlen;
7905
7906         rtw_wep_encrypt(padapter, (u8 *)pmgntframe);
7907         DBG_871X("%s\n", __FUNCTION__);
7908         dump_mgntframe(padapter, pmgntframe);
7909
7910         return;
7911 }
7912
7913
7914 void issue_asocrsp(_adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
7915 {
7916 #ifdef CONFIG_AP_MODE
7917         struct xmit_frame       *pmgntframe;
7918         struct rtw_ieee80211_hdr        *pwlanhdr;
7919         struct pkt_attrib *pattrib;
7920         unsigned char   *pbuf, *pframe;
7921         unsigned short val;
7922         unsigned short *fctrl;
7923         struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
7924         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
7925         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
7926         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7927         WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);
7928         u8 *ie = pnetwork->IEs;
7929 #ifdef CONFIG_P2P
7930         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
7931 #ifdef CONFIG_WFD
7932         u32                                     wfdielen = 0;
7933 #endif //CONFIG_WFD
7934
7935 #endif //CONFIG_P2P
7936
7937         DBG_871X("%s\n", __FUNCTION__);
7938
7939         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7940         {
7941                 return;
7942         }
7943
7944         //update attribute
7945         pattrib = &pmgntframe->attrib;
7946         update_mgntframe_attrib(padapter, pattrib);
7947
7948
7949         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7950
7951         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7952         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7953
7954         fctrl = &(pwlanhdr->frame_ctl);
7955         *(fctrl) = 0;
7956
7957         _rtw_memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
7958         _rtw_memcpy((void *)GetAddr2Ptr(pwlanhdr), adapter_mac_addr(padapter), ETH_ALEN);
7959         _rtw_memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7960
7961
7962         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7963         pmlmeext->mgnt_seq++;
7964         if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
7965                 SetFrameSubType(pwlanhdr, pkt_type);
7966         else
7967                 return;
7968
7969         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7970         pattrib->pktlen += pattrib->hdrlen;
7971         pframe += pattrib->hdrlen;
7972
7973         //capability
7974         val = *(unsigned short *)rtw_get_capability_from_ie(ie);
7975
7976         pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_ , (unsigned char *)&val, &(pattrib->pktlen));
7977
7978         status = cpu_to_le16(status);
7979         pframe = rtw_set_fixed_ie(pframe , _STATUS_CODE_ , (unsigned char *)&status, &(pattrib->pktlen));
7980
7981         val = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
7982         pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_ , (unsigned char *)&val, &(pattrib->pktlen));
7983
7984         if (pstat->bssratelen <= 8)
7985         {
7986                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen));
7987         }
7988         else
7989         {
7990                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &(pattrib->pktlen));
7991                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (pstat->bssratelen-8), pstat->bssrateset+8, &(pattrib->pktlen));
7992         }
7993
7994 #ifdef CONFIG_80211N_HT
7995         if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option))
7996         {
7997                 uint ie_len=0;
7998
7999                 //FILL HT CAP INFO IE
8000                 //p = hostapd_eid_ht_capabilities_info(hapd, p);
8001                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
8002                 if(pbuf && ie_len>0)
8003                 {
8004                         _rtw_memcpy(pframe, pbuf, ie_len+2);
8005                         pframe += (ie_len+2);
8006                         pattrib->pktlen +=(ie_len+2);
8007                 }
8008
8009                 //FILL HT ADD INFO IE
8010                 //p = hostapd_eid_ht_operation(hapd, p);
8011                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
8012                 if(pbuf && ie_len>0)
8013                 {
8014                         _rtw_memcpy(pframe, pbuf, ie_len+2);
8015                         pframe += (ie_len+2);
8016                         pattrib->pktlen +=(ie_len+2);
8017                 }
8018
8019         }
8020 #endif
8021
8022 #ifdef CONFIG_80211AC_VHT
8023         if ((pstat->flags & WLAN_STA_VHT) && (pmlmepriv->vhtpriv.vht_option))
8024         {
8025                 u32 ie_len=0;
8026
8027                 //FILL VHT CAP IE
8028                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, EID_VHTCapability, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
8029                 if(pbuf && ie_len>0)
8030                 {
8031                         _rtw_memcpy(pframe, pbuf, ie_len+2);
8032                         pframe += (ie_len+2);
8033                         pattrib->pktlen +=(ie_len+2);
8034                 }
8035
8036                 //FILL VHT OPERATION IE
8037                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, EID_VHTOperation, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
8038                 if(pbuf && ie_len>0)
8039                 {
8040                         _rtw_memcpy(pframe, pbuf, ie_len+2);
8041                         pframe += (ie_len+2);
8042                         pattrib->pktlen +=(ie_len+2);
8043                 }
8044         }
8045 #endif //CONFIG_80211AC_VHT
8046
8047         //FILL WMM IE
8048         if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option))
8049         {
8050                 uint ie_len=0;
8051                 unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
8052
8053                 for (pbuf = ie + _BEACON_IE_OFFSET_; ;pbuf+= (ie_len + 2))
8054                 {
8055                         pbuf = rtw_get_ie(pbuf, _VENDOR_SPECIFIC_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
8056                         if(pbuf && _rtw_memcmp(pbuf+2, WMM_PARA_IE, 6))
8057                         {
8058                                 _rtw_memcpy(pframe, pbuf, ie_len+2);
8059                                 pframe += (ie_len+2);
8060                                 pattrib->pktlen +=(ie_len+2);
8061
8062                                 break;
8063                         }
8064
8065                         if ((pbuf == NULL) || (ie_len == 0))
8066                         {
8067                                 break;
8068                         }
8069                 }
8070
8071         }
8072
8073
8074         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
8075         {
8076                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
8077         }
8078
8079         //add WPS IE ie for wps 2.0
8080         if(pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len>0)
8081         {
8082                 _rtw_memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len);
8083
8084                 pframe += pmlmepriv->wps_assoc_resp_ie_len;
8085                 pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
8086         }
8087
8088 #ifdef CONFIG_P2P
8089         if( padapter->wdinfo.driver_interface == DRIVER_WEXT )
8090         {
8091                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && (pstat->is_p2p_device == _TRUE))
8092                 {
8093                         u32 len;
8094
8095                         len = build_assoc_resp_p2p_ie(pwdinfo, pframe, pstat->p2p_status_code);
8096
8097                         pframe += len;
8098                         pattrib->pktlen += len;
8099                 }
8100         }
8101 #ifdef CONFIG_WFD
8102         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)
8103 #ifdef CONFIG_IOCTL_CFG80211
8104                 && (_TRUE == pwdinfo->wfd_info->wfd_enable)
8105 #endif //CONFIG_IOCTL_CFG80211
8106         )
8107         {
8108                 wfdielen = build_assoc_resp_wfd_ie(pwdinfo, pframe);
8109                 pframe += wfdielen;
8110                 pattrib->pktlen += wfdielen;
8111         }
8112 #endif //CONFIG_WFD
8113 #endif //CONFIG_P2P
8114
8115         pattrib->last_txcmdsz = pattrib->pktlen;
8116
8117         dump_mgntframe(padapter, pmgntframe);
8118
8119 #endif
8120 }
8121
8122 void issue_assocreq(_adapter *padapter)
8123 {
8124         int ret = _FAIL;
8125         struct xmit_frame                               *pmgntframe;
8126         struct pkt_attrib                               *pattrib;
8127         unsigned char                                   *pframe, *p;
8128         struct rtw_ieee80211_hdr                        *pwlanhdr;
8129         unsigned short                          *fctrl;
8130         unsigned short                          val16;
8131         unsigned int                                    i, j, ie_len, index=0;
8132         unsigned char                                   rf_type, bssrate[NumRates], sta_bssrate[NumRates];
8133         PNDIS_802_11_VARIABLE_IEs       pIE;
8134         struct registry_priv    *pregpriv = &padapter->registrypriv;
8135         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
8136         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
8137         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8138         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8139         int     bssrate_len = 0, sta_bssrate_len = 0;
8140         u8      vs_ie_length = 0;
8141 #ifdef CONFIG_P2P
8142         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
8143         u8                                      p2pie[ 255 ] = { 0x00 };
8144         u16                                     p2pielen = 0;
8145 #ifdef CONFIG_WFD
8146         u32                                     wfdielen = 0;
8147 #endif //CONFIG_WFD
8148 #endif //CONFIG_P2P
8149
8150 #ifdef CONFIG_DFS
8151         u16     cap;
8152
8153         /* Dot H */
8154         u8 pow_cap_ele[2] = { 0x00 };
8155         u8 sup_ch[ 30 * 2 ] = {0x00 }, sup_ch_idx = 0, idx_5g = 2;      //For supported channel
8156 #endif //CONFIG_DFS
8157
8158         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8159                 goto exit;
8160
8161         //update attribute
8162         pattrib = &pmgntframe->attrib;
8163         update_mgntframe_attrib(padapter, pattrib);
8164
8165
8166         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8167
8168         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8169         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8170
8171         fctrl = &(pwlanhdr->frame_ctl);
8172         *(fctrl) = 0;
8173         _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8174         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
8175         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8176
8177         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8178         pmlmeext->mgnt_seq++;
8179         SetFrameSubType(pframe, WIFI_ASSOCREQ);
8180
8181         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8182         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8183
8184         //caps
8185
8186 #ifdef CONFIG_DFS
8187         _rtw_memcpy(&cap, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
8188         cap |= cap_SpecMgmt;
8189         _rtw_memcpy(pframe, &cap, 2);
8190 #else
8191         _rtw_memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
8192 #endif //CONFIG_DFS
8193
8194         pframe += 2;
8195         pattrib->pktlen += 2;
8196
8197         //listen interval
8198         //todo: listen interval for power saving
8199         val16 = cpu_to_le16(3);
8200         _rtw_memcpy(pframe ,(unsigned char *)&val16, 2);
8201         pframe += 2;
8202         pattrib->pktlen += 2;
8203
8204         //SSID
8205         pframe = rtw_set_ie(pframe, _SSID_IE_,  pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &(pattrib->pktlen));
8206
8207 #ifdef CONFIG_DFS
8208         /* Dot H */
8209         if(pmlmeext->cur_channel > 14)
8210         {
8211                 pow_cap_ele[0] = 13;    // Minimum transmit power capability
8212                 pow_cap_ele[1] = 21;    // Maximum transmit power capability
8213                 pframe = rtw_set_ie(pframe, EID_PowerCap, 2, pow_cap_ele, &(pattrib->pktlen));
8214
8215                 //supported channels
8216                 do{
8217                         if( pmlmeext->channel_set[sup_ch_idx].ChannelNum <= 14 )
8218                         {
8219                                 sup_ch[0] = 1;  //First channel number
8220                                 sup_ch[1] = pmlmeext->channel_set[sup_ch_idx].ChannelNum;       //Number of channel
8221                         }
8222                         else
8223                         {
8224                                 sup_ch[idx_5g++] = pmlmeext->channel_set[sup_ch_idx].ChannelNum;
8225                                 sup_ch[idx_5g++] = 1;
8226                         }
8227                         sup_ch_idx++;
8228                 }
8229                 while( pmlmeext->channel_set[sup_ch_idx].ChannelNum != 0 );
8230                 pframe = rtw_set_ie(pframe, EID_SupportedChannels, idx_5g, sup_ch, &(pattrib->pktlen));
8231         }
8232 #endif //CONFIG_DFS
8233
8234         //supported rate & extended supported rate
8235
8236 #if 1   // Check if the AP's supported rates are also supported by STA.
8237         get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);
8238         //DBG_871X("sta_bssrate_len=%d\n", sta_bssrate_len);
8239
8240         if(pmlmeext->cur_channel == 14)// for JAPAN, channel 14 can only uses B Mode(CCK)
8241         {
8242                 sta_bssrate_len = 4;
8243         }
8244
8245
8246         //for (i = 0; i < sta_bssrate_len; i++) {
8247         //      DBG_871X("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]);
8248         //}
8249
8250         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
8251                 if (pmlmeinfo->network.SupportedRates[i] == 0) break;
8252                 DBG_871X("network.SupportedRates[%d]=%02X\n", i, pmlmeinfo->network.SupportedRates[i]);
8253         }
8254
8255
8256         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
8257                 if (pmlmeinfo->network.SupportedRates[i] == 0) break;
8258
8259
8260                 // Check if the AP's supported rates are also supported by STA.
8261                 for (j=0; j < sta_bssrate_len; j++) {
8262                          // Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP
8263                         if ( (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK)
8264                                         == (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)) {
8265                                 //DBG_871X("match i = %d, j=%d\n", i, j);
8266                                 break;
8267                         } else {
8268                                 //DBG_871X("not match: %02X != %02X\n", (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK), (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK));
8269                         }
8270                 }
8271
8272                 if (j == sta_bssrate_len) {
8273                         // the rate is not supported by STA
8274                         DBG_871X("%s(): the rate[%d]=%02X is not supported by STA!\n",__FUNCTION__, i, pmlmeinfo->network.SupportedRates[i]);
8275                 } else {
8276                         // the rate is supported by STA
8277                         bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
8278                 }
8279         }
8280
8281         bssrate_len = index;
8282         DBG_871X("bssrate_len = %d\n", bssrate_len);
8283
8284 #else   // Check if the AP's supported rates are also supported by STA.
8285 #if 0
8286         get_rate_set(padapter, bssrate, &bssrate_len);
8287 #else
8288         for (bssrate_len = 0; bssrate_len < NumRates; bssrate_len++) {
8289                 if (pmlmeinfo->network.SupportedRates[bssrate_len] == 0) break;
8290
8291                 if (pmlmeinfo->network.SupportedRates[bssrate_len] == 0x2C) // Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP
8292                         break;
8293
8294                 bssrate[bssrate_len] = pmlmeinfo->network.SupportedRates[bssrate_len];
8295         }
8296 #endif
8297 #endif  // Check if the AP's supported rates are also supported by STA.
8298
8299         if ((bssrate_len == 0) && (pmlmeinfo->network.SupportedRates[0] != 0)) {
8300                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
8301                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
8302                 goto exit; //don't connect to AP if no joint supported rate
8303         }
8304
8305
8306         if (bssrate_len > 8)
8307         {
8308                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
8309                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
8310         }
8311         else if (bssrate_len > 0)
8312         {
8313                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
8314         } else {
8315                 DBG_871X("%s: Connect to AP without 11b and 11g data rate!\n",__FUNCTION__);
8316         }
8317
8318         //vendor specific IE, such as WPA, WMM, WPS
8319         for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;)
8320         {
8321                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i);
8322
8323                 switch (pIE->ElementID)
8324                 {
8325                         case _VENDOR_SPECIFIC_IE_:
8326                                 if ((_rtw_memcmp(pIE->data, RTW_WPA_OUI, 4)) ||
8327                                                 (_rtw_memcmp(pIE->data, WMM_OUI, 4)) ||
8328                                                 (_rtw_memcmp(pIE->data, WPS_OUI, 4)))
8329                                 {
8330                                         vs_ie_length = pIE->Length;
8331                                         if((!padapter->registrypriv.wifi_spec) && (_rtw_memcmp(pIE->data, WPS_OUI, 4)))
8332                                         {
8333                                                 //Commented by Kurt 20110629
8334                                                 //In some older APs, WPS handshake
8335                                                 //would be fail if we append vender extensions informations to AP
8336
8337                                                 vs_ie_length = 14;
8338                                         }
8339
8340                                         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, vs_ie_length, pIE->data, &(pattrib->pktlen));
8341                                 }
8342                                 break;
8343
8344                         case EID_WPA2:
8345                                 pframe = rtw_set_ie(pframe, EID_WPA2, pIE->Length, pIE->data, &(pattrib->pktlen));
8346                                 break;
8347 #ifdef CONFIG_80211N_HT
8348                         case EID_HTCapability:
8349                                 if(padapter->mlmepriv.htpriv.ht_option==_TRUE) {
8350                                         if (!(is_ap_in_tkip(padapter)))
8351                                         {
8352                                                 _rtw_memcpy(&(pmlmeinfo->HT_caps), pIE->data, sizeof(struct HT_caps_element));
8353
8354                                                 pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = cpu_to_le16(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info);
8355
8356                                                 pframe = rtw_set_ie(pframe, EID_HTCapability, pIE->Length , (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
8357                                         }
8358                                 }
8359                                 break;
8360
8361                         case EID_EXTCapability:
8362                                 if(padapter->mlmepriv.htpriv.ht_option==_TRUE) {
8363                                         pframe = rtw_set_ie(pframe, EID_EXTCapability, pIE->Length, pIE->data, &(pattrib->pktlen));
8364                                 }
8365                                 break;
8366 #endif //CONFIG_80211N_HT
8367 #ifdef CONFIG_80211AC_VHT
8368                         case EID_VHTCapability:
8369                                 if (padapter->mlmepriv.vhtpriv.vht_option ==_TRUE) {
8370                                         pframe = rtw_set_ie(pframe, EID_VHTCapability, pIE->Length, pIE->data, &(pattrib->pktlen));
8371                                 }
8372                                 break;
8373
8374                         case EID_OpModeNotification:
8375                                 if (padapter->mlmepriv.vhtpriv.vht_option ==_TRUE) {
8376                                         pframe = rtw_set_ie(pframe, EID_OpModeNotification, pIE->Length, pIE->data, &(pattrib->pktlen));
8377                                 }
8378                                 break;
8379 #endif // CONFIG_80211AC_VHT
8380                         default:
8381                                 break;
8382                 }
8383
8384                 i += (pIE->Length + 2);
8385         }
8386
8387         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
8388         {
8389                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
8390         }
8391
8392
8393 #ifdef CONFIG_WAPI_SUPPORT
8394         rtw_build_assoc_req_wapi_ie(padapter, pframe, pattrib);
8395 #endif
8396
8397
8398 #ifdef CONFIG_P2P
8399
8400 #ifdef CONFIG_IOCTL_CFG80211
8401         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
8402         {
8403                 if(pmlmepriv->p2p_assoc_req_ie && pmlmepriv->p2p_assoc_req_ie_len>0)
8404                 {
8405                         _rtw_memcpy(pframe, pmlmepriv->p2p_assoc_req_ie, pmlmepriv->p2p_assoc_req_ie_len);
8406                         pframe += pmlmepriv->p2p_assoc_req_ie_len;
8407                         pattrib->pktlen += pmlmepriv->p2p_assoc_req_ie_len;
8408                 }
8409         }
8410         else
8411 #endif //CONFIG_IOCTL_CFG80211
8412         {
8413                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
8414                 {
8415                         //      Should add the P2P IE in the association request frame.
8416                         //      P2P OUI
8417
8418                         p2pielen = 0;
8419                         p2pie[ p2pielen++ ] = 0x50;
8420                         p2pie[ p2pielen++ ] = 0x6F;
8421                         p2pie[ p2pielen++ ] = 0x9A;
8422                         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
8423
8424                         //      Commented by Albert 20101109
8425                         //      According to the P2P Specification, the association request frame should contain 3 P2P attributes
8426                         //      1. P2P Capability
8427                         //      2. Extended Listen Timing
8428                         //      3. Device Info
8429                         //      Commented by Albert 20110516
8430                         //      4. P2P Interface
8431
8432                         //      P2P Capability
8433                         //      Type:
8434                         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
8435
8436                         //      Length:
8437                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
8438                         p2pielen += 2;
8439
8440                         //      Value:
8441                         //      Device Capability Bitmap, 1 byte
8442                         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
8443
8444                         //      Group Capability Bitmap, 1 byte
8445                         if ( pwdinfo->persistent_supported )
8446                                 p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
8447                         else
8448                                 p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
8449
8450                         //      Extended Listen Timing
8451                         //      Type:
8452                         p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
8453
8454                         //      Length:
8455                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
8456                         p2pielen += 2;
8457
8458                         //      Value:
8459                         //      Availability Period
8460                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
8461                         p2pielen += 2;
8462
8463                         //      Availability Interval
8464                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
8465                         p2pielen += 2;
8466
8467                         //      Device Info
8468                         //      Type:
8469                         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
8470
8471                         //      Length:
8472                         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
8473                         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
8474                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
8475                         p2pielen += 2;
8476
8477                         //      Value:
8478                         //      P2P Device Address
8479                         _rtw_memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
8480                         p2pielen += ETH_ALEN;
8481
8482                         //      Config Method
8483                         //      This field should be big endian. Noted by P2P specification.
8484                         if ( ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN ) ||
8485                                 ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN ) )
8486                         {
8487                                 *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
8488                         }
8489                         else
8490                         {
8491                                 *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_PBC );
8492                         }
8493
8494                         p2pielen += 2;
8495
8496                         //      Primary Device Type
8497                         //      Category ID
8498                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
8499                         p2pielen += 2;
8500
8501                         //      OUI
8502                         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
8503                         p2pielen += 4;
8504
8505                         //      Sub Category ID
8506                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
8507                         p2pielen += 2;
8508
8509                         //      Number of Secondary Device Types
8510                         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
8511
8512                         //      Device Name
8513                         //      Type:
8514                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
8515                         p2pielen += 2;
8516
8517                         //      Length:
8518                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
8519                         p2pielen += 2;
8520
8521                         //      Value:
8522                         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
8523                         p2pielen += pwdinfo->device_name_len;
8524
8525                         //      P2P Interface
8526                         //      Type:
8527                         p2pie[ p2pielen++ ] = P2P_ATTR_INTERFACE;
8528
8529                         //      Length:
8530                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x000D );
8531                         p2pielen += 2;
8532
8533                         //      Value:
8534                         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );        //      P2P Device Address
8535                         p2pielen += ETH_ALEN;
8536
8537                         p2pie[ p2pielen++ ] = 1;        //      P2P Interface Address Count
8538
8539                         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );        //      P2P Interface Address List
8540                         p2pielen += ETH_ALEN;
8541
8542                         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
8543
8544 #ifdef CONFIG_WFD
8545                         //wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);
8546                         //pframe += wfdielen;
8547                         //pattrib->pktlen += wfdielen;
8548 #endif //CONFIG_WFD
8549                 }
8550         }
8551
8552 #endif //CONFIG_P2P
8553
8554 #ifdef CONFIG_WFD
8555 #ifdef CONFIG_IOCTL_CFG80211
8556         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
8557 #endif //CONFIG_IOCTL_CFG80211
8558         {
8559                 wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);
8560                 pframe += wfdielen;
8561                 pattrib->pktlen += wfdielen;
8562         }
8563 #ifdef CONFIG_IOCTL_CFG80211
8564         else if (pmlmepriv->wfd_assoc_req_ie != NULL && pmlmepriv->wfd_assoc_req_ie_len>0)
8565         {
8566                 //WFD IE
8567                 _rtw_memcpy(pframe, pmlmepriv->wfd_assoc_req_ie, pmlmepriv->wfd_assoc_req_ie_len);
8568                 pattrib->pktlen += pmlmepriv->wfd_assoc_req_ie_len;
8569                 pframe += pmlmepriv->wfd_assoc_req_ie_len;
8570         }
8571 #endif //CONFIG_IOCTL_CFG80211
8572 #endif //CONFIG_WFD
8573
8574         pattrib->last_txcmdsz = pattrib->pktlen;
8575         dump_mgntframe(padapter, pmgntframe);
8576
8577         ret = _SUCCESS;
8578
8579 exit:
8580         if (ret == _SUCCESS)
8581                 rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen);
8582         else
8583                 rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
8584
8585         return;
8586 }
8587
8588 //when wait_ack is ture, this function shoule be called at process context
8589 static int _issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
8590 {
8591         int ret = _FAIL;
8592         struct xmit_frame                       *pmgntframe;
8593         struct pkt_attrib                       *pattrib;
8594         unsigned char                                   *pframe;
8595         struct rtw_ieee80211_hdr        *pwlanhdr;
8596         unsigned short                          *fctrl;
8597         struct xmit_priv        *pxmitpriv;
8598         struct mlme_ext_priv    *pmlmeext;
8599         struct mlme_ext_info    *pmlmeinfo;
8600
8601         //DBG_871X("%s:%d\n", __FUNCTION__, power_mode);
8602
8603         if(!padapter)
8604                 goto exit;
8605
8606         pxmitpriv = &(padapter->xmitpriv);
8607         pmlmeext = &(padapter->mlmeextpriv);
8608         pmlmeinfo = &(pmlmeext->mlmext_info);
8609
8610         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8611         {
8612                 goto exit;
8613         }
8614
8615         //update attribute
8616         pattrib = &pmgntframe->attrib;
8617         update_mgntframe_attrib(padapter, pattrib);
8618         pattrib->retry_ctrl = _FALSE;
8619
8620         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8621
8622         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8623         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8624
8625         fctrl = &(pwlanhdr->frame_ctl);
8626         *(fctrl) = 0;
8627
8628         if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
8629         {
8630                 SetFrDs(fctrl);
8631         }
8632         else if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
8633         {
8634                 SetToDs(fctrl);
8635         }
8636
8637         if (power_mode)
8638         {
8639                 SetPwrMgt(fctrl);
8640         }
8641
8642         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
8643         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
8644         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8645
8646         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8647         pmlmeext->mgnt_seq++;
8648         SetFrameSubType(pframe, WIFI_DATA_NULL);
8649
8650         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8651         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8652
8653         pattrib->last_txcmdsz = pattrib->pktlen;
8654
8655         if(wait_ack)
8656         {
8657                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
8658         }
8659         else
8660         {
8661                 dump_mgntframe(padapter, pmgntframe);
8662                 ret = _SUCCESS;
8663         }
8664
8665 exit:
8666         return ret;
8667 }
8668
8669 /*
8670  * [IMPORTANT] Don't call this function in interrupt context
8671  *
8672  * When wait_ms > 0, this function shoule be called at process context
8673  * da == NULL for station mode
8674  */
8675 int issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
8676 {
8677         int ret;
8678         int i = 0;
8679         u32 start = rtw_get_current_time();
8680         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8681         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8682         struct sta_info *psta;
8683
8684
8685         /* da == NULL, assum it's null data for sta to ap*/
8686         if (da == NULL)
8687                 da = get_my_bssid(&(pmlmeinfo->network));
8688
8689         psta = rtw_get_stainfo(&padapter->stapriv, da);
8690         if (psta) {
8691                 if (power_mode)
8692                         rtw_hal_macid_sleep(padapter, psta->mac_id);
8693                 else
8694                         rtw_hal_macid_wakeup(padapter, psta->mac_id);
8695         } else {
8696                 DBG_871X(FUNC_ADPT_FMT ": Can't find sta info for " MAC_FMT ", skip macid %s!!\n",
8697                         FUNC_ADPT_ARG(padapter), MAC_ARG(da), power_mode?"sleep":"wakeup");
8698                 rtw_warn_on(1);
8699         }
8700
8701         do {
8702                 ret = _issue_nulldata(padapter, da, power_mode, wait_ms>0?_TRUE:_FALSE);
8703
8704                 i++;
8705
8706                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
8707                         break;
8708
8709                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
8710                         rtw_msleep_os(wait_ms);
8711
8712         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
8713
8714         if (ret != _FAIL) {
8715                 ret = _SUCCESS;
8716                 #ifndef DBG_XMIT_ACK
8717                 goto exit;
8718                 #endif
8719         }
8720
8721         if (try_cnt && wait_ms) {
8722                 if (da)
8723                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
8724                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
8725                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8726                 else
8727                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
8728                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
8729                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8730         }
8731 exit:
8732         return ret;
8733 }
8734
8735 /*
8736  * [IMPORTANT] This function run in interrupt context
8737  *
8738  * The null data packet would be sent without power bit,
8739  * and not guarantee success.
8740  */
8741 s32 issue_nulldata_in_interrupt(PADAPTER padapter, u8 *da, unsigned int power_mode)
8742 {
8743         int ret;
8744         struct mlme_ext_priv *pmlmeext;
8745         struct mlme_ext_info *pmlmeinfo;
8746
8747
8748         pmlmeext = &padapter->mlmeextpriv;
8749         pmlmeinfo = &pmlmeext->mlmext_info;
8750
8751         /* da == NULL, assum it's null data for sta to ap*/
8752         if (da == NULL)
8753                 da = get_my_bssid(&(pmlmeinfo->network));
8754
8755         ret = _issue_nulldata(padapter, da, power_mode, _FALSE);
8756
8757         return ret;
8758 }
8759
8760 //when wait_ack is ture, this function shoule be called at process context
8761 static int _issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int wait_ack)
8762 {
8763         int ret = _FAIL;
8764         struct xmit_frame                       *pmgntframe;
8765         struct pkt_attrib                       *pattrib;
8766         unsigned char                                   *pframe;
8767         struct rtw_ieee80211_hdr        *pwlanhdr;
8768         unsigned short                          *fctrl, *qc;
8769         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
8770         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8771         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8772
8773         DBG_871X("%s\n", __FUNCTION__);
8774
8775         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8776         {
8777                 goto exit;
8778         }
8779
8780         //update attribute
8781         pattrib = &pmgntframe->attrib;
8782         update_mgntframe_attrib(padapter, pattrib);
8783
8784         pattrib->hdrlen +=2;
8785         pattrib->qos_en = _TRUE;
8786         pattrib->eosp = 1;
8787         pattrib->ack_policy = 0;
8788         pattrib->mdata = 0;
8789
8790         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8791
8792         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8793         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8794
8795         fctrl = &(pwlanhdr->frame_ctl);
8796         *(fctrl) = 0;
8797
8798         if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
8799         {
8800                 SetFrDs(fctrl);
8801         }
8802         else if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
8803         {
8804                 SetToDs(fctrl);
8805         }
8806
8807         if(pattrib->mdata)
8808                 SetMData(fctrl);
8809
8810         qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
8811
8812         SetPriority(qc, tid);
8813
8814         SetEOSP(qc, pattrib->eosp);
8815
8816         SetAckpolicy(qc, pattrib->ack_policy);
8817
8818         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
8819         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
8820         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8821
8822         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8823         pmlmeext->mgnt_seq++;
8824         SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
8825
8826         pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos);
8827         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos);
8828
8829         pattrib->last_txcmdsz = pattrib->pktlen;
8830
8831         if(wait_ack)
8832         {
8833                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
8834         }
8835         else
8836         {
8837                 dump_mgntframe(padapter, pmgntframe);
8838                 ret = _SUCCESS;
8839         }
8840
8841 exit:
8842         return ret;
8843 }
8844
8845 //when wait_ms >0 , this function shoule be called at process context
8846 //da == NULL for station mode
8847 int issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms)
8848 {
8849         int ret;
8850         int i = 0;
8851         u32 start = rtw_get_current_time();
8852         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8853         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8854
8855         /* da == NULL, assum it's null data for sta to ap*/
8856         if (da == NULL)
8857                 da = get_my_bssid(&(pmlmeinfo->network));
8858
8859         do
8860         {
8861                 ret = _issue_qos_nulldata(padapter, da, tid, wait_ms>0?_TRUE:_FALSE);
8862
8863                 i++;
8864
8865                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
8866                         break;
8867
8868                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
8869                         rtw_msleep_os(wait_ms);
8870
8871         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
8872
8873         if (ret != _FAIL) {
8874                 ret = _SUCCESS;
8875                 #ifndef DBG_XMIT_ACK
8876                 goto exit;
8877                 #endif
8878         }
8879
8880         if (try_cnt && wait_ms) {
8881                 if (da)
8882                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
8883                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
8884                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8885                 else
8886                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
8887                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
8888                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8889         }
8890 exit:
8891         return ret;
8892 }
8893
8894 static int _issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack)
8895 {
8896         struct xmit_frame                       *pmgntframe;
8897         struct pkt_attrib                       *pattrib;
8898         unsigned char                                   *pframe;
8899         struct rtw_ieee80211_hdr        *pwlanhdr;
8900         unsigned short                          *fctrl;
8901         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
8902         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8903         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8904         int ret = _FAIL;
8905 #ifdef CONFIG_P2P
8906         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
8907 #endif //CONFIG_P2P
8908
8909         //DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
8910
8911 #ifdef CONFIG_P2P
8912         if ( !( rtw_p2p_chk_state( pwdinfo, P2P_STATE_NONE ) ) && ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) )
8913         {
8914                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
8915                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
8916         }
8917 #endif //CONFIG_P2P
8918
8919         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8920         {
8921                 goto exit;
8922         }
8923
8924         //update attribute
8925         pattrib = &pmgntframe->attrib;
8926         update_mgntframe_attrib(padapter, pattrib);
8927         pattrib->retry_ctrl = _FALSE;
8928
8929         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8930
8931         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8932         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8933
8934         fctrl = &(pwlanhdr->frame_ctl);
8935         *(fctrl) = 0;
8936
8937         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
8938         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
8939         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8940
8941         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8942         pmlmeext->mgnt_seq++;
8943         SetFrameSubType(pframe, WIFI_DEAUTH);
8944
8945         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8946         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8947
8948         reason = cpu_to_le16(reason);
8949         pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_ , (unsigned char *)&reason, &(pattrib->pktlen));
8950
8951         pattrib->last_txcmdsz = pattrib->pktlen;
8952
8953
8954         if(wait_ack)
8955         {
8956                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
8957         }
8958         else
8959         {
8960                 dump_mgntframe(padapter, pmgntframe);
8961                 ret = _SUCCESS;
8962         }
8963
8964 exit:
8965         return ret;
8966 }
8967
8968 int issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason)
8969 {
8970         DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
8971         return _issue_deauth(padapter, da, reason, _FALSE);
8972 }
8973
8974 int issue_deauth_ex(_adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
8975         int wait_ms)
8976 {
8977         int ret;
8978         int i = 0;
8979         u32 start = rtw_get_current_time();
8980
8981         do
8982         {
8983                 ret = _issue_deauth(padapter, da, reason, wait_ms>0?_TRUE:_FALSE);
8984
8985                 i++;
8986
8987                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
8988                         break;
8989
8990                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
8991                         rtw_msleep_os(wait_ms);
8992
8993         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
8994
8995         if (ret != _FAIL) {
8996                 ret = _SUCCESS;
8997                 #ifndef DBG_XMIT_ACK
8998                 goto exit;
8999                 #endif
9000         }
9001
9002         if (try_cnt && wait_ms) {
9003                 if (da)
9004                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
9005                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
9006                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
9007                 else
9008                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
9009                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
9010                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
9011         }
9012 exit:
9013         return ret;
9014 }
9015
9016 void issue_action_spct_ch_switch(_adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset)
9017 {
9018         _irqL   irqL;
9019         _list           *plist, *phead;
9020         struct xmit_frame                       *pmgntframe;
9021         struct pkt_attrib                       *pattrib;
9022         unsigned char                           *pframe;
9023         struct rtw_ieee80211_hdr        *pwlanhdr;
9024         unsigned short                  *fctrl;
9025         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
9026         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
9027         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
9028         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9029
9030
9031         DBG_871X(FUNC_NDEV_FMT" ra="MAC_FMT", ch:%u, offset:%u\n",
9032                 FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(ra), new_ch, ch_offset);
9033
9034         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
9035                 return;
9036
9037         //update attribute
9038         pattrib = &pmgntframe->attrib;
9039         update_mgntframe_attrib(padapter, pattrib);
9040
9041         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
9042
9043         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
9044         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
9045
9046         fctrl = &(pwlanhdr->frame_ctl);
9047         *(fctrl) = 0;
9048
9049         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN); /* RA */
9050         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); /* TA */
9051         _rtw_memcpy(pwlanhdr->addr3, ra, ETH_ALEN); /* DA = RA */
9052
9053         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
9054         pmlmeext->mgnt_seq++;
9055         SetFrameSubType(pframe, WIFI_ACTION);
9056
9057         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
9058         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
9059
9060         /* category, action */
9061         {
9062                 u8 category, action;
9063                 category = RTW_WLAN_CATEGORY_SPECTRUM_MGMT;
9064                 action = RTW_WLAN_ACTION_SPCT_CHL_SWITCH;
9065
9066                 pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
9067                 pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
9068         }
9069
9070         pframe = rtw_set_ie_ch_switch(pframe, &(pattrib->pktlen), 0, new_ch, 0);
9071         pframe = rtw_set_ie_secondary_ch_offset(pframe, &(pattrib->pktlen),
9072                 hal_ch_offset_to_secondary_ch_offset(ch_offset));
9073
9074         pattrib->last_txcmdsz = pattrib->pktlen;
9075
9076         dump_mgntframe(padapter, pmgntframe);
9077
9078 }
9079
9080 #ifdef CONFIG_IEEE80211W
9081 void issue_action_SA_Query(_adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short tid)
9082 {
9083         u8      category = RTW_WLAN_CATEGORY_SA_QUERY;
9084         u16     reason_code;
9085         struct xmit_frame               *pmgntframe;
9086         struct pkt_attrib               *pattrib;
9087         u8                                      *pframe;
9088         struct rtw_ieee80211_hdr        *pwlanhdr;
9089         u16                                     *fctrl;
9090         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
9091         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
9092         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9093         struct sta_info         *psta;
9094         struct sta_priv         *pstapriv = &padapter->stapriv;
9095         struct registry_priv            *pregpriv = &padapter->registrypriv;
9096
9097
9098         DBG_871X("%s\n", __FUNCTION__);
9099
9100         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
9101         {
9102                 DBG_871X("%s: alloc_mgtxmitframe fail\n", __FUNCTION__);
9103                 return;
9104         }
9105
9106         //update attribute
9107         pattrib = &pmgntframe->attrib;
9108         update_mgntframe_attrib(padapter, pattrib);
9109
9110         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
9111
9112         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
9113         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
9114
9115         fctrl = &(pwlanhdr->frame_ctl);
9116         *(fctrl) = 0;
9117
9118         if(raddr)
9119                 _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
9120         else
9121                 _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
9122         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
9123         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
9124
9125         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
9126         pmlmeext->mgnt_seq++;
9127         SetFrameSubType(pframe, WIFI_ACTION);
9128
9129         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
9130         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
9131
9132         pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
9133         pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
9134
9135         switch (action)
9136         {
9137                 case 0: //SA Query req
9138                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&pmlmeext->sa_query_seq, &pattrib->pktlen);
9139                         pmlmeext->sa_query_seq++;
9140                         //send sa query request to AP, AP should reply sa query response in 1 second
9141                         set_sa_query_timer(pmlmeext, 1000);
9142                         break;
9143
9144                 case 1: //SA Query rsp
9145                         tid = cpu_to_le16(tid);
9146                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&tid, &pattrib->pktlen);
9147                         break;
9148                 default:
9149                         break;
9150         }
9151
9152         pattrib->last_txcmdsz = pattrib->pktlen;
9153
9154         dump_mgntframe(padapter, pmgntframe);
9155 }
9156 #endif //CONFIG_IEEE80211W
9157
9158 /**
9159  * issue_action_ba - internal function to TX Block Ack action frame
9160  * @padapter: the adapter to TX
9161  * @raddr: receiver address
9162  * @action: Block Ack Action
9163  * @tid: tid
9164  * @size: the announced AMPDU buffer size. used by ADDBA_RESP
9165  * @status: status/reason code. used by ADDBA_RESP, DELBA
9166  * @initiator: if we are the initiator of AMPDU association. used by DELBA
9167  * @wait_ack: used xmit ack
9168  *
9169  * Returns:
9170  * _SUCCESS: No xmit ack is used or acked
9171  * _FAIL: not acked when using xmit ack
9172  */
9173 static int issue_action_ba(_adapter *padapter, unsigned char *raddr, unsigned char action
9174         , u8 tid, u8 size, u16 status, u8 initiator, int wait_ack)
9175 {
9176         int ret = _FAIL;
9177         u8      category = RTW_WLAN_CATEGORY_BACK;
9178         u16     start_seq;
9179         u16     BA_para_set;
9180         u16     BA_timeout_value;
9181         u16     BA_starting_seqctrl;
9182         struct xmit_frame               *pmgntframe;
9183         struct pkt_attrib               *pattrib;
9184         u8                                      *pframe;
9185         struct rtw_ieee80211_hdr        *pwlanhdr;
9186         u16                                     *fctrl;
9187         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
9188         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
9189         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9190         struct sta_info         *psta;
9191         struct sta_priv         *pstapriv = &padapter->stapriv;
9192         struct registry_priv            *pregpriv = &padapter->registrypriv;
9193
9194 #ifdef CONFIG_80211N_HT
9195
9196         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
9197                 goto exit;
9198
9199         //update attribute
9200         pattrib = &pmgntframe->attrib;
9201         update_mgntframe_attrib(padapter, pattrib);
9202
9203         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
9204
9205         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
9206         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
9207
9208         fctrl = &(pwlanhdr->frame_ctl);
9209         *(fctrl) = 0;
9210
9211         //_rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
9212         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
9213         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
9214         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
9215
9216         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
9217         pmlmeext->mgnt_seq++;
9218         SetFrameSubType(pframe, WIFI_ACTION);
9219
9220         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
9221         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
9222
9223         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
9224         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
9225
9226         if (category == 3)
9227         {
9228                 switch (action)
9229                 {
9230                         case RTW_WLAN_ACTION_ADDBA_REQ:
9231                                 do {
9232                                         pmlmeinfo->dialogToken++;
9233                                 } while (pmlmeinfo->dialogToken == 0);
9234                                 pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen));
9235
9236                                 #if defined(CONFIG_RTL8188E) && defined(CONFIG_SDIO_HCI)
9237                                 BA_para_set = (0x0802 | ((tid & 0xf) << 2)); /* immediate ack & 16 buffer size */
9238                                 #else
9239                                 BA_para_set = (0x1002 | ((tid & 0xf) << 2)); /* immediate ack & 64 buffer size */
9240                                 #endif
9241
9242                                 BA_para_set = cpu_to_le16(BA_para_set);
9243                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
9244
9245                                 //BA_timeout_value = 0xffff;//max: 65535 TUs(~ 65 ms)
9246                                 BA_timeout_value = 5000;//~ 5ms
9247                                 BA_timeout_value = cpu_to_le16(BA_timeout_value);
9248                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_timeout_value)), &(pattrib->pktlen));
9249
9250                                 //if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
9251                                 if ((psta = rtw_get_stainfo(pstapriv, raddr)) != NULL)
9252                                 {
9253                                         start_seq = (psta->sta_xmitpriv.txseq_tid[tid & 0x07]&0xfff) + 1;
9254
9255                                         DBG_871X("BA_starting_seqctrl = %d for TID=%d\n", start_seq, tid & 0x07);
9256
9257                                         psta->BA_starting_seqctrl[tid & 0x07] = start_seq;
9258
9259                                         BA_starting_seqctrl = start_seq << 4;
9260                                 }
9261
9262                                 BA_starting_seqctrl = cpu_to_le16(BA_starting_seqctrl);
9263                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_starting_seqctrl)), &(pattrib->pktlen));
9264                                 break;
9265
9266                         case RTW_WLAN_ACTION_ADDBA_RESP:
9267                                 pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen));
9268                                 status = cpu_to_le16(status);
9269                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&status), &(pattrib->pktlen));
9270
9271                                 BA_para_set = le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set);
9272
9273                                 BA_para_set &= ~IEEE80211_ADDBA_PARAM_TID_MASK;
9274                                 BA_para_set |= (tid << 2) & IEEE80211_ADDBA_PARAM_TID_MASK;
9275
9276                                 BA_para_set &= ~RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
9277                                 BA_para_set |= (size << 6) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
9278
9279                                 if (!padapter->registrypriv.wifi_spec) {
9280                                         if(pregpriv->ampdu_amsdu==0)//disabled
9281                                                 BA_para_set &= ~BIT(0);
9282                                         else if(pregpriv->ampdu_amsdu==1)//enabled
9283                                                 BA_para_set |= BIT(0);
9284                                 }
9285
9286                                 BA_para_set = cpu_to_le16(BA_para_set);
9287
9288                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
9289                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen));
9290                                 break;
9291
9292                         case RTW_WLAN_ACTION_DELBA:
9293                                 BA_para_set = 0;
9294                                 BA_para_set |= (tid << 12) & IEEE80211_DELBA_PARAM_TID_MASK;
9295                                 BA_para_set |= (initiator << 11) & IEEE80211_DELBA_PARAM_INITIATOR_MASK;
9296
9297                                 BA_para_set = cpu_to_le16(BA_para_set);
9298                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
9299                                 status = cpu_to_le16(status);
9300                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(status)), &(pattrib->pktlen));
9301                                 break;
9302                         default:
9303                                 break;
9304                 }
9305         }
9306
9307         pattrib->last_txcmdsz = pattrib->pktlen;
9308
9309         if (wait_ack) {
9310                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
9311         } else {
9312                 dump_mgntframe(padapter, pmgntframe);
9313                 ret = _SUCCESS;
9314         }
9315
9316 exit:
9317 #endif //CONFIG_80211N_HT
9318         return ret;
9319 }
9320
9321 /**
9322  * issue_addba_req - TX ADDBA_REQ
9323  * @adapter: the adapter to TX
9324  * @ra: receiver address
9325  * @tid: tid
9326  */
9327 inline void issue_addba_req(_adapter *adapter, unsigned char *ra, u8 tid)
9328 {
9329         issue_action_ba(adapter, ra, RTW_WLAN_ACTION_ADDBA_REQ
9330                 , tid
9331                 , 0 /* unused */
9332                 , 0 /* unused */
9333                 , 0 /* unused */
9334                 , _FALSE
9335         );
9336         DBG_871X(FUNC_ADPT_FMT" ra="MAC_FMT" tid=%u\n"
9337                 , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), tid);
9338
9339 }
9340
9341 /**
9342  * issue_addba_rsp - TX ADDBA_RESP
9343  * @adapter: the adapter to TX
9344  * @ra: receiver address
9345  * @tid: tid
9346  * @status: status code
9347  * @size: the announced AMPDU buffer size
9348  */
9349 inline void issue_addba_rsp(_adapter *adapter, unsigned char *ra, u8 tid, u16 status, u8 size)
9350 {
9351         issue_action_ba(adapter, ra, RTW_WLAN_ACTION_ADDBA_RESP
9352                 , tid
9353                 , size
9354                 , status
9355                 , 0 /* unused */
9356                 , _FALSE
9357         );
9358         DBG_871X(FUNC_ADPT_FMT" ra="MAC_FMT" status=%u, tid=%u, size=%u\n"
9359                 , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), status, tid, size);
9360 }
9361
9362 /**
9363  * issue_del_ba - TX DELBA
9364  * @adapter: the adapter to TX
9365  * @ra: receiver address
9366  * @tid: tid
9367  * @reason: reason code
9368  * @initiator: if we are the initiator of AMPDU association. used by DELBA
9369  */
9370 inline void issue_del_ba(_adapter *adapter, unsigned char *ra, u8 tid, u16 reason, u8 initiator)
9371 {
9372         issue_action_ba(adapter, ra, RTW_WLAN_ACTION_DELBA
9373                 , tid
9374                 , 0 /* unused */
9375                 , reason
9376                 , initiator
9377                 , _FALSE
9378         );
9379         DBG_871X(FUNC_ADPT_FMT" ra="MAC_FMT" reason=%u, tid=%u, initiator=%u\n"
9380                 , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), reason, tid, initiator);
9381 }
9382
9383 /**
9384  * issue_del_ba_ex - TX DELBA with xmit ack options
9385  * @adapter: the adapter to TX
9386  * @ra: receiver address
9387  * @tid: tid
9388  * @reason: reason code
9389  * @initiator: if we are the initiator of AMPDU association. used by DELBA
9390  * @try_cnt: the max driver level TX count to try
9391  * @wait_ms: the waiting ms for each driver level TX
9392  */
9393 int issue_del_ba_ex(_adapter *adapter, unsigned char *ra, u8 tid, u16 reason, u8 initiator
9394         , int try_cnt, int wait_ms)
9395 {
9396         int ret;
9397         int i = 0;
9398         u32 start = rtw_get_current_time();
9399
9400         do {
9401                 ret = issue_action_ba(adapter, ra, RTW_WLAN_ACTION_DELBA
9402                         , tid
9403                         , 0 /* unused */
9404                         , reason
9405                         , initiator
9406                         , wait_ms > 0?_TRUE:_FALSE
9407                 );
9408
9409                 i++;
9410
9411                 if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
9412                         break;
9413
9414                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
9415                         rtw_msleep_os(wait_ms);
9416
9417         } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
9418
9419         if (ret != _FAIL) {
9420                 ret = _SUCCESS;
9421         #ifndef DBG_XMIT_ACK
9422                 /* goto exit; */
9423         #endif
9424         }
9425
9426         if (try_cnt && wait_ms) {
9427                 DBG_871X(FUNC_ADPT_FMT" ra="MAC_FMT" reason=%u, tid=%u, initiator=%u%s, %d/%d in %u ms\n"
9428                         , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), reason, tid, initiator
9429                         , ret == _SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
9430         }
9431 exit:
9432         return ret;
9433 }
9434
9435 static void issue_action_BSSCoexistPacket(_adapter *padapter)
9436 {
9437         _irqL   irqL;
9438         _list           *plist, *phead;
9439         unsigned char category, action;
9440         struct xmit_frame                       *pmgntframe;
9441         struct pkt_attrib                       *pattrib;
9442         unsigned char                           *pframe;
9443         struct rtw_ieee80211_hdr        *pwlanhdr;
9444         unsigned short                  *fctrl;
9445         struct  wlan_network    *pnetwork = NULL;
9446         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
9447         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
9448         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
9449         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9450         _queue          *queue  = &(pmlmepriv->scanned_queue);
9451         u8 InfoContent[16] = {0};
9452         u8 ICS[8][15];
9453 #ifdef CONFIG_80211N_HT
9454         if((pmlmepriv->num_FortyMHzIntolerant==0) || (pmlmepriv->num_sta_no_ht==0))
9455                 return;
9456
9457         if(_TRUE == pmlmeinfo->bwmode_updated)
9458                 return;
9459
9460
9461         DBG_871X("%s\n", __FUNCTION__);
9462
9463
9464         category = RTW_WLAN_CATEGORY_PUBLIC;
9465         action = ACT_PUBLIC_BSSCOEXIST;
9466
9467         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
9468         {
9469                 return;
9470         }
9471
9472         //update attribute
9473         pattrib = &pmgntframe->attrib;
9474         update_mgntframe_attrib(padapter, pattrib);
9475
9476         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
9477
9478         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
9479         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
9480
9481         fctrl = &(pwlanhdr->frame_ctl);
9482         *(fctrl) = 0;
9483
9484         _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
9485         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
9486         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
9487
9488         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
9489         pmlmeext->mgnt_seq++;
9490         SetFrameSubType(pframe, WIFI_ACTION);
9491
9492         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
9493         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
9494
9495         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
9496         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
9497
9498
9499         //
9500         if(pmlmepriv->num_FortyMHzIntolerant>0)
9501         {
9502                 u8 iedata=0;
9503
9504                 iedata |= BIT(2);//20 MHz BSS Width Request
9505
9506                 pframe = rtw_set_ie(pframe, EID_BSSCoexistence,  1, &iedata, &(pattrib->pktlen));
9507
9508         }
9509
9510
9511         //
9512         _rtw_memset(ICS, 0, sizeof(ICS));
9513         if(pmlmepriv->num_sta_no_ht>0)
9514         {
9515                 int i;
9516
9517                 _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
9518
9519                 phead = get_list_head(queue);
9520                 plist = get_next(phead);
9521
9522                 while(1)
9523                 {
9524                         int len;
9525                         u8 *p;
9526                         WLAN_BSSID_EX *pbss_network;
9527
9528                         if (rtw_end_of_queue_search(phead,plist)== _TRUE)
9529                                 break;
9530
9531                         pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
9532
9533                         plist = get_next(plist);
9534
9535                         pbss_network = (WLAN_BSSID_EX *)&pnetwork->network;
9536
9537                         p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
9538                         if((p==NULL) || (len==0))//non-HT
9539                         {
9540                                 if((pbss_network->Configuration.DSConfig<=0) || (pbss_network->Configuration.DSConfig>14))
9541                                         continue;
9542
9543                                 ICS[0][pbss_network->Configuration.DSConfig]=1;
9544
9545                                 if(ICS[0][0] == 0)
9546                                         ICS[0][0] = 1;
9547                         }
9548
9549                 }
9550
9551                 _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
9552
9553
9554                 for(i= 0;i<8;i++)
9555                 {
9556                         if(ICS[i][0] == 1)
9557                         {
9558                                 int j, k = 0;
9559
9560                                 InfoContent[k] = i;
9561                                 //SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent,i);
9562                                 k++;
9563
9564                                 for(j=1;j<=14;j++)
9565                                 {
9566                                         if(ICS[i][j]==1)
9567                                         {
9568                                                 if(k<16)
9569                                                 {
9570                                                         InfoContent[k] = j; //channel number
9571                                                         //SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j);
9572                                                         k++;
9573                                                 }
9574                                         }
9575                                 }
9576
9577                                 pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &(pattrib->pktlen));
9578
9579                         }
9580
9581                 }
9582
9583
9584         }
9585
9586
9587         pattrib->last_txcmdsz = pattrib->pktlen;
9588
9589         dump_mgntframe(padapter, pmgntframe);
9590 #endif //CONFIG_80211N_HT
9591 }
9592
9593 // Spatial Multiplexing Powersave (SMPS) action frame
9594 int _issue_action_SM_PS(_adapter *padapter ,  unsigned char *raddr , u8 NewMimoPsMode ,  u8 wait_ack)
9595 {
9596
9597         int ret=0;
9598         unsigned char category = RTW_WLAN_CATEGORY_HT;
9599         u8 action = RTW_WLAN_ACTION_HT_SM_PS;
9600         u8 sm_power_control=0;
9601         struct xmit_frame                       *pmgntframe;
9602         struct pkt_attrib                       *pattrib;
9603         unsigned char                                   *pframe;
9604         struct rtw_ieee80211_hdr        *pwlanhdr;
9605         unsigned short                          *fctrl;
9606         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
9607         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
9608         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9609
9610
9611         if(NewMimoPsMode==WLAN_HT_CAP_SM_PS_DISABLED)
9612         {
9613                 sm_power_control = sm_power_control  & ~(BIT(0)); // SM Power Save Enable = 0 SM Power Save Disable
9614         }
9615         else if(NewMimoPsMode==WLAN_HT_CAP_SM_PS_STATIC)
9616         {
9617                 sm_power_control = sm_power_control | BIT(0);    // SM Power Save Enable = 1 SM Power Save Enable
9618                 sm_power_control = sm_power_control & ~(BIT(1)); // SM Mode = 0 Static Mode
9619         }
9620         else if(NewMimoPsMode==WLAN_HT_CAP_SM_PS_DYNAMIC)
9621         {
9622                 sm_power_control = sm_power_control | BIT(0); // SM Power Save Enable = 1 SM Power Save Enable
9623                 sm_power_control = sm_power_control | BIT(1); // SM Mode = 1 Dynamic Mode
9624         }
9625         else
9626                 return ret;
9627
9628         DBG_871X("%s, sm_power_control=%u, NewMimoPsMode=%u\n", __FUNCTION__ , sm_power_control , NewMimoPsMode );
9629
9630         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
9631                 return ret;
9632
9633         //update attribute
9634         pattrib = &pmgntframe->attrib;
9635         update_mgntframe_attrib(padapter, pattrib);
9636
9637         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
9638
9639         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
9640         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
9641
9642         fctrl = &(pwlanhdr->frame_ctl);
9643         *(fctrl) = 0;
9644
9645         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); /* RA */
9646         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); /* TA */
9647         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); /* DA = RA */
9648
9649         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
9650         pmlmeext->mgnt_seq++;
9651         SetFrameSubType(pframe, WIFI_ACTION);
9652
9653         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
9654         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
9655
9656         /* category, action */
9657         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
9658         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
9659
9660         pframe = rtw_set_fixed_ie(pframe, 1, &(sm_power_control), &(pattrib->pktlen));
9661
9662         pattrib->last_txcmdsz = pattrib->pktlen;
9663
9664         if(wait_ack)
9665         {
9666                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
9667         }
9668         else
9669         {
9670                 dump_mgntframe(padapter, pmgntframe);
9671                 ret = _SUCCESS;
9672         }
9673
9674         if (ret != _SUCCESS)
9675             DBG_8192C("%s, ack to\n", __func__);
9676
9677         return ret;
9678 }
9679
9680 int issue_action_SM_PS_wait_ack(_adapter *padapter, unsigned char *raddr, u8 NewMimoPsMode, int try_cnt, int wait_ms)
9681 {
9682         int ret = 0;
9683         int i = 0;
9684         u32 start = rtw_get_current_time();
9685
9686         do {
9687                 ret = _issue_action_SM_PS(padapter, raddr, NewMimoPsMode , wait_ms>0?_TRUE:_FALSE );
9688
9689                 i++;
9690
9691                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
9692                         break;
9693
9694                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
9695                         rtw_msleep_os(wait_ms);
9696
9697         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
9698
9699         if (ret != _FAIL) {
9700                 ret = _SUCCESS;
9701                 #ifndef DBG_XMIT_ACK
9702                 goto exit;
9703                 #endif
9704         }
9705
9706         if (try_cnt && wait_ms) {
9707                 if (raddr)
9708                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", %s , %d/%d in %u ms\n",
9709                                 FUNC_ADPT_ARG(padapter), MAC_ARG(raddr),
9710                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
9711                 else
9712                         DBG_871X(FUNC_ADPT_FMT", %s , %d/%d in %u ms\n",
9713                                 FUNC_ADPT_ARG(padapter),
9714                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
9715         }
9716 exit:
9717
9718         return ret;
9719 }
9720
9721 int issue_action_SM_PS(_adapter *padapter ,  unsigned char *raddr , u8 NewMimoPsMode )
9722 {
9723         DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(raddr));
9724         return _issue_action_SM_PS(padapter, raddr, NewMimoPsMode , _FALSE );
9725 }
9726
9727 /**
9728  * _send_delba_sta_tid - Cancel the AMPDU association for the specific @sta, @tid
9729  * @adapter: the adapter to which @sta belongs
9730  * @initiator: if we are the initiator of AMPDU association
9731  * @sta: the sta to be checked
9732  * @tid: the tid to be checked
9733  * @force: cancel and send DELBA even when no AMPDU association is setup
9734  * @wait_ack: send delba with xmit ack (valid when initiator == 0)
9735  *
9736  * Returns:
9737  * _FAIL if sta is NULL
9738  * when initiator is 1, always _SUCCESS
9739  * when initiator is 0, _SUCCESS if DELBA is acked
9740  */
9741 static unsigned int _send_delba_sta_tid(_adapter *adapter, u8 initiator, struct sta_info *sta, u8 tid
9742         , u8 force, int wait_ack)
9743 {
9744         int ret = _SUCCESS;
9745
9746         if (sta == NULL) {
9747                 ret = _FAIL;
9748                 goto exit;
9749         }
9750
9751         if (initiator == 0) {
9752                 /* recipient */
9753                 if (force || sta->recvreorder_ctrl[tid].enable == _TRUE) {
9754                         u8 ampdu_size_bak = sta->recvreorder_ctrl[tid].ampdu_size;
9755
9756                         sta->recvreorder_ctrl[tid].enable = _FALSE;
9757                         sta->recvreorder_ctrl[tid].ampdu_size = RX_AMPDU_SIZE_INVALID;
9758
9759                         if (rtw_del_rx_ampdu_test_trigger_no_tx_fail())
9760                                 ret = _FAIL;
9761                         else if (wait_ack)
9762                                 ret = issue_del_ba_ex(adapter, sta->hwaddr, tid, 37, initiator, 3, 1);
9763                         else
9764                                 issue_del_ba(adapter, sta->hwaddr, tid, 37, initiator);
9765
9766                         if (ret == _FAIL && sta->recvreorder_ctrl[tid].enable == _FALSE)
9767                                 sta->recvreorder_ctrl[tid].ampdu_size = ampdu_size_bak;
9768                 }
9769         } else if (initiator == 1) {
9770                 /* originator */
9771 #ifdef CONFIG_80211N_HT
9772                 if (force || sta->htpriv.agg_enable_bitmap & BIT(tid)) {
9773                         sta->htpriv.agg_enable_bitmap &= ~BIT(tid);
9774                         sta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
9775                         issue_del_ba(adapter, sta->hwaddr, tid, 37, initiator);
9776                 }
9777 #endif
9778         }
9779
9780 exit:
9781         return ret;
9782 }
9783
9784 inline unsigned int send_delba_sta_tid(_adapter *adapter, u8 initiator, struct sta_info *sta, u8 tid
9785         , u8 force)
9786 {
9787         return _send_delba_sta_tid(adapter, initiator, sta, tid, force, 0);
9788 }
9789
9790 inline unsigned int send_delba_sta_tid_wait_ack(_adapter *adapter, u8 initiator, struct sta_info *sta, u8 tid
9791         , u8 force)
9792 {
9793         return _send_delba_sta_tid(adapter, initiator, sta, tid, force, 1);
9794 }
9795
9796 unsigned int send_delba(_adapter *padapter, u8 initiator, u8 *addr)
9797 {
9798         struct sta_priv *pstapriv = &padapter->stapriv;
9799         struct sta_info *psta = NULL;
9800         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9801         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9802         u16 tid;
9803
9804         if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
9805                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
9806                         return _SUCCESS;
9807
9808         psta = rtw_get_stainfo(pstapriv, addr);
9809         if(psta==NULL)
9810                 return _SUCCESS;
9811
9812         #if 0
9813         DBG_871X("%s:%s\n", __func__, (initiator == 0)?"RX_DIR":"TX_DIR");
9814         if (initiator == 1) /* originator */
9815                 DBG_871X("tx agg_enable_bitmap(0x%08x)\n", psta->htpriv.agg_enable_bitmap);
9816         #endif
9817
9818         for (tid = 0; tid < TID_NUM; tid++)
9819                 send_delba_sta_tid(padapter, initiator, psta, tid, 0);
9820
9821         return _SUCCESS;
9822 }
9823
9824 unsigned int send_beacon(_adapter *padapter)
9825 {
9826         u8      bxmitok = _FALSE;
9827         int     issue=0;
9828         int poll = 0;
9829 //#ifdef CONFIG_CONCURRENT_MODE
9830         //struct mlme_ext_priv  *pmlmeext = &(padapter->mlmeextpriv);
9831         //struct mlme_ext_info  *pmlmeinfo = &(pmlmeext->mlmext_info);
9832         //_adapter *pbuddy_adapter = padapter->pbuddy_adapter;
9833         //struct mlme_priv *pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
9834 //#endif
9835
9836 #ifdef CONFIG_PCI_HCI
9837         //DBG_871X("%s\n", __FUNCTION__);
9838
9839         rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
9840
9841         /* 8192EE Port select for Beacon DL */
9842         rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
9843
9844         issue_beacon(padapter, 0);
9845
9846         return _SUCCESS;
9847 #endif
9848
9849 #if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
9850         u32 start = rtw_get_current_time();
9851
9852         rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
9853         rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
9854         do{
9855                 issue_beacon(padapter, 100);
9856                 issue++;
9857                 do {
9858                         rtw_yield_os();
9859                         rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
9860                         poll++;
9861                 }while((poll%10)!=0 && _FALSE == bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
9862
9863         }while(_FALSE == bxmitok && issue<100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
9864
9865         if(padapter->bSurpriseRemoved || padapter->bDriverStopped)
9866         {
9867                 return _FAIL;
9868         }
9869
9870
9871         if(_FALSE == bxmitok)
9872         {
9873                 DBG_871X("%s fail! %u ms\n", __FUNCTION__, rtw_get_passing_time_ms(start));
9874                 return _FAIL;
9875         }
9876         else
9877         {
9878                 u32 passing_time = rtw_get_passing_time_ms(start);
9879
9880                 if(passing_time > 100 || issue > 3)
9881                         DBG_871X("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start));
9882                 //else
9883                 //      DBG_871X("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start));
9884
9885                 return _SUCCESS;
9886         }
9887
9888 #endif
9889
9890 }
9891
9892 /****************************************************************************
9893
9894 Following are some utitity fuctions for WiFi MLME
9895
9896 *****************************************************************************/
9897
9898 BOOLEAN IsLegal5GChannel(
9899         IN PADAPTER                     Adapter,
9900         IN u8                   channel)
9901 {
9902
9903         int i=0;
9904         u8 Channel_5G[45] = {36,38,40,42,44,46,48,50,52,54,56,58,
9905                 60,62,64,100,102,104,106,108,110,112,114,116,118,120,122,
9906                 124,126,128,130,132,134,136,138,140,149,151,153,155,157,159,
9907                 161,163,165};
9908         for(i=0;i<sizeof(Channel_5G);i++)
9909                 if(channel == Channel_5G[i])
9910                         return _TRUE;
9911         return _FALSE;
9912 }
9913
9914 void site_survey(_adapter *padapter)
9915 {
9916         unsigned char           survey_channel = 0, val8;
9917         RT_SCAN_TYPE    ScanType = SCAN_PASSIVE;
9918         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9919         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9920         u8 initialgain = 0;
9921         u32 channel_scan_time_ms = 0,val32 = 0;
9922
9923 #ifdef CONFIG_P2P
9924
9925 #ifdef CONFIG_CONCURRENT_MODE
9926
9927 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
9928         u8 stay_buddy_ch = 0;
9929 #endif
9930         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
9931         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
9932         struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
9933
9934 #endif //CONFIG_CONCURRENT_MODE
9935         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
9936         static unsigned char  prev_survey_channel = 0;
9937         static unsigned int p2p_scan_count = 0;
9938
9939         if ( ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) || ( pwdinfo->p2p_info.scan_op_ch_only ) )
9940         {
9941                 if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
9942                 {
9943                         survey_channel = pwdinfo->rx_invitereq_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
9944                 }
9945                 else
9946                 {
9947                         survey_channel = pwdinfo->p2p_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
9948                 }
9949                 ScanType = SCAN_ACTIVE;
9950         }
9951         else if(rtw_p2p_findphase_ex_is_social(pwdinfo))
9952         {
9953                 //      Commented by Albert 2011/06/03
9954                 //      The driver is in the find phase, it should go through the social channel.
9955                 int ch_set_idx;
9956                 survey_channel = pwdinfo->social_chan[pmlmeext->sitesurvey_res.channel_idx];
9957                 ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, survey_channel);
9958                 if (ch_set_idx >= 0)
9959                         ScanType = pmlmeext->channel_set[ch_set_idx].ScanType;
9960                 else
9961                         ScanType = SCAN_ACTIVE;
9962         }
9963         else
9964 #endif //CONFIG_P2P
9965         {
9966                 struct rtw_ieee80211_channel *ch;
9967                 if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
9968                         ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
9969                         survey_channel = ch->hw_value;
9970                         ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
9971                 }
9972         }
9973
9974         if (0){
9975 #ifdef CONFIG_P2P
9976                 DBG_871X(FUNC_ADPT_FMT" ch:%u (cnt:%u,idx:%d) at %dms, %c%c%c\n"
9977                 , FUNC_ADPT_ARG(padapter)
9978                 , survey_channel
9979                 , pwdinfo->find_phase_state_exchange_cnt, pmlmeext->sitesurvey_res.channel_idx
9980                 , rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time)
9981                 , ScanType?'A':'P', pmlmeext->sitesurvey_res.scan_mode?'A':'P'
9982                 , pmlmeext->sitesurvey_res.ssid[0].SsidLength?'S':' '
9983                 );
9984 #else
9985                 DBG_871X(FUNC_ADPT_FMT" ch:%u (cnt:%u) at %dms, %c%c%c\n"
9986                 , FUNC_ADPT_ARG(padapter)
9987                 , survey_channel
9988                 , pmlmeext->sitesurvey_res.channel_idx
9989                 , rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time)
9990                 , ScanType?'A':'P', pmlmeext->sitesurvey_res.scan_mode?'A':'P'
9991                 , pmlmeext->sitesurvey_res.ssid[0].SsidLength?'S':' '
9992                 );
9993 #endif // CONFIG_P2P
9994                 #ifdef DBG_FIXED_CHAN
9995                 DBG_871X(FUNC_ADPT_FMT" fixed_chan:%u\n", pmlmeext->fixed_chan);
9996                 #endif
9997         }
9998
9999         if(survey_channel != 0)
10000         {
10001                 //PAUSE 4-AC Queue when site_survey
10002                 //rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
10003                 //val8 |= 0x0f;
10004                 //rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
10005 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
10006                 if ((padapter->pbuddy_adapter->mlmeextpriv.mlmext_info.state&0x03) == WIFI_FW_AP_STATE) {
10007                         if (pmlmeinfo->scan_cnt == RTW_SCAN_NUM_OF_CH) {
10008                                 if (pmlmeinfo->backop_cnt == 0)
10009                                         stay_buddy_ch = 1;
10010                                 else if (pmlmeinfo->backop_cnt == RTW_STAY_AP_CH_MILLISECOND)
10011                                         stay_buddy_ch = 2;
10012
10013                                 if (stay_buddy_ch == 2) {
10014                                         pmlmeinfo->scan_cnt = 1;
10015                                         pmlmeinfo->backop_cnt = 0;
10016                                 } else if (stay_buddy_ch == 1) {
10017                                         pmlmeinfo->backop_cnt++;
10018                                         survey_channel = pbuddy_mlmeext->cur_channel;
10019                                 } else {
10020                                         pmlmeinfo->backop_cnt++;
10021                                         set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
10022                                         return;
10023                                 }
10024                         } else {
10025                                 pmlmeinfo->scan_cnt++;
10026                         }
10027                 } else {
10028                         if (pmlmeinfo->backop_cnt > 0) {
10029                                 stay_buddy_ch = 2;
10030                                 pmlmeinfo->scan_cnt = 1;
10031                                 pmlmeinfo->backop_cnt = 0;
10032                         }
10033                 }
10034 #endif
10035                 if(pmlmeext->sitesurvey_res.channel_idx == 0)
10036                 {
10037 #ifdef DBG_FIXED_CHAN
10038                         if(pmlmeext->fixed_chan !=0xff)
10039                                 set_channel_bwmode(padapter, pmlmeext->fixed_chan, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
10040                         else
10041 #endif
10042                                 set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
10043                 }
10044                 else
10045                 {
10046 #ifdef DBG_FIXED_CHAN
10047                         if(pmlmeext->fixed_chan!=0xff)
10048                                 SelectChannel(padapter, pmlmeext->fixed_chan);
10049                         else
10050 #endif
10051                                 SelectChannel(padapter, survey_channel);
10052                 }
10053
10054 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
10055                 if( stay_buddy_ch == 1 )
10056                 {
10057                         val8 = 0; //survey done
10058                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
10059
10060                         if(check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) &&
10061                                 check_buddy_fwstate(padapter, _FW_LINKED))
10062                         {
10063                                 update_beacon(padapter->pbuddy_adapter, 0, NULL, _TRUE);
10064                         }
10065                 }
10066                 else if( stay_buddy_ch == 2 )
10067                 {
10068                         val8 = 1; //under site survey
10069                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
10070                 }
10071 #endif
10072
10073                 if(ScanType == SCAN_ACTIVE) //obey the channel plan setting...
10074                 {
10075                         #ifdef CONFIG_P2P
10076                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) ||
10077                                 rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)
10078                         )
10079                         {
10080                                 issue_probereq_p2p(padapter, NULL);
10081                                 issue_probereq_p2p(padapter, NULL);
10082                                 issue_probereq_p2p(padapter, NULL);
10083                         }
10084                         else
10085                         #endif //CONFIG_P2P
10086                         {
10087                                 int i;
10088                                 for(i=0;i<RTW_SSID_SCAN_AMOUNT;i++){
10089                                         if(pmlmeext->sitesurvey_res.ssid[i].SsidLength) {
10090                                                 /* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */
10091                                                 if (padapter->registrypriv.wifi_spec)
10092                                                         issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
10093                                                 else
10094                                                         issue_probereq_ex(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL, 0, 0, 0, 0);
10095                                                 issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
10096                                         }
10097                                 }
10098
10099                                 if(pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
10100                                         /* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */
10101                                         if (padapter->registrypriv.wifi_spec)
10102                                                 issue_probereq(padapter, NULL, NULL);
10103                                         else
10104                                                 issue_probereq_ex(padapter, NULL, NULL, 0, 0, 0, 0);
10105                                         issue_probereq(padapter, NULL, NULL);
10106                                 }
10107                         }
10108                 }
10109
10110 #if  defined(CONFIG_ATMEL_RC_PATCH)
10111                 // treat wlan0 & p2p0 in same way, may be changed in near feature.
10112                 // assume home channel is 6, channel switch sequence will be
10113                 //      1,2-6-3,4-6-5,6-6-7,8-6-9,10-6-11,12-6-13,14
10114                 //if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)==_TRUE)
10115
10116                 if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
10117                         channel_scan_time_ms = 20;
10118                 else
10119                         channel_scan_time_ms = 40;
10120 #else
10121                 channel_scan_time_ms = pmlmeext->chan_scan_time;
10122 #endif
10123
10124                 set_survey_timer(pmlmeext, channel_scan_time_ms);
10125 #if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
10126                 {
10127                         struct noise_info info;
10128                         info.bPauseDIG = _FALSE;
10129                         info.IGIValue = 0;
10130                         info.max_time = channel_scan_time_ms/2;//ms
10131                         info.chan = survey_channel;
10132                         rtw_hal_set_odm_var(padapter, HAL_ODM_NOISE_MONITOR,&info, _FALSE);
10133                 }
10134 #endif
10135
10136         }
10137         else
10138         {
10139
10140                 //      channel number is 0 or this channel is not valid.
10141
10142 #ifdef CONFIG_CONCURRENT_MODE
10143                 u8 cur_channel;
10144                 u8 cur_bwmode;
10145                 u8 cur_ch_offset;
10146
10147                 if (rtw_get_ch_setting_union(padapter, &cur_channel, &cur_bwmode, &cur_ch_offset) != 0)
10148                 {
10149                         if (0)
10150                                 DBG_871X(FUNC_ADPT_FMT" back to linked/linking union - ch:%u, bw:%u, offset:%u\n",
10151                                         FUNC_ADPT_ARG(padapter), cur_channel, cur_bwmode, cur_ch_offset);
10152                 }
10153                 #ifdef CONFIG_IOCTL_CFG80211
10154                 else if(padapter->pbuddy_adapter
10155                         && pbuddy_adapter->wdinfo.driver_interface == DRIVER_CFG80211
10156                         && adapter_wdev_data(pbuddy_adapter)->p2p_enabled
10157                         && rtw_p2p_chk_state(&pbuddy_adapter->wdinfo, P2P_STATE_LISTEN)
10158                         )
10159                 {
10160                         cur_channel = pbuddy_adapter->wdinfo.listen_channel;
10161                         cur_bwmode = pbuddy_mlmeext->cur_bwmode;
10162                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
10163                 }
10164                 #endif
10165                 else
10166                 {
10167                         cur_channel = pmlmeext->cur_channel;
10168                         cur_bwmode = pmlmeext->cur_bwmode;
10169                         cur_ch_offset = pmlmeext->cur_ch_offset;
10170                 }
10171 #endif
10172
10173
10174 #ifdef CONFIG_P2P
10175                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
10176                 {
10177                         if( ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) || ( pwdinfo->p2p_info.scan_op_ch_only ) )
10178                         {
10179                                 //      Set the find_phase_state_exchange_cnt to P2P_FINDPHASE_EX_CNT.
10180                                 //      This will let the following flow to run the scanning end.
10181                                 rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
10182                         }
10183                         #ifdef CONFIG_DBG_P2P
10184                         DBG_871X( "[%s] find phase exchange cnt = %d\n", __FUNCTION__, pwdinfo->find_phase_state_exchange_cnt );
10185                         #endif
10186                 }
10187
10188                 if(rtw_p2p_findphase_ex_is_needed(pwdinfo))
10189                 {
10190                         //      Set the P2P State to the listen state of find phase and set the current channel to the listen channel
10191                         set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
10192                         rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN);
10193                         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
10194
10195                         //turn on phy-dynamic functions
10196                         Restore_DM_Func_Flag(padapter);
10197
10198                         initialgain = 0xff; //restore RX GAIN
10199                         rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
10200
10201                         _set_timer( &pwdinfo->find_phase_timer, ( u32 ) ( ( u32 ) ( pwdinfo->listen_dwell ) * 100 ) );
10202                 }
10203                 else
10204 #endif //CONFIG_P2P
10205                 {
10206
10207 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
10208                         pmlmeinfo->scan_cnt = 0;
10209                         pmlmeinfo->backop_cnt = 0;
10210 #endif
10211
10212 #ifdef CONFIG_ANTENNA_DIVERSITY
10213                         // 20100721:Interrupt scan operation here.
10214                         // For SW antenna diversity before link, it needs to switch to another antenna and scan again.
10215                         // It compares the scan result and select beter one to do connection.
10216                         if(rtw_hal_antdiv_before_linked(padapter))
10217                         {
10218                                 pmlmeext->sitesurvey_res.bss_cnt = 0;
10219                                 pmlmeext->sitesurvey_res.channel_idx = -1;
10220                                 pmlmeext->chan_scan_time = SURVEY_TO /2;
10221                                 set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
10222                                 return;
10223                         }
10224 #endif
10225
10226 #ifdef CONFIG_P2P
10227                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
10228                         {
10229                         #ifdef CONFIG_CONCURRENT_MODE
10230                                 if( pwdinfo->driver_interface == DRIVER_WEXT )
10231                                 {
10232                                         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
10233                                         {
10234                                                 _set_timer( &pwdinfo->ap_p2p_switch_timer, 500 );
10235                                         }
10236                                 }
10237                                 rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
10238                         #else
10239                                 rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
10240                         #endif
10241                         }
10242                         rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
10243 #endif //CONFIG_P2P
10244
10245                         pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
10246
10247                         //switch back to the original channel
10248                         //SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset);
10249
10250                         {
10251 #ifdef CONFIG_CONCURRENT_MODE
10252                                 set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
10253 #else
10254
10255 #ifdef CONFIG_P2P
10256                         if( (pwdinfo->driver_interface == DRIVER_WEXT) && (rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN)) )
10257                                 set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
10258                         else
10259 #endif //CONFIG_P2P
10260                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
10261
10262 #endif //CONFIG_CONCURRENT_MODE
10263                         }
10264
10265                         //flush 4-AC Queue after site_survey
10266                         //val8 = 0;
10267                         //rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
10268
10269                         //config MSR
10270                         Set_MSR(padapter, (pmlmeinfo->state & 0x3));
10271
10272                         //turn on phy-dynamic functions
10273                         Restore_DM_Func_Flag(padapter);
10274
10275                         initialgain = 0xff; //restore RX GAIN
10276                         rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
10277
10278
10279                         if (is_client_associated_to_ap(padapter) == _TRUE)
10280                         {
10281                                 issue_nulldata(padapter, NULL, 0, 3, 500);
10282
10283 #ifdef CONFIG_CONCURRENT_MODE
10284                                 if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
10285                                 {
10286                                         DBG_871X("adapter is surveydone(buddy_adapter is linked), issue nulldata(pwrbit=0)\n");
10287
10288                                         issue_nulldata(padapter->pbuddy_adapter, NULL, 0, 3, 500);
10289                                 }
10290 #endif
10291                         }
10292 #ifdef CONFIG_CONCURRENT_MODE
10293                         else if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
10294                         {
10295                                 issue_nulldata(padapter->pbuddy_adapter, NULL, 0, 3, 500);
10296                         }
10297 #endif
10298
10299                         val8 = 0; //survey done
10300                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
10301
10302                         report_surveydone_event(padapter);
10303
10304                         pmlmeext->chan_scan_time = SURVEY_TO;
10305                         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
10306
10307                         issue_action_BSSCoexistPacket(padapter);
10308                         issue_action_BSSCoexistPacket(padapter);
10309                         issue_action_BSSCoexistPacket(padapter);
10310
10311                 }
10312
10313 #ifdef CONFIG_CONCURRENT_MODE
10314                 if(check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) &&
10315                         check_buddy_fwstate(padapter, _FW_LINKED))
10316                 {
10317
10318                         DBG_871X("survey done, current CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
10319
10320                         DBG_871X("restart pbuddy_adapter's beacon\n");
10321
10322                         update_beacon(padapter->pbuddy_adapter, 0, NULL, _TRUE);
10323                 }
10324 #endif
10325
10326         }
10327
10328         return;
10329
10330 }
10331
10332 //collect bss info from Beacon and Probe request/response frames.
10333 u8 collect_bss_info(_adapter *padapter, union recv_frame *precv_frame, WLAN_BSSID_EX *bssid)
10334 {
10335         int     i;
10336         u32     len;
10337         u8      *p;
10338         u16     val16, subtype;
10339         u8      *pframe = precv_frame->u.hdr.rx_data;
10340         u32     packet_len = precv_frame->u.hdr.len;
10341         u8 ie_offset;
10342         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
10343         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10344         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10345
10346         len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
10347
10348         if (len > MAX_IE_SZ)
10349         {
10350                 //DBG_871X("IE too long for survey event\n");
10351                 return _FAIL;
10352         }
10353
10354         _rtw_memset(bssid, 0, sizeof(WLAN_BSSID_EX));
10355
10356         subtype = GetFrameSubType(pframe);
10357
10358         if(subtype==WIFI_BEACON) {
10359                 bssid->Reserved[0] = 1;
10360                 ie_offset = _BEACON_IE_OFFSET_;
10361         } else {
10362                 // FIXME : more type
10363                 if (subtype == WIFI_PROBERSP) {
10364                         ie_offset = _PROBERSP_IE_OFFSET_;
10365                         bssid->Reserved[0] = 3;
10366                 }
10367                 else if (subtype == WIFI_PROBEREQ) {
10368                         ie_offset = _PROBEREQ_IE_OFFSET_;
10369                         bssid->Reserved[0] = 2;
10370                 }
10371                 else {
10372                         bssid->Reserved[0] = 0;
10373                         ie_offset = _FIXED_IE_LENGTH_;
10374                 }
10375         }
10376
10377         bssid->Length = sizeof(WLAN_BSSID_EX) - MAX_IE_SZ + len;
10378
10379         //below is to copy the information element
10380         bssid->IELength = len;
10381         _rtw_memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength);
10382
10383         //get the signal strength
10384         //bssid->Rssi = precv_frame->u.hdr.attrib.SignalStrength; // 0-100 index.
10385         bssid->Rssi = precv_frame->u.hdr.attrib.phy_info.RecvSignalPower; // in dBM.raw data
10386         bssid->PhyInfo.SignalQuality = precv_frame->u.hdr.attrib.phy_info.SignalQuality;//in percentage
10387         bssid->PhyInfo.SignalStrength = precv_frame->u.hdr.attrib.phy_info.SignalStrength;//in percentage
10388 #ifdef CONFIG_ANTENNA_DIVERSITY
10389         //rtw_hal_get_hwreg(padapter, HW_VAR_CURRENT_ANTENNA, (u8 *)(&bssid->PhyInfo.Optimum_antenna));
10390         rtw_hal_get_def_var(padapter, HAL_DEF_CURRENT_ANTENNA,  &bssid->PhyInfo.Optimum_antenna);
10391 #endif
10392
10393         // checking SSID
10394         if ((p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset)) == NULL)
10395         {
10396                 DBG_871X("marc: cannot find SSID for survey event\n");
10397                 return _FAIL;
10398         }
10399
10400         if (*(p + 1))
10401         {
10402                 if (len > NDIS_802_11_LENGTH_SSID)
10403                 {
10404                         DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
10405                         return _FAIL;
10406                 }
10407                 _rtw_memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
10408                 bssid->Ssid.SsidLength = *(p + 1);
10409         }
10410         else
10411         {
10412                 bssid->Ssid.SsidLength = 0;
10413         }
10414
10415         _rtw_memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
10416
10417         //checking rate info...
10418         i = 0;
10419         p = rtw_get_ie(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
10420         if (p != NULL)
10421         {
10422                 if (len > NDIS_802_11_LENGTH_RATES_EX)
10423                 {
10424                         DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
10425                         return _FAIL;
10426                 }
10427                 _rtw_memcpy(bssid->SupportedRates, (p + 2), len);
10428                 i = len;
10429         }
10430
10431         p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
10432         if (p != NULL)
10433         {
10434                 if (len > (NDIS_802_11_LENGTH_RATES_EX-i))
10435                 {
10436                         DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
10437                         return _FAIL;
10438                 }
10439                 _rtw_memcpy(bssid->SupportedRates + i, (p + 2), len);
10440         }
10441
10442         //todo:
10443 #if 0
10444         if (judge_network_type(bssid->SupportedRates, (len + i)) == WIRELESS_11B)
10445         {
10446                 bssid->NetworkTypeInUse = Ndis802_11DS;
10447         }
10448         else
10449 #endif
10450         {
10451                 bssid->NetworkTypeInUse = Ndis802_11OFDM24;
10452         }
10453
10454 #ifdef CONFIG_P2P
10455         if (subtype == WIFI_PROBEREQ)
10456         {
10457                 u8 *p2p_ie;
10458                 u32     p2p_ielen;
10459                 // Set Listion Channel
10460                 if ((p2p_ie = rtw_get_p2p_ie(bssid->IEs, bssid->IELength, NULL, &p2p_ielen)))
10461                 {
10462                         u32     attr_contentlen = 0;
10463                         u8 listen_ch[5] = { 0x00 };
10464
10465                         if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_LISTEN_CH, listen_ch, &attr_contentlen) != NULL)
10466                                 bssid->Configuration.DSConfig = listen_ch[4];
10467                         else
10468                                 return _FALSE; // Intel device maybe no bring Listen Channel
10469                 } else
10470                 { // use current channel
10471                         bssid->Configuration.DSConfig = padapter->mlmeextpriv.cur_channel;
10472                         DBG_871X("%s()-%d: Cannot get p2p_ie. set DSconfig to op_ch(%d)\n", __FUNCTION__, __LINE__, bssid->Configuration.DSConfig);
10473                 }
10474
10475                 // FIXME
10476                 bssid->InfrastructureMode = Ndis802_11Infrastructure;
10477                 _rtw_memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
10478                 bssid->Privacy = 1;
10479                 return _SUCCESS;
10480         }
10481 #endif //CONFIG_P2P
10482
10483         if (bssid->IELength < 12)
10484                 return _FAIL;
10485
10486         // Checking for DSConfig
10487         p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
10488
10489         bssid->Configuration.DSConfig = 0;
10490         bssid->Configuration.Length = 0;
10491
10492         if (p)
10493         {
10494                 bssid->Configuration.DSConfig = *(p + 2);
10495         }
10496         else
10497         {// In 5G, some ap do not have DSSET IE
10498                 // checking HT info for channel
10499                 p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
10500                 if(p)
10501                 {
10502                         struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
10503                         bssid->Configuration.DSConfig = HT_info->primary_channel;
10504                 }
10505                 else
10506                 { // use current channel
10507                         bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter);
10508                 }
10509         }
10510
10511         _rtw_memcpy(&bssid->Configuration.BeaconPeriod, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
10512         bssid->Configuration.BeaconPeriod = le32_to_cpu(bssid->Configuration.BeaconPeriod);
10513
10514         val16 = rtw_get_capability((WLAN_BSSID_EX *)bssid);
10515
10516         if (val16 & BIT(0))
10517         {
10518                 bssid->InfrastructureMode = Ndis802_11Infrastructure;
10519                 _rtw_memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
10520         }
10521         else
10522         {
10523                 bssid->InfrastructureMode = Ndis802_11IBSS;
10524                 _rtw_memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN);
10525         }
10526
10527         if (val16 & BIT(4))
10528                 bssid->Privacy = 1;
10529         else
10530                 bssid->Privacy = 0;
10531
10532         bssid->Configuration.ATIMWindow = 0;
10533
10534         //20/40 BSS Coexistence check
10535         if((pregistrypriv->wifi_spec==1) && (_FALSE == pmlmeinfo->bwmode_updated))
10536         {
10537                 struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
10538 #ifdef CONFIG_80211N_HT
10539                 p = rtw_get_ie(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset);
10540                 if(p && len>0)
10541                 {
10542                         struct HT_caps_element  *pHT_caps;
10543                         pHT_caps = (struct HT_caps_element      *)(p + 2);
10544
10545                         if(pHT_caps->u.HT_cap_element.HT_caps_info&BIT(14))
10546                         {
10547                                 pmlmepriv->num_FortyMHzIntolerant++;
10548                         }
10549                 }
10550                 else
10551                 {
10552                         pmlmepriv->num_sta_no_ht++;
10553                 }
10554 #endif //CONFIG_80211N_HT
10555
10556         }
10557
10558 #ifdef CONFIG_INTEL_WIDI
10559         //process_intel_widi_query_or_tigger(padapter, bssid);
10560         if(process_intel_widi_query_or_tigger(padapter, bssid))
10561         {
10562                 return _FAIL;
10563         }
10564 #endif // CONFIG_INTEL_WIDI
10565
10566         #if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) & 1
10567         if(strcmp(bssid->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
10568                 DBG_871X("Receiving %s("MAC_FMT", DSConfig:%u) from ch%u with ss:%3u, sq:%3u, RawRSSI:%3ld\n"
10569                         , bssid->Ssid.Ssid, MAC_ARG(bssid->MacAddress), bssid->Configuration.DSConfig
10570                         , rtw_get_oper_ch(padapter)
10571                         , bssid->PhyInfo.SignalStrength, bssid->PhyInfo.SignalQuality, bssid->Rssi
10572                 );
10573         }
10574         #endif
10575
10576         // mark bss info receving from nearby channel as SignalQuality 101
10577         if(bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
10578         {
10579                 bssid->PhyInfo.SignalQuality= 101;
10580         }
10581
10582         return _SUCCESS;
10583 }
10584
10585 void start_create_ibss(_adapter* padapter)
10586 {
10587         unsigned short  caps;
10588         u8      val8;
10589         u8      join_type;
10590         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10591         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10592         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
10593         pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
10594         pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
10595
10596         //update wireless mode
10597         update_wireless_mode(padapter);
10598
10599         //udpate capability
10600         caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
10601         update_capinfo(padapter, caps);
10602         if(caps&cap_IBSS)//adhoc master
10603         {
10604                 //set_opmode_cmd(padapter, adhoc);//removed
10605
10606                 val8 = 0xcf;
10607                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
10608
10609                 rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
10610
10611                 //switch channel
10612                 //SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE);
10613                 set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
10614
10615                 beacon_timing_control(padapter);
10616
10617                 //set msr to WIFI_FW_ADHOC_STATE
10618                 pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
10619                 Set_MSR(padapter, (pmlmeinfo->state & 0x3));
10620
10621                 //issue beacon
10622                 if(send_beacon(padapter)==_FAIL)
10623                 {
10624                         RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("issuing beacon frame fail....\n"));
10625
10626                         report_join_res(padapter, -1);
10627                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
10628                 }
10629                 else
10630                 {
10631                         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
10632                         join_type = 0;
10633                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
10634
10635                         report_join_res(padapter, 1);
10636                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
10637                         rtw_indicate_connect(padapter);
10638                 }
10639         }
10640         else
10641         {
10642                 DBG_871X("start_create_ibss, invalid cap:%x\n", caps);
10643                 return;
10644         }
10645         //update bc/mc sta_info
10646         update_bmc_sta(padapter);
10647
10648 }
10649
10650 void start_clnt_join(_adapter* padapter)
10651 {
10652         unsigned short  caps;
10653         u8      val8;
10654         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10655         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10656         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
10657         int beacon_timeout;
10658         u8 ASIX_ID[]= {0x00, 0x0E, 0xC6};
10659
10660         //update wireless mode
10661         update_wireless_mode(padapter);
10662
10663         //udpate capability
10664         caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
10665         update_capinfo(padapter, caps);
10666
10667         //check if sta is ASIX peer and fix IOT issue if it is.
10668         if (_rtw_memcmp(get_my_bssid(&pmlmeinfo->network) ,ASIX_ID ,3)) {
10669                 u8 iot_flag = _TRUE;
10670                 rtw_hal_set_hwreg(padapter, HW_VAR_ASIX_IOT, (u8 *)(&iot_flag));
10671         }
10672
10673         if (caps&cap_ESS)
10674         {
10675                 Set_MSR(padapter, WIFI_FW_STATION_STATE);
10676
10677                 val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf;
10678
10679 #ifdef CONFIG_WAPI_SUPPORT
10680                 if (padapter->wapiInfo.bWapiEnable && pmlmeinfo->auth_algo == dot11AuthAlgrthm_WAPI)
10681                 {
10682                         //Disable TxUseDefaultKey, RxUseDefaultKey, RxBroadcastUseDefaultKey.
10683                         val8 = 0x4c;
10684                 }
10685 #endif
10686                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
10687
10688                 #ifdef CONFIG_DEAUTH_BEFORE_CONNECT
10689                 // Because of AP's not receiving deauth before
10690                 // AP may: 1)not response auth or 2)deauth us after link is complete
10691                 // issue deauth before issuing auth to deal with the situation
10692
10693                 //      Commented by Albert 2012/07/21
10694                 //      For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it.
10695                 {
10696                         #ifdef CONFIG_P2P
10697                         _queue *queue = &(padapter->mlmepriv.scanned_queue);
10698                         _list   *head = get_list_head(queue);
10699                         _list *pos = get_next(head);
10700                         struct wlan_network *scanned = NULL;
10701                         u8 ie_offset = 0;
10702                         _irqL irqL;
10703                         bool has_p2p_ie = _FALSE;
10704
10705                         _enter_critical_bh(&(padapter->mlmepriv.scanned_queue.lock), &irqL);
10706
10707                         for (pos = get_next(head);!rtw_end_of_queue_search(head, pos); pos = get_next(pos)) {
10708
10709                                 scanned = LIST_CONTAINOR(pos, struct wlan_network, list);
10710
10711                                 if (_rtw_memcmp(&(scanned->network.Ssid), &(pnetwork->Ssid), sizeof(NDIS_802_11_SSID)) == _TRUE
10712                                         && _rtw_memcmp(scanned->network.MacAddress, pnetwork->MacAddress, sizeof(NDIS_802_11_MAC_ADDRESS)) == _TRUE
10713                                 ) {
10714                                         ie_offset = (scanned->network.Reserved[0] == 2? 0:12);
10715                                         if (rtw_get_p2p_ie(scanned->network.IEs+ie_offset, scanned->network.IELength-ie_offset, NULL, NULL))
10716                                                 has_p2p_ie = _TRUE;
10717                                         break;
10718                                 }
10719                         }
10720
10721                         _exit_critical_bh(&(padapter->mlmepriv.scanned_queue.lock), &irqL);
10722
10723                         if (scanned == NULL || rtw_end_of_queue_search(head, pos) || has_p2p_ie == _FALSE)
10724                         #endif /* CONFIG_P2P */
10725                                 //To avoid connecting to AP fail during resume process, change retry count from 5 to 1
10726                                 issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
10727                 }
10728                 #endif /* CONFIG_DEAUTH_BEFORE_CONNECT */
10729
10730                 //here wait for receiving the beacon to start auth
10731                 //and enable a timer
10732                 beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
10733                 set_link_timer(pmlmeext, beacon_timeout);
10734                 _set_timer( &padapter->mlmepriv.assoc_timer,
10735                         (REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) +beacon_timeout);
10736
10737                 pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
10738         }
10739         else if (caps&cap_IBSS) //adhoc client
10740         {
10741                 Set_MSR(padapter, WIFI_FW_ADHOC_STATE);
10742
10743                 val8 = 0xcf;
10744                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
10745
10746                 beacon_timing_control(padapter);
10747
10748                 pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
10749
10750                 report_join_res(padapter, 1);
10751         }
10752         else
10753         {
10754                 //DBG_871X("marc: invalid cap:%x\n", caps);
10755                 return;
10756         }
10757
10758 }
10759
10760 void start_clnt_auth(_adapter* padapter)
10761 {
10762         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10763         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10764
10765         _cancel_timer_ex(&pmlmeext->link_timer);
10766
10767         pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
10768         pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
10769
10770         pmlmeinfo->auth_seq = 1;
10771         pmlmeinfo->reauth_count = 0;
10772         pmlmeinfo->reassoc_count = 0;
10773         pmlmeinfo->link_count = 0;
10774         pmlmeext->retry = 0;
10775
10776
10777         DBG_871X_LEVEL(_drv_always_, "start auth\n");
10778         issue_auth(padapter, NULL, 0);
10779
10780         set_link_timer(pmlmeext, REAUTH_TO);
10781
10782 }
10783
10784
10785 void start_clnt_assoc(_adapter* padapter)
10786 {
10787         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10788         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10789
10790         _cancel_timer_ex(&pmlmeext->link_timer);
10791
10792         pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
10793         pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
10794
10795         issue_assocreq(padapter);
10796
10797         set_link_timer(pmlmeext, REASSOC_TO);
10798 }
10799
10800 unsigned int receive_disconnect(_adapter *padapter, unsigned char *MacAddr, unsigned short reason)
10801 {
10802         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10803         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10804
10805         if (!(_rtw_memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
10806                 return _SUCCESS;
10807
10808         DBG_871X("%s\n", __FUNCTION__);
10809
10810         if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
10811         {
10812                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
10813                 {
10814                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
10815                         report_del_sta_event(padapter, MacAddr, reason);
10816
10817                 }
10818                 else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE)
10819                 {
10820                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
10821                         report_join_res(padapter, -2);
10822                 }
10823         }
10824
10825         return _SUCCESS;
10826 }
10827
10828 #ifdef CONFIG_80211D
10829 static void process_80211d(PADAPTER padapter, WLAN_BSSID_EX *bssid)
10830 {
10831         struct registry_priv *pregistrypriv;
10832         struct mlme_ext_priv *pmlmeext;
10833         RT_CHANNEL_INFO *chplan_new;
10834         u8 channel;
10835         u8 i;
10836
10837
10838         pregistrypriv = &padapter->registrypriv;
10839         pmlmeext = &padapter->mlmeextpriv;
10840
10841         // Adjust channel plan by AP Country IE
10842         if (pregistrypriv->enable80211d &&
10843                 (!pmlmeext->update_channel_plan_by_ap_done))
10844         {
10845                 u8 *ie, *p;
10846                 u32 len;
10847                 RT_CHANNEL_PLAN chplan_ap;
10848                 RT_CHANNEL_INFO chplan_sta[MAX_CHANNEL_NUM];
10849                 u8 country[4];
10850                 u8 fcn; // first channel number
10851                 u8 noc; // number of channel
10852                 u8 j, k;
10853
10854                 ie = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
10855                 if (!ie) return;
10856                 if (len < 6) return;
10857
10858                 ie += 2;
10859                 p = ie;
10860                 ie += len;
10861
10862                 _rtw_memset(country, 0, 4);
10863                 _rtw_memcpy(country, p, 3);
10864                 p += 3;
10865                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
10866                                 ("%s: 802.11d country=%s\n", __FUNCTION__, country));
10867
10868                 i = 0;
10869                 while ((ie - p) >= 3)
10870                 {
10871                         fcn = *(p++);
10872                         noc = *(p++);
10873                         p++;
10874
10875                         for (j = 0; j < noc; j++)
10876                         {
10877                                 if (fcn <= 14) channel = fcn + j; // 2.4 GHz
10878                                 else channel = fcn + j*4; // 5 GHz
10879
10880                                 chplan_ap.Channel[i++] = channel;
10881                         }
10882                 }
10883                 chplan_ap.Len = i;
10884
10885 #ifdef CONFIG_DEBUG_RTL871X
10886                 i = 0;
10887                 DBG_871X("%s: AP[%s] channel plan {", __FUNCTION__, bssid->Ssid.Ssid);
10888                 while ((i < chplan_ap.Len) && (chplan_ap.Channel[i] != 0))
10889                 {
10890                         DBG_8192C("%02d,", chplan_ap.Channel[i]);
10891                         i++;
10892                 }
10893                 DBG_871X("}\n");
10894 #endif
10895
10896                 _rtw_memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
10897 #ifdef CONFIG_DEBUG_RTL871X
10898                 i = 0;
10899                 DBG_871X("%s: STA channel plan {", __FUNCTION__);
10900                 while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
10901                 {
10902                         DBG_871X("%02d(%c),", chplan_sta[i].ChannelNum, chplan_sta[i].ScanType==SCAN_PASSIVE?'p':'a');
10903                         i++;
10904                 }
10905                 DBG_871X("}\n");
10906 #endif
10907
10908                 _rtw_memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
10909                 chplan_new = pmlmeext->channel_set;
10910
10911                 i = j = k = 0;
10912                 if (pregistrypriv->wireless_mode & WIRELESS_11G)
10913                 {
10914                         do {
10915                                 if ((i == MAX_CHANNEL_NUM) ||
10916                                         (chplan_sta[i].ChannelNum == 0) ||
10917                                         (chplan_sta[i].ChannelNum > 14))
10918                                         break;
10919
10920                                 if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
10921                                         break;
10922
10923                                 if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j])
10924                                 {
10925                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10926                                         chplan_new[k].ScanType = SCAN_ACTIVE;
10927                                         i++;
10928                                         j++;
10929                                         k++;
10930                                 }
10931                                 else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j])
10932                                 {
10933                                         chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10934 //                                      chplan_new[k].ScanType = chplan_sta[i].ScanType;
10935                                         chplan_new[k].ScanType = SCAN_PASSIVE;
10936                                         i++;
10937                                         k++;
10938                                 }
10939                                 else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j])
10940                                 {
10941                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10942                                         chplan_new[k].ScanType = SCAN_ACTIVE;
10943                                         j++;
10944                                         k++;
10945                                 }
10946                         } while (1);
10947
10948                         // change AP not support channel to Passive scan
10949                         while ((i < MAX_CHANNEL_NUM) &&
10950                                 (chplan_sta[i].ChannelNum != 0) &&
10951                                 (chplan_sta[i].ChannelNum <= 14))
10952                         {
10953                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10954 //                              chplan_new[k].ScanType = chplan_sta[i].ScanType;
10955                                 chplan_new[k].ScanType = SCAN_PASSIVE;
10956                                 i++;
10957                                 k++;
10958                         }
10959
10960                         // add channel AP supported
10961                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
10962                         {
10963                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10964                                 chplan_new[k].ScanType = SCAN_ACTIVE;
10965                                 j++;
10966                                 k++;
10967                         }
10968                 }
10969                 else
10970                 {
10971                         // keep original STA 2.4G channel plan
10972                         while ((i < MAX_CHANNEL_NUM) &&
10973                                 (chplan_sta[i].ChannelNum != 0) &&
10974                                 (chplan_sta[i].ChannelNum <= 14))
10975                         {
10976                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10977                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
10978                                 i++;
10979                                 k++;
10980                         }
10981
10982                         // skip AP 2.4G channel plan
10983                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
10984                         {
10985                                 j++;
10986                         }
10987                 }
10988
10989                 if (pregistrypriv->wireless_mode & WIRELESS_11A)
10990                 {
10991                         do {
10992                                 if ((i >= MAX_CHANNEL_NUM) ||
10993                                         (chplan_sta[i].ChannelNum == 0))
10994                                         break;
10995
10996                                 if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0))
10997                                         break;
10998
10999                                 if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j])
11000                                 {
11001                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
11002                                         chplan_new[k].ScanType = SCAN_ACTIVE;
11003                                         i++;
11004                                         j++;
11005                                         k++;
11006                                 }
11007                                 else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j])
11008                                 {
11009                                         chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
11010 //                                      chplan_new[k].ScanType = chplan_sta[i].ScanType;
11011                                         chplan_new[k].ScanType = SCAN_PASSIVE;
11012                                         i++;
11013                                         k++;
11014                                 }
11015                                 else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j])
11016                                 {
11017                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
11018                                         chplan_new[k].ScanType = SCAN_ACTIVE;
11019                                         j++;
11020                                         k++;
11021                                 }
11022                         } while (1);
11023
11024                         // change AP not support channel to Passive scan
11025                         while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
11026                         {
11027                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
11028 //                              chplan_new[k].ScanType = chplan_sta[i].ScanType;
11029                                 chplan_new[k].ScanType = SCAN_PASSIVE;
11030                                 i++;
11031                                 k++;
11032                         }
11033
11034                         // add channel AP supported
11035                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] != 0))
11036                         {
11037                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
11038                                 chplan_new[k].ScanType = SCAN_ACTIVE;
11039                                 j++;
11040                                 k++;
11041                         }
11042                 }
11043                 else
11044                 {
11045                         // keep original STA 5G channel plan
11046                         while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
11047                         {
11048                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
11049                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
11050                                 i++;
11051                                 k++;
11052                         }
11053                 }
11054
11055                 pmlmeext->update_channel_plan_by_ap_done = 1;
11056
11057 #ifdef CONFIG_DEBUG_RTL871X
11058                 k = 0;
11059                 DBG_871X("%s: new STA channel plan {", __FUNCTION__);
11060                 while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0))
11061                 {
11062                         DBG_871X("%02d(%c),", chplan_new[k].ChannelNum, chplan_new[k].ScanType==SCAN_PASSIVE?'p':'c');
11063                         k++;
11064                 }
11065                 DBG_871X("}\n");
11066 #endif
11067
11068 #if 0
11069                 // recover the right channel index
11070                 channel = chplan_sta[pmlmeext->sitesurvey_res.channel_idx].ChannelNum;
11071                 k = 0;
11072                 while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0))
11073                 {
11074                         if (chplan_new[k].ChannelNum == channel) {
11075                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
11076                                                  ("%s: change mlme_ext sitesurvey channel index from %d to %d\n",
11077                                                   __FUNCTION__, pmlmeext->sitesurvey_res.channel_idx, k));
11078                                 pmlmeext->sitesurvey_res.channel_idx = k;
11079                                 break;
11080                         }
11081                         k++;
11082                 }
11083 #endif
11084         }
11085
11086         // If channel is used by AP, set channel scan type to active
11087         channel = bssid->Configuration.DSConfig;
11088         chplan_new = pmlmeext->channel_set;
11089         i = 0;
11090         while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0))
11091         {
11092                 if (chplan_new[i].ChannelNum == channel)
11093                 {
11094                         if (chplan_new[i].ScanType == SCAN_PASSIVE)
11095                         {
11096                                 //5G Bnad 2, 3 (DFS) doesn't change to active scan
11097                                 if(channel >= 52 && channel <= 144)
11098                                         break;
11099
11100                                 chplan_new[i].ScanType = SCAN_ACTIVE;
11101                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
11102                                                  ("%s: change channel %d scan type from passive to active\n",
11103                                                   __FUNCTION__, channel));
11104                         }
11105                         break;
11106                 }
11107                 i++;
11108         }
11109 }
11110 #endif
11111
11112 /****************************************************************************
11113
11114 Following are the functions to report events
11115
11116 *****************************************************************************/
11117
11118 void report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
11119 {
11120         struct cmd_obj *pcmd_obj;
11121         u8      *pevtcmd;
11122         u32 cmdsz;
11123         struct survey_event     *psurvey_evt;
11124         struct C2HEvent_Header *pc2h_evt_hdr;
11125         struct mlme_ext_priv *pmlmeext;
11126         struct cmd_priv *pcmdpriv;
11127         //u8 *pframe = precv_frame->u.hdr.rx_data;
11128         //uint len = precv_frame->u.hdr.len;
11129
11130         if(!padapter)
11131                 return;
11132
11133         pmlmeext = &padapter->mlmeextpriv;
11134         pcmdpriv = &padapter->cmdpriv;
11135
11136
11137         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
11138         {
11139                 return;
11140         }
11141
11142         cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
11143         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
11144         {
11145                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
11146                 return;
11147         }
11148
11149         _rtw_init_listhead(&pcmd_obj->list);
11150
11151         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
11152         pcmd_obj->cmdsz = cmdsz;
11153         pcmd_obj->parmbuf = pevtcmd;
11154
11155         pcmd_obj->rsp = NULL;
11156         pcmd_obj->rspsz  = 0;
11157
11158         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
11159         pc2h_evt_hdr->len = sizeof(struct survey_event);
11160         pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
11161         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
11162
11163         psurvey_evt = (struct survey_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
11164
11165         if (collect_bss_info(padapter, precv_frame, (WLAN_BSSID_EX *)&psurvey_evt->bss) == _FAIL)
11166         {
11167                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
11168                 rtw_mfree((u8 *)pevtcmd, cmdsz);
11169                 return;
11170         }
11171
11172 #ifdef CONFIG_80211D
11173         process_80211d(padapter, &psurvey_evt->bss);
11174 #endif
11175
11176         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
11177
11178         pmlmeext->sitesurvey_res.bss_cnt++;
11179
11180         return;
11181
11182 }
11183
11184 void report_surveydone_event(_adapter *padapter)
11185 {
11186         struct cmd_obj *pcmd_obj;
11187         u8      *pevtcmd;
11188         u32 cmdsz;
11189         struct surveydone_event *psurveydone_evt;
11190         struct C2HEvent_Header  *pc2h_evt_hdr;
11191         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
11192         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
11193
11194         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
11195         {
11196                 return;
11197         }
11198
11199         cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
11200         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
11201         {
11202                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
11203                 return;
11204         }
11205
11206         _rtw_init_listhead(&pcmd_obj->list);
11207
11208         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
11209         pcmd_obj->cmdsz = cmdsz;
11210         pcmd_obj->parmbuf = pevtcmd;
11211
11212         pcmd_obj->rsp = NULL;
11213         pcmd_obj->rspsz  = 0;
11214
11215         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
11216         pc2h_evt_hdr->len = sizeof(struct surveydone_event);
11217         pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
11218         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
11219
11220         psurveydone_evt = (struct surveydone_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
11221         psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
11222
11223         DBG_871X("survey done event(%x) band:%d for "ADPT_FMT"\n", psurveydone_evt->bss_cnt, padapter->setband, ADPT_ARG(padapter));
11224
11225         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
11226
11227         return;
11228
11229 }
11230
11231 void report_join_res(_adapter *padapter, int res)
11232 {
11233         struct cmd_obj *pcmd_obj;
11234         u8      *pevtcmd;
11235         u32 cmdsz;
11236         struct joinbss_event            *pjoinbss_evt;
11237         struct C2HEvent_Header  *pc2h_evt_hdr;
11238         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
11239         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11240         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
11241
11242         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
11243         {
11244                 return;
11245         }
11246
11247         cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
11248         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
11249         {
11250                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
11251                 return;
11252         }
11253
11254         _rtw_init_listhead(&pcmd_obj->list);
11255
11256         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
11257         pcmd_obj->cmdsz = cmdsz;
11258         pcmd_obj->parmbuf = pevtcmd;
11259
11260         pcmd_obj->rsp = NULL;
11261         pcmd_obj->rspsz  = 0;
11262
11263         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
11264         pc2h_evt_hdr->len = sizeof(struct joinbss_event);
11265         pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
11266         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
11267
11268         pjoinbss_evt = (struct joinbss_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
11269         _rtw_memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
11270         pjoinbss_evt->network.join_res  = pjoinbss_evt->network.aid = res;
11271
11272         DBG_871X("report_join_res(%d)\n", res);
11273
11274
11275         rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network);
11276
11277
11278         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
11279
11280         return;
11281
11282 }
11283
11284 void report_wmm_edca_update(_adapter *padapter)
11285 {
11286         struct cmd_obj *pcmd_obj;
11287         u8      *pevtcmd;
11288         u32 cmdsz;
11289         struct wmm_event                *pwmm_event;
11290         struct C2HEvent_Header  *pc2h_evt_hdr;
11291         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
11292         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11293         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
11294
11295         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
11296         {
11297                 return;
11298         }
11299
11300         cmdsz = (sizeof(struct wmm_event) + sizeof(struct C2HEvent_Header));
11301         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
11302         {
11303                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
11304                 return;
11305         }
11306
11307         _rtw_init_listhead(&pcmd_obj->list);
11308
11309         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
11310         pcmd_obj->cmdsz = cmdsz;
11311         pcmd_obj->parmbuf = pevtcmd;
11312
11313         pcmd_obj->rsp = NULL;
11314         pcmd_obj->rspsz  = 0;
11315
11316         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
11317         pc2h_evt_hdr->len = sizeof(struct wmm_event);
11318         pc2h_evt_hdr->ID = GEN_EVT_CODE(_WMM);
11319         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
11320
11321         pwmm_event = (struct wmm_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
11322         pwmm_event->wmm =0;
11323
11324         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
11325
11326         return;
11327
11328 }
11329
11330 void report_del_sta_event(_adapter *padapter, unsigned char* MacAddr, unsigned short reason)
11331 {
11332         struct cmd_obj *pcmd_obj;
11333         u8      *pevtcmd;
11334         u32 cmdsz;
11335         struct sta_info *psta;
11336         int     mac_id;
11337         struct stadel_event                     *pdel_sta_evt;
11338         struct C2HEvent_Header  *pc2h_evt_hdr;
11339         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
11340         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
11341
11342         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
11343         {
11344                 return;
11345         }
11346
11347         cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
11348         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
11349         {
11350                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
11351                 return;
11352         }
11353
11354         _rtw_init_listhead(&pcmd_obj->list);
11355
11356         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
11357         pcmd_obj->cmdsz = cmdsz;
11358         pcmd_obj->parmbuf = pevtcmd;
11359
11360         pcmd_obj->rsp = NULL;
11361         pcmd_obj->rspsz  = 0;
11362
11363         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
11364         pc2h_evt_hdr->len = sizeof(struct stadel_event);
11365         pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
11366         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
11367
11368         pdel_sta_evt = (struct stadel_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
11369         _rtw_memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
11370         _rtw_memcpy((unsigned char *)(pdel_sta_evt->rsvd),(unsigned char *)(&reason),2);
11371
11372
11373         psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
11374         if(psta)
11375                 mac_id = (int)psta->mac_id;
11376         else
11377                 mac_id = (-1);
11378
11379         pdel_sta_evt->mac_id = mac_id;
11380
11381         DBG_871X("report_del_sta_event: delete STA, mac_id=%d\n", mac_id);
11382
11383         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
11384
11385         return;
11386 }
11387
11388 void report_add_sta_event(_adapter *padapter, unsigned char* MacAddr, int cam_idx)
11389 {
11390         struct cmd_obj *pcmd_obj;
11391         u8      *pevtcmd;
11392         u32 cmdsz;
11393         struct stassoc_event            *padd_sta_evt;
11394         struct C2HEvent_Header  *pc2h_evt_hdr;
11395         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
11396         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
11397
11398         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
11399         {
11400                 return;
11401         }
11402
11403         cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
11404         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
11405         {
11406                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
11407                 return;
11408         }
11409
11410         _rtw_init_listhead(&pcmd_obj->list);
11411
11412         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
11413         pcmd_obj->cmdsz = cmdsz;
11414         pcmd_obj->parmbuf = pevtcmd;
11415
11416         pcmd_obj->rsp = NULL;
11417         pcmd_obj->rspsz  = 0;
11418
11419         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
11420         pc2h_evt_hdr->len = sizeof(struct stassoc_event);
11421         pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
11422         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
11423
11424         padd_sta_evt = (struct stassoc_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
11425         _rtw_memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN);
11426         padd_sta_evt->cam_id = cam_idx;
11427
11428         DBG_871X("report_add_sta_event: add STA\n");
11429
11430         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
11431
11432         return;
11433 }
11434
11435
11436 bool rtw_port_switch_chk(_adapter *adapter)
11437 {
11438         bool switch_needed = _FALSE;
11439 #ifdef CONFIG_CONCURRENT_MODE
11440 #ifdef CONFIG_RUNTIME_PORT_SWITCH
11441         struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
11442         struct pwrctrl_priv *pwrctl = dvobj_to_pwrctl(dvobj);
11443         _adapter *if_port0 = NULL;
11444         _adapter *if_port1 = NULL;
11445         struct mlme_ext_info *if_port0_mlmeinfo = NULL;
11446         struct mlme_ext_info *if_port1_mlmeinfo = NULL;
11447         int i;
11448
11449         for (i = 0; i < dvobj->iface_nums; i++) {
11450                 if (get_iface_type(dvobj->padapters[i]) == IFACE_PORT0) {
11451                         if_port0 = dvobj->padapters[i];
11452                         if_port0_mlmeinfo = &(if_port0->mlmeextpriv.mlmext_info);
11453                 }
11454                 else if (get_iface_type(dvobj->padapters[i]) == IFACE_PORT1) {
11455                         if_port1 = dvobj->padapters[i];
11456                         if_port1_mlmeinfo = &(if_port1->mlmeextpriv.mlmext_info);
11457                 }
11458         }
11459
11460         if (if_port0 == NULL) {
11461                 rtw_warn_on(1);
11462                 goto exit;
11463         }
11464
11465         if (if_port1 == NULL) {
11466                 rtw_warn_on(1);
11467                 goto exit;
11468         }
11469
11470 #ifdef DBG_RUNTIME_PORT_SWITCH
11471         DBG_871X(FUNC_ADPT_FMT" wowlan_mode:%u\n"
11472                 ADPT_FMT", port0, mlmeinfo->state:0x%08x, p2p_state:%d, %d\n"
11473                 ADPT_FMT", port1, mlmeinfo->state:0x%08x, p2p_state:%d, %d\n",
11474                 FUNC_ADPT_ARG(adapter), pwrctl->wowlan_mode,
11475                 ADPT_ARG(if_port0), if_port0_mlmeinfo->state, rtw_p2p_state(&if_port0->wdinfo), rtw_p2p_chk_state(&if_port0->wdinfo, P2P_STATE_NONE),
11476                 ADPT_ARG(if_port1), if_port1_mlmeinfo->state, rtw_p2p_state(&if_port1->wdinfo), rtw_p2p_chk_state(&if_port1->wdinfo, P2P_STATE_NONE));
11477 #endif /* DBG_RUNTIME_PORT_SWITCH */
11478
11479 #ifdef CONFIG_WOWLAN
11480         /* WOWLAN interface(primary, for now) should be port0 */
11481         if (pwrctl->wowlan_mode == _TRUE) {
11482                 if(!is_primary_adapter(if_port0)) {
11483                         DBG_871X("%s "ADPT_FMT" enable WOWLAN\n", __func__, ADPT_ARG(if_port1));
11484                         switch_needed = _TRUE;
11485                 }
11486                 goto exit;
11487         }
11488 #endif /* CONFIG_WOWLAN */
11489
11490         /* AP should use port0 for ctl frame's ack */
11491         if ((if_port1_mlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
11492                 DBG_871X("%s "ADPT_FMT" is AP/GO\n", __func__, ADPT_ARG(if_port1));
11493                 switch_needed = _TRUE;
11494                 goto exit;
11495         }
11496
11497         /* GC should use port0 for p2p ps */
11498         if (((if_port1_mlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE)
11499                 && (if_port1_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
11500                 && !rtw_p2p_chk_state(&if_port1->wdinfo, P2P_STATE_NONE)
11501                 && !check_fwstate(&if_port1->mlmepriv, WIFI_UNDER_WPS)
11502         ) {
11503                 DBG_871X("%s "ADPT_FMT" is GC\n", __func__, ADPT_ARG(if_port1));
11504                 switch_needed = _TRUE;
11505                 goto exit;
11506         }
11507
11508         /* port1 linked, but port0 not linked */
11509         if ((if_port1_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
11510                 && !(if_port0_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
11511                 && ((if_port0_mlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
11512         ) {
11513                 DBG_871X("%s "ADPT_FMT" is SINGLE_LINK\n", __func__, ADPT_ARG(if_port1));
11514                 switch_needed = _TRUE;
11515                 goto exit;
11516         }
11517
11518 exit:
11519 #ifdef DBG_RUNTIME_PORT_SWITCH
11520         DBG_871X(FUNC_ADPT_FMT" ret:%d\n", FUNC_ADPT_ARG(adapter), switch_needed);
11521 #endif /* DBG_RUNTIME_PORT_SWITCH */
11522 #endif /* CONFIG_RUNTIME_PORT_SWITCH */
11523 #endif /* CONFIG_CONCURRENT_MODE */
11524         return switch_needed;
11525 }
11526
11527 /****************************************************************************
11528
11529 Following are the event callback functions
11530
11531 *****************************************************************************/
11532
11533 //for sta/adhoc mode
11534 void update_sta_info(_adapter *padapter, struct sta_info *psta)
11535 {
11536         _irqL   irqL;
11537         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
11538         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11539         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11540
11541         //ERP
11542         VCS_update(padapter, psta);
11543
11544 #ifdef CONFIG_80211N_HT
11545         //HT
11546         if(pmlmepriv->htpriv.ht_option)
11547         {
11548                 psta->htpriv.ht_option = _TRUE;
11549
11550                 psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
11551
11552                 psta->htpriv.rx_ampdu_min_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para&IEEE80211_HT_CAP_AMPDU_DENSITY)>>2;
11553
11554                 if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_20))
11555                         psta->htpriv.sgi_20m = _TRUE;
11556
11557                 if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_40))
11558                         psta->htpriv.sgi_40m = _TRUE;
11559
11560                 psta->qos_option = _TRUE;
11561
11562                 psta->htpriv.ldpc_cap = pmlmepriv->htpriv.ldpc_cap;
11563                 psta->htpriv.stbc_cap = pmlmepriv->htpriv.stbc_cap;
11564                 psta->htpriv.beamform_cap = pmlmepriv->htpriv.beamform_cap;
11565
11566                 _rtw_memcpy(&psta->htpriv.ht_cap, &pmlmeinfo->HT_caps, sizeof(struct rtw_ieee80211_ht_cap));
11567         }
11568         else
11569 #endif //CONFIG_80211N_HT
11570         {
11571 #ifdef CONFIG_80211N_HT
11572                 psta->htpriv.ht_option = _FALSE;
11573
11574                 psta->htpriv.ampdu_enable = _FALSE;
11575
11576                 psta->htpriv.sgi_20m = _FALSE;
11577                 psta->htpriv.sgi_40m = _FALSE;
11578 #endif //CONFIG_80211N_HT
11579                 psta->qos_option = _FALSE;
11580
11581         }
11582
11583 #ifdef CONFIG_80211N_HT
11584         psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
11585
11586         psta->htpriv.agg_enable_bitmap = 0x0;//reset
11587         psta->htpriv.candidate_tid_bitmap = 0x0;//reset
11588 #endif //CONFIG_80211N_HT
11589
11590         psta->bw_mode = pmlmeext->cur_bwmode;
11591
11592         //QoS
11593         if(pmlmepriv->qospriv.qos_option)
11594                 psta->qos_option = _TRUE;
11595
11596 #ifdef CONFIG_80211AC_VHT
11597         _rtw_memcpy(&psta->vhtpriv, &pmlmepriv->vhtpriv, sizeof(struct vht_priv));
11598 #endif //CONFIG_80211AC_VHT
11599
11600         update_ldpc_stbc_cap(psta);
11601
11602         _enter_critical_bh(&psta->lock, &irqL);
11603         psta->state = _FW_LINKED;
11604         _exit_critical_bh(&psta->lock, &irqL);
11605
11606 }
11607
11608 static void rtw_mlmeext_disconnect(_adapter *padapter)
11609 {
11610         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
11611         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11612         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11613         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
11614         u8 state_backup = (pmlmeinfo->state&0x03);
11615         u8 ASIX_ID[]= {0x00, 0x0E, 0xC6};
11616
11617         //set_opmode_cmd(padapter, infra_client_with_mlme);
11618
11619 #if 1
11620         /*
11621          * For safety, prevent from keeping macid sleep.
11622          * If we can sure all power mode enter/leave are paired,
11623          * this check can be removed.
11624          * Lucas@20131113
11625          */
11626         /* wakeup macid after disconnect. */
11627         {
11628                 struct sta_info *psta;
11629                 psta = rtw_get_stainfo(&padapter->stapriv, get_my_bssid(pnetwork));
11630                 if (psta)
11631                         rtw_hal_macid_wakeup(padapter, psta->mac_id);
11632         }
11633 #endif
11634
11635         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
11636         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
11637
11638         //set MSR to no link state -> infra. mode
11639         Set_MSR(padapter, _HW_STATE_STATION_);
11640
11641         //check if sta is ASIX peer and fix IOT issue if it is.
11642         if (_rtw_memcmp(get_my_bssid(&pmlmeinfo->network) ,ASIX_ID ,3)) {
11643                 u8 iot_flag = _FALSE;
11644                 rtw_hal_set_hwreg(padapter, HW_VAR_ASIX_IOT, (u8 *)(&iot_flag));
11645         }
11646         pmlmeinfo->state = WIFI_FW_NULL_STATE;
11647
11648         if(state_backup == WIFI_FW_STATION_STATE)
11649         {
11650                 if (rtw_port_switch_chk(padapter) == _TRUE) {
11651                         rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
11652                         #ifdef CONFIG_LPS
11653                         {
11654                                 _adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter));
11655                                 if (port0_iface)
11656                                         rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0);
11657                         }
11658                         #endif
11659                 }
11660         }
11661
11662 #ifdef CONFIG_CONCURRENT_MODE
11663         if((check_buddy_fwstate(padapter, _FW_LINKED)) != _TRUE)
11664 #endif //CONFIG_CONCURRENT_MODE
11665         {
11666
11667                 //switch to the 20M Hz mode after disconnect
11668                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
11669                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
11670
11671                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
11672         }
11673
11674
11675 #ifdef CONFIG_FCS_MODE
11676         else
11677         {
11678                 PADAPTER pbuddy_adapter;
11679                 struct mlme_ext_priv *pbuddy_mlmeext;
11680
11681                 if(EN_FCS(padapter))
11682                 {
11683                         pbuddy_adapter = padapter->pbuddy_adapter;
11684                         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
11685
11686                         rtw_hal_set_hwreg(padapter, HW_VAR_STOP_FCS_MODE, NULL);
11687
11688                         //switch to buddy's channel setting if buddy is linked
11689                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
11690                 }
11691         }
11692 #endif //!CONFIG_FCS_MODE
11693
11694
11695         flush_all_cam_entry(padapter);
11696
11697         _cancel_timer_ex(&pmlmeext->link_timer);
11698
11699         //pmlmepriv->LinkDetectInfo.TrafficBusyState = _FALSE;
11700         pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
11701         pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
11702
11703 #ifdef CONFIG_TDLS
11704         padapter->tdlsinfo.ap_prohibited = _FALSE;
11705
11706         /* For TDLS channel switch, currently we only allow it to work in wifi logo test mode */
11707         if (padapter->registrypriv.wifi_spec == 1)
11708         {
11709                 padapter->tdlsinfo.ch_switch_prohibited = _FALSE;
11710         }
11711 #endif /* CONFIG_TDLS */
11712
11713 }
11714
11715 void mlmeext_joinbss_event_callback(_adapter *padapter, int join_res)
11716 {
11717         struct sta_info         *psta, *psta_bmc;
11718         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11719         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11720         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
11721         struct sta_priv         *pstapriv = &padapter->stapriv;
11722         u8      join_type;
11723
11724         if(join_res < 0)
11725         {
11726                 join_type = 1;
11727                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
11728                 rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
11729
11730                 goto exit_mlmeext_joinbss_event_callback;
11731         }
11732
11733         if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
11734         {
11735                 //update bc/mc sta_info
11736                 update_bmc_sta(padapter);
11737         }
11738
11739
11740         //turn on dynamic functions
11741         /* Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE); */
11742
11743         // update IOT-releated issue
11744         update_IOT_info(padapter);
11745
11746         rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates);
11747
11748         //BCN interval
11749         rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
11750
11751         //udpate capability
11752         update_capinfo(padapter, pmlmeinfo->capability);
11753
11754         //WMM, Update EDCA param
11755         WMMOnAssocRsp(padapter);
11756
11757         //HT
11758         HTOnAssocRsp(padapter);
11759
11760 #ifdef CONFIG_80211AC_VHT
11761         //VHT
11762         VHTOnAssocRsp(padapter);
11763 #endif
11764
11765 #ifndef CONFIG_CONCURRENT_MODE
11766         //      Call set_channel_bwmode when the CONFIG_CONCURRENT_MODE doesn't be defined.
11767         //Set cur_channel&cur_bwmode&cur_ch_offset
11768         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
11769 #endif
11770
11771         psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
11772         if (psta) //only for infra. mode
11773         {
11774                 pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
11775
11776                 //DBG_871X("set_sta_rate\n");
11777
11778                 psta->wireless_mode = pmlmeext->cur_wireless_mode;
11779
11780                 //set per sta rate after updating HT cap.
11781                 set_sta_rate(padapter, psta);
11782
11783                 rtw_sta_media_status_rpt(padapter, psta, 1);
11784
11785                 /* wakeup macid after join bss successfully to ensure
11786                         the subsequent data frames can be sent out normally */
11787                 rtw_hal_macid_wakeup(padapter, psta->mac_id);
11788         }
11789
11790         if (rtw_port_switch_chk(padapter) == _TRUE)
11791                 rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
11792
11793         join_type = 2;
11794         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
11795
11796         if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
11797         {
11798                 // correcting TSF
11799                 correct_TSF(padapter, pmlmeext);
11800
11801                 //set_link_timer(pmlmeext, DISCONNECT_TO);
11802         }
11803
11804 #ifdef CONFIG_LPS
11805         if(get_iface_type(padapter) == IFACE_PORT0)
11806                 rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
11807 #endif
11808
11809 #ifdef CONFIG_BEAMFORMING
11810         if (psta)
11811                 beamforming_wk_cmd(padapter, BEAMFORMING_CTRL_ENTER, (u8 *)psta, sizeof(struct sta_info), 0);
11812 #endif
11813
11814 exit_mlmeext_joinbss_event_callback:
11815
11816 #ifdef CONFIG_CONCURRENT_MODE
11817         concurrent_chk_joinbss_done(padapter, join_res);
11818 #endif
11819
11820         DBG_871X("=>%s\n", __FUNCTION__);
11821
11822 }
11823
11824 //currently only adhoc mode will go here
11825 void mlmeext_sta_add_event_callback(_adapter *padapter, struct sta_info *psta)
11826 {
11827         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
11828         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11829         u8      join_type;
11830
11831         DBG_871X("%s\n", __FUNCTION__);
11832
11833         if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
11834         {
11835                 if(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)//adhoc master or sta_count>1
11836                 {
11837                         //nothing to do
11838                 }
11839                 else//adhoc client
11840                 {
11841                         //update TSF Value
11842                         //update_TSF(pmlmeext, pframe, len);
11843
11844                         // correcting TSF
11845                         correct_TSF(padapter, pmlmeext);
11846
11847                         //start beacon
11848                         if(send_beacon(padapter)==_FAIL)
11849                         {
11850                                 pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
11851
11852                                 pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
11853
11854                                 return;
11855                         }
11856
11857                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
11858
11859                 }
11860
11861                 join_type = 2;
11862                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
11863         }
11864
11865         pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
11866
11867         psta->bssratelen = rtw_get_rateset_len(pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates);
11868         _rtw_memcpy(psta->bssrateset, pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates, psta->bssratelen);
11869
11870         //update adhoc sta_info
11871         update_sta_info(padapter, psta);
11872
11873         rtw_hal_update_sta_rate_mask(padapter, psta);
11874
11875         // ToDo: HT for Ad-hoc
11876         psta->wireless_mode = rtw_check_network_type(psta->bssrateset, psta->bssratelen, pmlmeext->cur_channel);
11877         psta->raid = rtw_hal_networktype_to_raid(padapter, psta);
11878
11879         //rate radaptive
11880         Update_RA_Entry(padapter, psta);
11881 }
11882
11883 void mlmeext_sta_del_event_callback(_adapter *padapter)
11884 {
11885         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11886         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11887
11888         if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter))
11889         {
11890                 rtw_mlmeext_disconnect(padapter);
11891         }
11892
11893 }
11894
11895 /****************************************************************************
11896
11897 Following are the functions for the timer handlers
11898
11899 *****************************************************************************/
11900 void _linked_info_dump(_adapter *padapter)
11901 {
11902         int i;
11903         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11904         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11905         int UndecoratedSmoothedPWDB = 0;
11906         struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
11907         struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
11908
11909         if(padapter->bLinkInfoDump){
11910
11911                 DBG_871X("\n============["ADPT_FMT"] linked status check ===================\n",ADPT_ARG(padapter));
11912
11913                 if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
11914                 {
11915                         rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
11916
11917                         DBG_871X("AP[" MAC_FMT "] - UndecoratedSmoothedPWDB:%d\n",
11918                                 MAC_ARG(padapter->mlmepriv.cur_network.network.MacAddress),UndecoratedSmoothedPWDB);
11919                 }
11920                 else if((pmlmeinfo->state&0x03) == _HW_STATE_AP_)
11921                 {
11922                         _irqL irqL;
11923                         _list   *phead, *plist;
11924
11925                         struct sta_info *psta=NULL;
11926                         struct sta_priv *pstapriv = &padapter->stapriv;
11927
11928                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
11929                         phead = &pstapriv->asoc_list;
11930                         plist = get_next(phead);
11931                         while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
11932                         {
11933                                 psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
11934                                 plist = get_next(plist);
11935
11936                                 DBG_871X("STA[" MAC_FMT "]:UndecoratedSmoothedPWDB:%d\n",
11937                                         MAC_ARG(psta->hwaddr),psta->rssi_stat.UndecoratedSmoothedPWDB);
11938                         }
11939                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
11940
11941                 }
11942                 for(i=0; i<macid_ctl->num; i++)
11943                 {
11944                         if(rtw_macid_is_used(macid_ctl, i)
11945                                 && !rtw_macid_is_bmc(macid_ctl, i) /* skip bc/mc sta */
11946                         ) {
11947                                 //============  tx info ============
11948                                 rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, &i);
11949                         }
11950                 }
11951                 rtw_hal_set_def_var(padapter, HAL_DEF_DBG_RX_INFO_DUMP, NULL);
11952
11953
11954         }
11955
11956
11957 }
11958
11959 u8 chk_ap_is_alive(_adapter *padapter, struct sta_info *psta)
11960 {
11961         u8 ret = _FALSE;
11962         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11963         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11964
11965         #ifdef DBG_EXPIRATION_CHK
11966         DBG_871X(FUNC_ADPT_FMT" rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu"
11967                                 /*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/
11968                                 ", retry:%u\n"
11969                 , FUNC_ADPT_ARG(padapter)
11970                 , STA_RX_PKTS_DIFF_ARG(psta)
11971                 , psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts
11972                 , psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts
11973                 /*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts
11974                 , psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts
11975                 , psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts
11976                 , pmlmeinfo->bcn_interval*/
11977                 , pmlmeext->retry
11978         );
11979
11980         DBG_871X(FUNC_ADPT_FMT" tx_pkts:%llu, link_count:%u\n", FUNC_ADPT_ARG(padapter)
11981                 , padapter->xmitpriv.tx_pkts
11982                 , pmlmeinfo->link_count
11983         );
11984         #endif
11985
11986         if((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta))
11987                 && sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta)
11988                 && sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta)
11989         )
11990         {
11991                 ret = _FALSE;
11992         }
11993         else
11994         {
11995                 ret = _TRUE;
11996         }
11997
11998         sta_update_last_rx_pkts(psta);
11999
12000         return ret;
12001 }
12002
12003 #ifdef CONFIG_TDLS
12004 void linked_status_chk_tdls(_adapter *padapter)
12005 {
12006 struct candidate_pool{
12007         struct sta_info *psta;
12008         u8 addr[ETH_ALEN];
12009 };
12010         struct sta_priv *pstapriv = &padapter->stapriv;
12011         _irqL irqL;
12012         u8 ack_chk;
12013         struct sta_info *psta;
12014         int i, num_teardown=0, num_checkalive=0;
12015         _list   *plist, *phead;
12016         struct tdls_txmgmt txmgmt;
12017         struct candidate_pool checkalive[NUM_STA];
12018         struct candidate_pool teardown[NUM_STA];
12019 #define ALIVE_MIN 2
12020 #define ALIVE_MAX 5
12021
12022         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
12023         _rtw_memset(checkalive, 0x00, sizeof(checkalive));
12024         _rtw_memset(teardown, 0x00, sizeof(teardown));
12025
12026         if((padapter->tdlsinfo.link_established == _TRUE)){
12027                 _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
12028                 for(i=0; i< NUM_STA; i++)
12029                 {
12030                         phead = &(pstapriv->sta_hash[i]);
12031                         plist = get_next(phead);
12032
12033                         while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
12034                         {
12035                                 psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
12036                                 plist = get_next(plist);
12037
12038                                 if(psta->tdls_sta_state & TDLS_LINKED_STATE)
12039                                 {
12040                                         psta->alive_count++;
12041                                         if(psta->alive_count >= ALIVE_MIN)
12042                                         {
12043 #ifdef CONFIG_XMIT_ACK
12044                                                 if(psta->sta_stats.last_rx_data_pkts >= psta->sta_stats.rx_data_pkts)
12045 #else
12046                                                 if((psta->sta_stats.last_rx_data_pkts >= psta->sta_stats.rx_data_pkts) &&
12047                                                         (!(psta->tdls_sta_state & TDLS_ALIVE_STATE)) )
12048 #endif
12049                                                 {
12050                                                         if(psta->alive_count < ALIVE_MAX)
12051                                                         {
12052                                                                 _rtw_memcpy(checkalive[num_checkalive].addr, psta->hwaddr, ETH_ALEN);
12053                                                                 checkalive[num_checkalive].psta = psta;
12054                                                                 num_checkalive++;
12055                                                         }
12056                                                         else
12057                                                         {
12058                                                                 _rtw_memcpy(teardown[num_teardown].addr, psta->hwaddr, ETH_ALEN);
12059                                                                 teardown[num_teardown].psta = psta;
12060                                                                 num_teardown++;
12061                                                         }
12062                                                 }
12063                                                 else
12064                                                 {
12065                                                         psta->tdls_sta_state &= (~TDLS_ALIVE_STATE);
12066                                                         psta->alive_count = 0;
12067                                                 }
12068                                         }
12069                                         psta->sta_stats.last_rx_data_pkts = psta->sta_stats.rx_data_pkts;
12070                                 }
12071                         }
12072                 }
12073                 _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
12074
12075                 if(num_checkalive > 0)
12076                 {
12077                         for(i=0; i< num_checkalive; i++)
12078                         {
12079 #ifdef CONFIG_XMIT_ACK
12080                                 ack_chk = issue_nulldata_to_TDLS_peer_STA(padapter,     checkalive[i].addr, 0, 3, 500);
12081
12082                                 if(ack_chk == _SUCCESS)
12083                                 {
12084                                         checkalive[i].psta->alive_count = 0;
12085                                 }
12086 #else
12087                                 checkalive[i].psta->tdls_sta_state &= (~TDLS_ALIVE_STATE);
12088                                 _rtw_memcpy(txmgmt.peer, checkalive[i].addr, ETH_ALEN);
12089                                 issue_tdls_dis_req(padapter, &txmgmt);
12090                                 issue_tdls_dis_req(padapter, &txmgmt);
12091                                 issue_tdls_dis_req(padapter, &txmgmt);
12092 #endif //CONFIG_XMIT_ACK
12093                         }
12094                 }
12095
12096                 if(num_teardown > 0)
12097                 {
12098                         for(i=0; i< num_teardown; i++)
12099                         {
12100                                 DBG_871X("[%s %d] Send teardown to "MAC_FMT" \n", __FUNCTION__, __LINE__, MAC_ARG(teardown[i].addr));
12101                                 txmgmt.status_code = _RSON_TDLS_TEAR_TOOFAR_;
12102                                 _rtw_memcpy(txmgmt.peer, teardown[i].addr, ETH_ALEN);
12103                                 issue_tdls_teardown(padapter, &txmgmt, _FALSE);
12104                         }
12105                 }
12106         }
12107
12108 }
12109 #endif //CONFIG_TDLS
12110
12111 //from_timer == 1 means driver is in LPS
12112 void linked_status_chk(_adapter *padapter, u8 from_timer)
12113 {
12114         u32     i;
12115         struct sta_info         *psta;
12116         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
12117         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12118         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12119         struct sta_priv         *pstapriv = &padapter->stapriv;
12120
12121
12122         if (is_client_associated_to_ap(padapter))
12123         {
12124                 //linked infrastructure client mode
12125
12126                 int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
12127                 int rx_chk_limit;
12128                 int link_count_limit;
12129
12130                 #if defined(DBG_ROAMING_TEST)
12131                 rx_chk_limit = 1;
12132                 #elif defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && !defined(CONFIG_LPS_LCLK_WD_TIMER)
12133                 rx_chk_limit = 4;
12134                 #else
12135                 rx_chk_limit = 8;
12136                 #endif
12137 #ifdef CONFIG_P2P
12138                 if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE))
12139                 {
12140                         if(!from_timer)
12141                                 link_count_limit = 3; // 8 sec
12142                         else
12143                                 link_count_limit = 15; // 32 sec
12144                 }
12145                 else
12146 #endif // CONFIG_P2P
12147                 {
12148                         if(!from_timer)
12149                                 link_count_limit = 7; // 16 sec
12150                         else
12151                                 link_count_limit = 29; // 60 sec
12152                 }
12153
12154 #ifdef CONFIG_TDLS
12155 #ifdef CONFIG_TDLS_CH_SW
12156                 if (ATOMIC_READ(&padapter->tdlsinfo.chsw_info.chsw_on) == _TRUE)
12157                         return;
12158 #endif /* CONFIG_TDLS_CH_SW */
12159
12160 #ifdef CONFIG_TDLS_AUTOCHECKALIVE
12161                 linked_status_chk_tdls(padapter);
12162 #endif /* CONFIG_TDLS_AUTOCHECKALIVE */
12163 #endif /* CONFIG_TDLS */
12164
12165                 if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
12166                 {
12167                         bool is_p2p_enable = _FALSE;
12168                         #ifdef CONFIG_P2P
12169                         is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
12170                         #endif
12171
12172                         if (chk_ap_is_alive(padapter, psta) == _FALSE)
12173                                 rx_chk = _FAIL;
12174
12175                         if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
12176                                 tx_chk = _FAIL;
12177
12178                         #if defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && !defined(CONFIG_LPS_LCLK_WD_TIMER)
12179                         if (pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL)) {
12180                                 u8 backup_oper_channel=0;
12181
12182                                 /* switch to correct channel of current network  before issue keep-alive frames */
12183                                 if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
12184                                         backup_oper_channel = rtw_get_oper_ch(padapter);
12185                                         SelectChannel(padapter, pmlmeext->cur_channel);
12186                                 }
12187
12188                                 if (rx_chk != _SUCCESS)
12189                                         issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 0, 0, 3, 1);
12190
12191                                 if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit) || rx_chk != _SUCCESS) {
12192                                         tx_chk = issue_nulldata(padapter, psta->hwaddr, 0, 3, 1);
12193                                         /* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */
12194                                         if (tx_chk == _SUCCESS && !is_p2p_enable)
12195                                                 rx_chk = _SUCCESS;
12196                                 }
12197
12198                                 /* back to the original operation channel */
12199                                 if(backup_oper_channel>0)
12200                                         SelectChannel(padapter, backup_oper_channel);
12201
12202                         }
12203                         else
12204                         #endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */
12205                         {
12206                                 if (rx_chk != _SUCCESS) {
12207                                         if (pmlmeext->retry == 0) {
12208                                                 #ifdef DBG_EXPIRATION_CHK
12209                                                 DBG_871X("issue_probereq to trigger probersp, retry=%d\n", pmlmeext->retry);
12210                                                 #endif
12211                                                 issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
12212                                                 issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
12213                                                 issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
12214                                         }
12215                                 }
12216
12217                                 if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit) {
12218                                         #ifdef DBG_EXPIRATION_CHK
12219                                         DBG_871X("%s issue_nulldata(%d)\n", __FUNCTION__, from_timer?1:0);
12220                                         #endif
12221                                         tx_chk = issue_nulldata_in_interrupt(padapter, NULL, from_timer?1:0);
12222                                 }
12223                         }
12224
12225                         if (rx_chk == _FAIL) {
12226                                 pmlmeext->retry++;
12227                                 if (pmlmeext->retry > rx_chk_limit) {
12228                                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n",
12229                                                 FUNC_ADPT_ARG(padapter));
12230                                         receive_disconnect(padapter, pmlmeinfo->network.MacAddress
12231                                                 , WLAN_REASON_EXPIRATION_CHK);
12232                                         return;
12233                                 }
12234                         } else {
12235                                 pmlmeext->retry = 0;
12236                         }
12237
12238                         if (tx_chk == _FAIL) {
12239                                 pmlmeinfo->link_count %= (link_count_limit+1);
12240                         } else {
12241                                 pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
12242                                 pmlmeinfo->link_count = 0;
12243                         }
12244
12245                 } //end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL)
12246
12247         }
12248         else if (is_client_associated_to_ibss(padapter))
12249         {
12250                 //linked IBSS mode
12251                 //for each assoc list entry to check the rx pkt counter
12252                 for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
12253                 {
12254                         if (pmlmeinfo->FW_sta_info[i].status == 1)
12255                         {
12256                                 psta = pmlmeinfo->FW_sta_info[i].psta;
12257
12258                                 if(NULL==psta) continue;
12259
12260                                 if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta))
12261                                 {
12262
12263                                         if(pmlmeinfo->FW_sta_info[i].retry<3)
12264                                         {
12265                                                 pmlmeinfo->FW_sta_info[i].retry++;
12266                                         }
12267                                         else
12268                                         {
12269                                                 pmlmeinfo->FW_sta_info[i].retry = 0;
12270                                                 pmlmeinfo->FW_sta_info[i].status = 0;
12271                                                 report_del_sta_event(padapter, psta->hwaddr
12272                                                         , 65535// indicate disconnect caused by no rx
12273                                                 );
12274                                         }
12275                                 }
12276                                 else
12277                                 {
12278                                         pmlmeinfo->FW_sta_info[i].retry = 0;
12279                                         pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
12280                                 }
12281                         }
12282                 }
12283
12284                 //set_link_timer(pmlmeext, DISCONNECT_TO);
12285
12286         }
12287
12288 }
12289
12290 void survey_timer_hdl(_adapter *padapter)
12291 {
12292         struct cmd_obj  *ph2c;
12293         struct sitesurvey_parm  *psurveyPara;
12294         struct cmd_priv                                 *pcmdpriv=&padapter->cmdpriv;
12295         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
12296 #ifdef CONFIG_P2P
12297         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
12298 #endif
12299
12300         //DBG_871X("marc: survey timer\n");
12301
12302         //issue rtw_sitesurvey_cmd
12303         if (pmlmeext->sitesurvey_res.state > SCAN_START)
12304         {
12305                 if(pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
12306                 {
12307 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
12308                         if (padapter->mlmeextpriv.mlmext_info.scan_cnt != RTW_SCAN_NUM_OF_CH
12309                                 || padapter->mlmeextpriv.mlmext_info.backop_cnt == RTW_STAY_AP_CH_MILLISECOND)
12310 #endif
12311                                 pmlmeext->sitesurvey_res.channel_idx++;
12312                 }
12313
12314                 if(pmlmeext->scan_abort == _TRUE)
12315                 {
12316                         #ifdef CONFIG_P2P
12317                         if(!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE))
12318                         {
12319                                 rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
12320                                 pmlmeext->sitesurvey_res.channel_idx = 3;
12321                                 DBG_871X("%s idx:%d, cnt:%u\n", __FUNCTION__
12322                                         , pmlmeext->sitesurvey_res.channel_idx
12323                                         , pwdinfo->find_phase_state_exchange_cnt
12324                                 );
12325                         }
12326                         else
12327                         #endif
12328                         {
12329                                 pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
12330                                 DBG_871X("%s idx:%d\n", __FUNCTION__
12331                                         , pmlmeext->sitesurvey_res.channel_idx
12332                                 );
12333                         }
12334
12335                         pmlmeext->scan_abort = _FALSE;//reset
12336                 }
12337
12338                 if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
12339                 {
12340                         goto exit_survey_timer_hdl;
12341                 }
12342
12343                 if ((psurveyPara = (struct sitesurvey_parm*)rtw_zmalloc(sizeof(struct sitesurvey_parm))) == NULL)
12344                 {
12345                         rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
12346                         goto exit_survey_timer_hdl;
12347                 }
12348
12349                 init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
12350                 rtw_enqueue_cmd(pcmdpriv, ph2c);
12351         }
12352
12353
12354 exit_survey_timer_hdl:
12355
12356         return;
12357 }
12358
12359 void link_timer_hdl(_adapter *padapter)
12360 {
12361         //static unsigned int           rx_pkt = 0;
12362         //static u64                            tx_cnt = 0;
12363         //struct xmit_priv              *pxmitpriv = &(padapter->xmitpriv);
12364         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12365         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12366         //struct sta_priv               *pstapriv = &padapter->stapriv;
12367
12368
12369         if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
12370         {
12371                 DBG_871X("link_timer_hdl:no beacon while connecting\n");
12372                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
12373                 report_join_res(padapter, -3);
12374         }
12375         else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE)
12376         {
12377                 //re-auth timer
12378                 if (++pmlmeinfo->reauth_count > REAUTH_LIMIT)
12379                 {
12380                         //if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto)
12381                         //{
12382                                 pmlmeinfo->state = 0;
12383                                 report_join_res(padapter, -1);
12384                                 return;
12385                         //}
12386                         //else
12387                         //{
12388                         //      pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
12389                         //      pmlmeinfo->reauth_count = 0;
12390                         //}
12391                 }
12392
12393                 DBG_871X("link_timer_hdl: auth timeout and try again\n");
12394                 pmlmeinfo->auth_seq = 1;
12395                 issue_auth(padapter, NULL, 0);
12396                 set_link_timer(pmlmeext, REAUTH_TO);
12397         }
12398         else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)
12399         {
12400                 //re-assoc timer
12401                 if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT)
12402                 {
12403                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
12404                         report_join_res(padapter, -2);
12405                         return;
12406                 }
12407
12408                 DBG_871X("link_timer_hdl: assoc timeout and try again\n");
12409                 issue_assocreq(padapter);
12410                 set_link_timer(pmlmeext, REASSOC_TO);
12411         }
12412 #if 0
12413         else if (is_client_associated_to_ap(padapter))
12414         {
12415                 //linked infrastructure client mode
12416                 if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
12417                 {
12418                         /*to monitor whether the AP is alive or not*/
12419                         if (rx_pkt == psta->sta_stats.rx_pkts)
12420                         {
12421                                 receive_disconnect(padapter, pmlmeinfo->network.MacAddress);
12422                                 return;
12423                         }
12424                         else
12425                         {
12426                                 rx_pkt = psta->sta_stats.rx_pkts;
12427                                 set_link_timer(pmlmeext, DISCONNECT_TO);
12428                         }
12429
12430                         //update the EDCA paramter according to the Tx/RX mode
12431                         update_EDCA_param(padapter);
12432
12433                         /*to send the AP a nulldata if no frame is xmitted in order to keep alive*/
12434                         if (pmlmeinfo->link_count++ == 0)
12435                         {
12436                                 tx_cnt = pxmitpriv->tx_pkts;
12437                         }
12438                         else if ((pmlmeinfo->link_count & 0xf) == 0)
12439                         {
12440                                 if (tx_cnt == pxmitpriv->tx_pkts)
12441                                 {
12442                                         issue_nulldata_in_interrupt(padapter, NULL, 0);
12443                                 }
12444
12445                                 tx_cnt = pxmitpriv->tx_pkts;
12446                         }
12447                 } //end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL)
12448         }
12449         else if (is_client_associated_to_ibss(padapter))
12450         {
12451                 //linked IBSS mode
12452                 //for each assoc list entry to check the rx pkt counter
12453                 for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
12454                 {
12455                         if (pmlmeinfo->FW_sta_info[i].status == 1)
12456                         {
12457                                 psta = pmlmeinfo->FW_sta_info[i].psta;
12458
12459                                 if (pmlmeinfo->FW_sta_info[i].rx_pkt == psta->sta_stats.rx_pkts)
12460                                 {
12461                                         pmlmeinfo->FW_sta_info[i].status = 0;
12462                                         report_del_sta_event(padapter, psta->hwaddr);
12463                                 }
12464                                 else
12465                                 {
12466                                         pmlmeinfo->FW_sta_info[i].rx_pkt = psta->sta_stats.rx_pkts;
12467                                 }
12468                         }
12469                 }
12470
12471                 set_link_timer(pmlmeext, DISCONNECT_TO);
12472         }
12473 #endif
12474
12475         return;
12476 }
12477
12478 void addba_timer_hdl(struct sta_info *psta)
12479 {
12480 #ifdef CONFIG_80211N_HT
12481         struct ht_priv  *phtpriv;
12482
12483         if(!psta)
12484                 return;
12485
12486         phtpriv = &psta->htpriv;
12487
12488         if((phtpriv->ht_option==_TRUE) && (phtpriv->ampdu_enable==_TRUE))
12489         {
12490                 if(phtpriv->candidate_tid_bitmap)
12491                         phtpriv->candidate_tid_bitmap=0x0;
12492
12493         }
12494 #endif //CONFIG_80211N_HT
12495 }
12496
12497 #ifdef CONFIG_IEEE80211W
12498 void sa_query_timer_hdl(_adapter *padapter)
12499 {
12500         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12501         struct mlme_priv * pmlmepriv = &padapter->mlmepriv;
12502         _irqL irqL;
12503         //disconnect
12504         _enter_critical_bh(&pmlmepriv->lock, &irqL);
12505
12506         if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
12507         {
12508                 rtw_disassoc_cmd(padapter, 0, _TRUE);
12509                 rtw_indicate_disconnect(padapter);
12510                 rtw_free_assoc_resources(padapter, 1);
12511         }
12512
12513         _exit_critical_bh(&pmlmepriv->lock, &irqL);
12514         DBG_871X("SA query timeout disconnect\n");
12515 }
12516 #endif //CONFIG_IEEE80211W
12517
12518 u8 NULL_hdl(_adapter *padapter, u8 *pbuf)
12519 {
12520         return H2C_SUCCESS;
12521 }
12522
12523 #ifdef CONFIG_AUTO_AP_MODE
12524 void rtw_start_auto_ap(_adapter *adapter)
12525 {
12526         DBG_871X("%s\n", __FUNCTION__);
12527
12528         rtw_set_802_11_infrastructure_mode(adapter, Ndis802_11APMode);
12529
12530         rtw_setopmode_cmd(adapter, Ndis802_11APMode,_TRUE);
12531 }
12532
12533 static int rtw_auto_ap_start_beacon(_adapter *adapter)
12534 {
12535         int ret=0;
12536         u8 *pbuf = NULL;
12537         uint len;
12538         u8      supportRate[16];
12539         int     sz = 0, rateLen;
12540         u8 *    ie;
12541         u8      wireless_mode, oper_channel;
12542         u8 ssid[3] = {0}; //hidden ssid
12543         u32 ssid_len = sizeof(ssid);
12544         struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
12545
12546
12547         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) != _TRUE)
12548                 return -EINVAL;
12549
12550
12551         len = 128;
12552         pbuf = rtw_zmalloc(len);
12553         if(!pbuf)
12554                 return -ENOMEM;
12555
12556
12557         //generate beacon
12558         ie = pbuf;
12559
12560         //timestamp will be inserted by hardware
12561         sz += 8;
12562         ie += sz;
12563
12564         //beacon interval : 2bytes
12565         *(u16*)ie = cpu_to_le16((u16)100);//BCN_INTERVAL=100;
12566         sz += 2;
12567         ie += 2;
12568
12569         //capability info
12570         *(u16*)ie = 0;
12571         *(u16*)ie |= cpu_to_le16(cap_ESS);
12572         *(u16*)ie |= cpu_to_le16(cap_ShortPremble);
12573         //*(u16*)ie |= cpu_to_le16(cap_Privacy);
12574         sz += 2;
12575         ie += 2;
12576
12577         //SSID
12578         ie = rtw_set_ie(ie, _SSID_IE_, ssid_len, ssid, &sz);
12579
12580         //supported rates
12581         wireless_mode = WIRELESS_11BG_24N;
12582         rtw_set_supported_rate(supportRate, wireless_mode) ;
12583         rateLen = rtw_get_rateset_len(supportRate);
12584         if (rateLen > 8)
12585         {
12586                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, supportRate, &sz);
12587         }
12588         else
12589         {
12590                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, supportRate, &sz);
12591         }
12592
12593
12594         //DS parameter set
12595         if(check_buddy_fwstate(adapter, _FW_LINKED) &&
12596                 check_buddy_fwstate(adapter, WIFI_STATION_STATE))
12597         {
12598                 PADAPTER pbuddy_adapter = adapter->pbuddy_adapter;
12599                 struct mlme_ext_priv *pbuddy_mlmeext  = &pbuddy_adapter->mlmeextpriv;
12600
12601                 oper_channel = pbuddy_mlmeext->cur_channel;
12602         }
12603         else
12604         {
12605                 oper_channel = adapter_to_dvobj(adapter)->oper_channel;
12606         }
12607         ie = rtw_set_ie(ie, _DSSET_IE_, 1, &oper_channel, &sz);
12608
12609         //ext supported rates
12610         if (rateLen > 8)
12611         {
12612                 ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (supportRate + 8), &sz);
12613         }
12614
12615         DBG_871X("%s, start auto ap beacon sz=%d\n", __FUNCTION__, sz);
12616
12617         //lunch ap mode & start to issue beacon
12618         if(rtw_check_beacon_data(adapter, pbuf,  sz) == _SUCCESS)
12619         {
12620
12621         }
12622         else
12623         {
12624                 ret = -EINVAL;
12625         }
12626
12627
12628         rtw_mfree(pbuf, len);
12629
12630         return ret;
12631
12632 }
12633 #endif//CONFIG_AUTO_AP_MODE
12634
12635 u8 setopmode_hdl(_adapter *padapter, u8 *pbuf)
12636 {
12637         u8      type;
12638         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12639         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12640         struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
12641
12642         if(psetop->mode == Ndis802_11APMode)
12643         {
12644                 pmlmeinfo->state = WIFI_FW_AP_STATE;
12645                 type = _HW_STATE_AP_;
12646 #ifdef CONFIG_NATIVEAP_MLME
12647                 //start_ap_mode(padapter);
12648 #endif
12649         }
12650         else if(psetop->mode == Ndis802_11Infrastructure)
12651         {
12652                 pmlmeinfo->state &= ~(BIT(0)|BIT(1));// clear state
12653                 pmlmeinfo->state |= WIFI_FW_STATION_STATE;//set to      STATION_STATE
12654                 type = _HW_STATE_STATION_;
12655         }
12656         else if(psetop->mode == Ndis802_11IBSS)
12657         {
12658                 type = _HW_STATE_ADHOC_;
12659         } else if (psetop->mode == Ndis802_11Monitor) {
12660                 type = _HW_STATE_MONITOR_;
12661         }
12662         else
12663         {
12664                 type = _HW_STATE_NOLINK_;
12665         }
12666
12667         rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
12668         //Set_NETYPE0_MSR(padapter, type);
12669
12670
12671 #ifdef CONFIG_AUTO_AP_MODE
12672         if(psetop->mode == Ndis802_11APMode)
12673                 rtw_auto_ap_start_beacon(padapter);
12674 #endif
12675
12676         if (rtw_port_switch_chk(padapter) == _TRUE)
12677         {
12678                 rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
12679
12680                 if(psetop->mode == Ndis802_11APMode)
12681                         adapter_to_pwrctl(padapter)->fw_psmode_iface_id = 0xff; //ap mode won't dowload rsvd pages
12682                 else if (psetop->mode == Ndis802_11Infrastructure) {
12683                         #ifdef CONFIG_LPS
12684                         _adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter));
12685                         if (port0_iface)
12686                                 rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0);
12687                         #endif
12688                 }
12689         }
12690
12691 #ifdef CONFIG_BT_COEXIST
12692         if (psetop->mode == Ndis802_11APMode)
12693         {
12694                 // Do this after port switch to
12695                 // prevent from downloading rsvd page to wrong port
12696                 rtw_btcoex_MediaStatusNotify(padapter, 1); //connect
12697         }
12698 #endif // CONFIG_BT_COEXIST
12699
12700         return H2C_SUCCESS;
12701
12702 }
12703
12704 u8 createbss_hdl(_adapter *padapter, u8 *pbuf)
12705 {
12706         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12707         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12708         WLAN_BSSID_EX   *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
12709         struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
12710         //u8    initialgain;
12711
12712 #ifdef CONFIG_AP_MODE
12713         if (pmlmeinfo->state == WIFI_FW_AP_STATE) {
12714                 WLAN_BSSID_EX *network = &padapter->mlmepriv.cur_network.network;
12715                 start_bss_network(padapter, (u8*)network);
12716                 return H2C_SUCCESS;
12717         }
12718 #endif
12719
12720         //below is for ad-hoc master
12721         if(pparm->network.InfrastructureMode == Ndis802_11IBSS)
12722         {
12723                 rtw_joinbss_reset(padapter);
12724
12725                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
12726                 pmlmeext->cur_ch_offset= HAL_PRIME_CHNL_OFFSET_DONT_CARE;
12727                 pmlmeinfo->ERP_enable = 0;
12728                 pmlmeinfo->WMM_enable = 0;
12729                 pmlmeinfo->HT_enable = 0;
12730                 pmlmeinfo->HT_caps_enable = 0;
12731                 pmlmeinfo->HT_info_enable = 0;
12732                 pmlmeinfo->agg_enable_bitmap = 0;
12733                 pmlmeinfo->candidate_tid_bitmap = 0;
12734
12735                 //config the initial gain under linking, need to write the BB registers
12736                 //initialgain = 0x1E;
12737                 //rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
12738
12739                 //disable dynamic functions, such as high power, DIG
12740                 Save_DM_Func_Flag(padapter);
12741                 Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
12742
12743                 //cancel link timer
12744                 _cancel_timer_ex(&pmlmeext->link_timer);
12745
12746                 //clear CAM
12747                 flush_all_cam_entry(padapter);
12748
12749                 _rtw_memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength));
12750                 pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength;
12751
12752                 if(pnetwork->IELength>MAX_IE_SZ)//Check pbuf->IELength
12753                         return H2C_PARAMETERS_ERROR;
12754
12755                 _rtw_memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength);
12756
12757                 start_create_ibss(padapter);
12758
12759         }
12760
12761         return H2C_SUCCESS;
12762
12763 }
12764
12765 u8 join_cmd_hdl(_adapter *padapter, u8 *pbuf)
12766 {
12767         u8      join_type;
12768         PNDIS_802_11_VARIABLE_IEs       pIE;
12769         struct registry_priv    *pregpriv = &padapter->registrypriv;
12770         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12771         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12772         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
12773 #ifdef CONFIG_ANTENNA_DIVERSITY
12774         struct joinbss_parm     *pparm = (struct joinbss_parm *)pbuf;
12775 #endif //CONFIG_ANTENNA_DIVERSITY
12776         u32 i;
12777         u8      cbw40_enable=0;
12778         //u8    initialgain;
12779         //u32   acparm;
12780         u8 ch, bw, offset;
12781
12782         //check already connecting to AP or not
12783         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
12784         {
12785                 if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
12786                 {
12787                         issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
12788                 }
12789                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
12790
12791                 //clear CAM
12792                 flush_all_cam_entry(padapter);
12793
12794                 _cancel_timer_ex(&pmlmeext->link_timer);
12795
12796                 //set MSR to nolink -> infra. mode
12797                 //Set_MSR(padapter, _HW_STATE_NOLINK_);
12798                 Set_MSR(padapter, _HW_STATE_STATION_);
12799
12800
12801                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
12802         }
12803
12804 #ifdef CONFIG_ANTENNA_DIVERSITY
12805         rtw_antenna_select_cmd(padapter, pparm->network.PhyInfo.Optimum_antenna, _FALSE);
12806 #endif
12807
12808 #ifdef CONFIG_WAPI_SUPPORT
12809         rtw_wapi_clear_all_cam_entry(padapter);
12810 #endif
12811
12812         rtw_joinbss_reset(padapter);
12813
12814         pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
12815         pmlmeext->cur_ch_offset= HAL_PRIME_CHNL_OFFSET_DONT_CARE;
12816         pmlmeinfo->ERP_enable = 0;
12817         pmlmeinfo->WMM_enable = 0;
12818         pmlmeinfo->HT_enable = 0;
12819         pmlmeinfo->HT_caps_enable = 0;
12820         pmlmeinfo->HT_info_enable = 0;
12821         pmlmeinfo->agg_enable_bitmap = 0;
12822         pmlmeinfo->candidate_tid_bitmap = 0;
12823         pmlmeinfo->bwmode_updated = _FALSE;
12824         //pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX;
12825         pmlmeinfo->VHT_enable = 0;
12826
12827         _rtw_memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength));
12828         pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength;
12829
12830         if(pnetwork->IELength>MAX_IE_SZ)//Check pbuf->IELength
12831                 return H2C_PARAMETERS_ERROR;
12832
12833         if (pnetwork->IELength < 2) {
12834                 report_join_res(padapter, (-4));
12835                 return H2C_SUCCESS;
12836         }
12837         _rtw_memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength);
12838
12839         pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
12840         pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
12841
12842         //Check AP vendor to move rtw_joinbss_cmd()
12843         //pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->IEs, pnetwork->IELength);
12844
12845         //sizeof(NDIS_802_11_FIXED_IEs)
12846         for (i = _FIXED_IE_LENGTH_ ; i < pnetwork->IELength - 2 ;) {
12847                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pnetwork->IEs + i);
12848
12849                 switch (pIE->ElementID)
12850                 {
12851                         case _VENDOR_SPECIFIC_IE_://Get WMM IE.
12852                                 if ( _rtw_memcmp(pIE->data, WMM_OUI, 4) )
12853                                 {
12854                                         WMM_param_handler(padapter, pIE);
12855                                 }
12856                                 break;
12857
12858                         case _HT_CAPABILITY_IE_:        //Get HT Cap IE.
12859                                 pmlmeinfo->HT_caps_enable = 1;
12860                                 break;
12861
12862                         case _HT_EXTRA_INFO_IE_:        //Get HT Info IE.
12863 #ifdef CONFIG_80211N_HT
12864                                 pmlmeinfo->HT_info_enable = 1;
12865
12866                                 //spec case only for cisco's ap because cisco's ap issue assoc rsp using mcs rate @40MHz or @20MHz
12867                                 {
12868                                         struct HT_info_element *pht_info = (struct HT_info_element *)(pIE->data);
12869
12870                                         if (pnetwork->Configuration.DSConfig > 14) {
12871                                                 if ((pregpriv->bw_mode >> 4) > CHANNEL_WIDTH_20)
12872                                                         cbw40_enable = 1;
12873                                         } else {
12874                                                 if ((pregpriv->bw_mode & 0x0f) > CHANNEL_WIDTH_20)
12875                                                         cbw40_enable = 1;
12876                                         }
12877
12878                                         if ((cbw40_enable) && (pht_info->infos[0] & BIT(2)))
12879                                         {
12880                                                 //switch to the 40M Hz mode according to the AP
12881                                                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_40;
12882                                                 switch (pht_info->infos[0] & 0x3)
12883                                                 {
12884                                                         case 1:
12885                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
12886                                                                 break;
12887
12888                                                         case 3:
12889                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
12890                                                                 break;
12891
12892                                                         default:
12893                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
12894                                                                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
12895                                                                 break;
12896                                                 }
12897
12898                                                 DBG_871X("set HT ch/bw before connected\n");
12899                                         }
12900                                 }
12901 #endif //CONFIG_80211N_HT
12902                                 break;
12903 #ifdef CONFIG_80211AC_VHT
12904                         case EID_VHTCapability://Get VHT Cap IE.
12905                                 pmlmeinfo->VHT_enable = 1;
12906                                 break;
12907
12908                         case EID_VHTOperation://Get VHT Operation IE.
12909                                 if((GET_VHT_OPERATION_ELE_CHL_WIDTH(pIE->data) >= 1)
12910                                         && ((pregpriv->bw_mode >> 4) >= CHANNEL_WIDTH_80)) {
12911                                         pmlmeext->cur_bwmode = CHANNEL_WIDTH_80;
12912                                         DBG_871X("VHT center ch = %d\n", GET_VHT_OPERATION_ELE_CENTER_FREQ1(pIE->data));
12913                                         DBG_871X("set VHT ch/bw before connected\n");
12914                                 }
12915                                 break;
12916 #endif
12917                         default:
12918                                 break;
12919                 }
12920
12921                 i += (pIE->Length + 2);
12922         }
12923 #if 0
12924         if (padapter->registrypriv.wifi_spec) {
12925                 // for WiFi test, follow WMM test plan spec
12926                 acparm = 0x002F431C; // VO
12927                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
12928                 acparm = 0x005E541C; // VI
12929                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
12930                 acparm = 0x0000A525; // BE
12931                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
12932                 acparm = 0x0000A549; // BK
12933                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
12934
12935                 // for WiFi test, mixed mode with intel STA under bg mode throughput issue
12936                 if (padapter->mlmepriv.htpriv.ht_option == _FALSE){
12937                         acparm = 0x00004320;
12938                         rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
12939                 }
12940         }
12941         else {
12942                 acparm = 0x002F3217; // VO
12943                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
12944                 acparm = 0x005E4317; // VI
12945                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
12946                 acparm = 0x00105320; // BE
12947                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
12948                 acparm = 0x0000A444; // BK
12949                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
12950         }
12951 #endif
12952
12953         /* check channel, bandwidth, offset and switch */
12954         if(rtw_chk_start_clnt_join(padapter, &ch, &bw, &offset) == _FAIL) {
12955                 report_join_res(padapter, (-4));
12956                 return H2C_SUCCESS;
12957         }
12958
12959         //disable dynamic functions, such as high power, DIG
12960         //Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
12961
12962         //config the initial gain under linking, need to write the BB registers
12963         //initialgain = 0x1E;
12964         //rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
12965
12966         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
12967         join_type = 0;
12968         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
12969         rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
12970
12971         set_channel_bwmode(padapter, ch, offset, bw);
12972
12973         //cancel link timer
12974         _cancel_timer_ex(&pmlmeext->link_timer);
12975
12976         start_clnt_join(padapter);
12977
12978         return H2C_SUCCESS;
12979
12980 }
12981
12982 u8 disconnect_hdl(_adapter *padapter, unsigned char *pbuf)
12983 {
12984         struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
12985         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12986         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12987         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
12988         u8 val8;
12989
12990         if (is_client_associated_to_ap(padapter))
12991         {
12992 #ifdef CONFIG_DFS
12993                 if(padapter->mlmepriv.handle_dfs == _FALSE)
12994 #endif //CONFIG_DFS
12995 #ifdef CONFIG_PLATFORM_ROCKCHIPS
12996                         //To avoid connecting to AP fail during resume process, change retry count from 5 to 1
12997                         issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
12998 #else
12999                         issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100);
13000 #endif //CONFIG_PLATFORM_ROCKCHIPS
13001         }
13002
13003 #ifdef CONFIG_DFS
13004         if( padapter->mlmepriv.handle_dfs == _TRUE )
13005                 padapter->mlmepriv.handle_dfs = _FALSE;
13006 #endif //CONFIG_DFS
13007
13008         if(((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
13009         {
13010                 //Stop BCN
13011                 val8 = 0;
13012                 rtw_hal_set_hwreg(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8));
13013         }
13014
13015         rtw_mlmeext_disconnect(padapter);
13016
13017         rtw_free_uc_swdec_pending_queue(padapter);
13018
13019         return  H2C_SUCCESS;
13020 }
13021
13022 u8 rtw_scan_sparse(_adapter *adapter, struct rtw_ieee80211_channel *ch, u8 ch_num)
13023 {
13024 /* interval larger than this is treated as backgroud scan */
13025 #ifndef RTW_SCAN_SPARSE_BG_INTERVAL_MS
13026 #define RTW_SCAN_SPARSE_BG_INTERVAL_MS 12000
13027 #endif
13028
13029 #ifndef RTW_SCAN_SPARSE_CH_NUM_MIRACAST
13030 #define RTW_SCAN_SPARSE_CH_NUM_MIRACAST 1
13031 #endif
13032 #ifndef RTW_SCAN_SPARSE_CH_NUM_BG
13033 #define RTW_SCAN_SPARSE_CH_NUM_BG 4
13034 #endif
13035
13036 #define SCAN_SPARSE_CH_NUM_INVALID 255
13037
13038         static u8 token = 255;
13039         u32 interval;
13040         bool busy_traffic = _FALSE;
13041         bool miracast_enabled = _FALSE;
13042         bool bg_scan = _FALSE;
13043         u8 max_allow_ch = SCAN_SPARSE_CH_NUM_INVALID;
13044         u8 scan_division_num;
13045         u8 ret_num = ch_num;
13046         struct registry_priv *regsty = dvobj_to_regsty(adapter_to_dvobj(adapter));
13047         struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
13048
13049         if (regsty->wifi_spec)
13050                 goto exit;
13051
13052         /* assume ch_num > 6 is normal scan */
13053         if (ch_num <= 6)
13054                 goto exit;
13055
13056         if (mlmeext->last_scan_time == 0)
13057                 mlmeext->last_scan_time = rtw_get_current_time();
13058
13059         interval = rtw_get_passing_time_ms(mlmeext->last_scan_time);
13060
13061         if (adapter->mlmepriv.LinkDetectInfo.bBusyTraffic == _TRUE
13062                 #ifdef CONFIG_CONCURRENT_MODE
13063                 || (adapter->pbuddy_adapter && adapter->pbuddy_adapter->mlmepriv.LinkDetectInfo.bBusyTraffic == _TRUE)
13064                 #endif
13065         )
13066                         busy_traffic = _TRUE;
13067
13068         #ifdef CONFIG_WFD
13069         if (is_miracast_enabled(adapter->wfd_info.stack_wfd_mode)
13070                 #ifdef CONFIG_CONCURRENT_MODE
13071                 || (adapter->pbuddy_adapter && is_miracast_enabled(adapter->pbuddy_adapter->wfd_info.stack_wfd_mode))
13072                 #endif
13073         )
13074                 miracast_enabled = _TRUE;
13075         #endif
13076
13077         if (interval > RTW_SCAN_SPARSE_BG_INTERVAL_MS)
13078                 bg_scan = _TRUE;
13079
13080         /* max_allow_ch by conditions*/
13081
13082         #if RTW_SCAN_SPARSE_MIRACAST
13083         if (miracast_enabled == _TRUE && busy_traffic == _TRUE)
13084                 max_allow_ch = rtw_min(max_allow_ch, RTW_SCAN_SPARSE_CH_NUM_MIRACAST);
13085         #endif
13086
13087         #if RTW_SCAN_SPARSE_BG
13088         if (bg_scan == _TRUE)
13089                 max_allow_ch = rtw_min(max_allow_ch, RTW_SCAN_SPARSE_CH_NUM_BG);
13090         #endif
13091
13092
13093         if (max_allow_ch != SCAN_SPARSE_CH_NUM_INVALID) {
13094                 int i;
13095                 int k = 0;
13096
13097                 scan_division_num = (ch_num / max_allow_ch) + ((ch_num % max_allow_ch)?1:0);
13098                 token = (token + 1) % scan_division_num;
13099
13100                 if (0)
13101                         DBG_871X("scan_division_num:%u, token:%u\n", scan_division_num, token);
13102
13103                 for (i = 0; i < ch_num; i++) {
13104                         if (ch[i].hw_value && (i % scan_division_num) == token
13105                         ) {
13106                                 if (i != k)
13107                                         _rtw_memcpy(&ch[k], &ch[i], sizeof(struct rtw_ieee80211_channel));
13108                                 k++;
13109                         }
13110                 }
13111
13112                 _rtw_memset(&ch[k], 0, sizeof(struct rtw_ieee80211_channel));
13113
13114                 ret_num = k;
13115                 mlmeext->last_scan_time = rtw_get_current_time();
13116         }
13117
13118 exit:
13119         return ret_num;
13120 }
13121
13122 static int rtw_scan_ch_decision(_adapter *padapter, struct rtw_ieee80211_channel *out,
13123         u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
13124 {
13125         int i, j;
13126         int scan_ch_num = 0;
13127         int set_idx;
13128         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13129
13130         /* clear first */
13131         _rtw_memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num);
13132
13133         /* acquire channels from in */
13134         j = 0;
13135         for (i=0;i<in_num;i++) {
13136
13137                 if (0)
13138                         DBG_871X(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[i]));
13139
13140                 if(in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED)
13141                         && (set_idx=rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value)) >=0
13142                         && rtw_mlme_band_check(padapter, in[i].hw_value) == _TRUE
13143                 )
13144                 {
13145                         if (j >= out_num) {
13146                                 DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" out_num:%u not enough\n",
13147                                         FUNC_ADPT_ARG(padapter), out_num);
13148                                 break;
13149                         }
13150
13151                         _rtw_memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel));
13152
13153                         if(pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
13154                                 out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
13155
13156                         j++;
13157                 }
13158                 if(j>=out_num)
13159                         break;
13160         }
13161
13162         /* if out is empty, use channel_set as default */
13163         if(j == 0) {
13164                 for (i=0;i<pmlmeext->max_chan_nums;i++) {
13165
13166                         if (0)
13167                                 DBG_871X(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter), pmlmeext->channel_set[i].ChannelNum);
13168
13169                         if (rtw_mlme_band_check(padapter, pmlmeext->channel_set[i].ChannelNum) == _TRUE) {
13170
13171                                 if (j >= out_num) {
13172                                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" out_num:%u not enough\n",
13173                                                 FUNC_ADPT_ARG(padapter), out_num);
13174                                         break;
13175                                 }
13176
13177                                 out[j].hw_value = pmlmeext->channel_set[i].ChannelNum;
13178
13179                                 if(pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
13180                                         out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
13181
13182                                 j++;
13183                         }
13184                 }
13185         }
13186
13187         /* scan_sparse */
13188         j = rtw_scan_sparse(padapter, out, j);
13189
13190         return j;
13191 }
13192
13193 u8 sitesurvey_cmd_hdl(_adapter *padapter, u8 *pbuf)
13194 {
13195         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13196         struct sitesurvey_parm  *pparm = (struct sitesurvey_parm *)pbuf;
13197         u8      bdelayscan = _FALSE;
13198         u8      val8;
13199         u8      initialgain;
13200         u32     i;
13201         struct dvobj_priv *psdpriv = padapter->dvobj;
13202         struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
13203
13204 #ifdef CONFIG_P2P
13205         struct wifidirect_info* pwdinfo = &padapter->wdinfo;
13206 #endif
13207 #ifdef DBG_CHECK_FW_PS_STATE
13208         if(rtw_fw_ps_state(padapter) == _FAIL)
13209         {
13210                 DBG_871X("scan without leave 32k\n");
13211                 pdbgpriv->dbg_scan_pwr_state_cnt++;
13212         }
13213 #endif //DBG_CHECK_FW_PS_STATE
13214
13215         if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE)
13216         {
13217 #ifdef CONFIG_CONCURRENT_MODE
13218                 //for first time sitesurvey_cmd
13219                 rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
13220 #endif //CONFIG_CONCURRENT_MODE
13221
13222                 pmlmeext->sitesurvey_res.state = SCAN_START;
13223                 pmlmeext->sitesurvey_res.bss_cnt = 0;
13224                 pmlmeext->sitesurvey_res.channel_idx = 0;
13225
13226                 for(i=0;i<RTW_SSID_SCAN_AMOUNT;i++){
13227                         if(pparm->ssid[i].SsidLength) {
13228                                 _rtw_memcpy(pmlmeext->sitesurvey_res.ssid[i].Ssid, pparm->ssid[i].Ssid, IW_ESSID_MAX_SIZE);
13229                                 pmlmeext->sitesurvey_res.ssid[i].SsidLength= pparm->ssid[i].SsidLength;
13230                         } else {
13231                                 pmlmeext->sitesurvey_res.ssid[i].SsidLength= 0;
13232                         }
13233                 }
13234
13235                 pmlmeext->sitesurvey_res.ch_num = rtw_scan_ch_decision(padapter
13236                         , pmlmeext->sitesurvey_res.ch, RTW_CHANNEL_SCAN_AMOUNT
13237                         , pparm->ch, pparm->ch_num
13238                 );
13239
13240                 pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
13241
13242                 //issue null data if associating to the AP
13243                 if (is_client_associated_to_ap(padapter) == _TRUE)
13244                 {
13245                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
13246
13247                         issue_nulldata(padapter, NULL, 1, 3, 500);
13248
13249 #ifdef CONFIG_CONCURRENT_MODE
13250                         if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
13251                         {
13252                                 DBG_871X("adapter is scanning(buddy_adapter is linked), issue nulldata(pwrbit=1)\n");
13253
13254                                 issue_nulldata(padapter->pbuddy_adapter, NULL, 1, 3, 500);
13255                         }
13256 #endif
13257                         bdelayscan = _TRUE;
13258                 }
13259 #ifdef CONFIG_CONCURRENT_MODE
13260                 else if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
13261                 {
13262                         #ifdef CONFIG_TDLS
13263                         if(padapter->pbuddy_adapter->wdinfo.wfd_tdls_enable == 1)
13264                         {
13265                                 issue_tunneled_probe_req(padapter->pbuddy_adapter);
13266                         }
13267                         #endif //CONFIG_TDLS
13268
13269                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
13270
13271                         issue_nulldata(padapter->pbuddy_adapter, NULL, 1, 3, 500);
13272
13273                         bdelayscan = _TRUE;
13274                 }
13275 #endif
13276                 if(bdelayscan)
13277                 {
13278                         //delay 50ms to protect nulldata(1).
13279                         set_survey_timer(pmlmeext, 50);
13280                         return H2C_SUCCESS;
13281                 }
13282         }
13283
13284         if ((pmlmeext->sitesurvey_res.state == SCAN_START) || (pmlmeext->sitesurvey_res.state == SCAN_TXNULL))
13285         {
13286 #ifdef CONFIG_FIND_BEST_CHANNEL
13287 #if 0
13288                 for (i=0; pmlmeext->channel_set[i].ChannelNum !=0; i++) {
13289                         pmlmeext->channel_set[i].rx_count = 0;
13290                 }
13291 #endif
13292 #endif /* CONFIG_FIND_BEST_CHANNEL */
13293
13294                 //config the initial gain under scaning, need to write the BB registers
13295 #ifdef CONFIG_P2P
13296 #ifdef CONFIG_IOCTL_CFG80211
13297                 if(adapter_wdev_data(padapter)->p2p_enabled == _TRUE && pwdinfo->driver_interface == DRIVER_CFG80211 )
13298                         initialgain = 0x30;
13299                 else
13300 #endif //CONFIG_IOCTL_CFG80211
13301                 if ( !rtw_p2p_chk_state( pwdinfo, P2P_STATE_NONE ) )
13302                         initialgain = 0x28;
13303                 else
13304 #endif //CONFIG_P2P
13305                         initialgain = 0x1e;
13306
13307                 rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
13308
13309                 //disable dynamic functions, such as high power, DIG
13310                 Save_DM_Func_Flag(padapter);
13311                 Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
13312
13313                 //set MSR to no link state
13314                 Set_MSR(padapter, _HW_STATE_NOLINK_);
13315
13316                 val8 = 1; //under site survey
13317                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
13318
13319                 pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
13320         }
13321
13322         site_survey(padapter);
13323
13324         return H2C_SUCCESS;
13325
13326 }
13327
13328 u8 setauth_hdl(_adapter *padapter, unsigned char *pbuf)
13329 {
13330         struct setauth_parm             *pparm = (struct setauth_parm *)pbuf;
13331         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13332         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13333
13334         if (pparm->mode < 4)
13335         {
13336                 pmlmeinfo->auth_algo = pparm->mode;
13337         }
13338
13339         return  H2C_SUCCESS;
13340 }
13341
13342 u8 setkey_hdl(_adapter *padapter, u8 *pbuf)
13343 {
13344         u16     ctrl = 0;
13345         s16 cam_id = 0;
13346         struct setkey_parm              *pparm = (struct setkey_parm *)pbuf;
13347         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13348         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13349         unsigned char null_addr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
13350         u8 *addr;
13351
13352         //main tx key for wep.
13353         if(pparm->set_tx)
13354                 pmlmeinfo->key_index = pparm->keyid;
13355
13356         cam_id = rtw_camid_alloc(padapter, NULL, pparm->keyid);
13357
13358         if (cam_id < 0){
13359         } else {
13360                 if (cam_id > 3) /* not default key, searched by A2 */
13361                         addr = get_bssid(&padapter->mlmepriv);
13362                 else
13363                         addr = null_addr;
13364
13365                 ctrl = BIT(15) | BIT6 |((pparm->algorithm) << 2) | pparm->keyid;
13366                 write_cam(padapter, cam_id, ctrl, addr, pparm->key);
13367                 DBG_871X_LEVEL(_drv_always_, "set group key camid:%d, addr:"MAC_FMT", kid:%d, type:%s\n"
13368                         ,cam_id, MAC_ARG(addr), pparm->keyid, security_type_str(pparm->algorithm));
13369         }
13370
13371         #ifdef DYNAMIC_CAMID_ALLOC
13372         if (cam_id >=0 && cam_id <=3)
13373                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8*)_TRUE);
13374         #endif
13375
13376         //allow multicast packets to driver
13377         rtw_hal_set_hwreg(padapter, HW_VAR_ON_RCR_AM, null_addr);
13378
13379         return H2C_SUCCESS;
13380 }
13381
13382 u8 set_stakey_hdl(_adapter *padapter, u8 *pbuf)
13383 {
13384         u16 ctrl = 0;
13385         s16 cam_id = 0;
13386         u8 ret = H2C_SUCCESS;
13387         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13388         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13389         struct set_stakey_parm  *pparm = (struct set_stakey_parm *)pbuf;
13390         struct sta_priv *pstapriv = &padapter->stapriv;
13391         struct sta_info *psta;
13392
13393         if(pparm->algorithm == _NO_PRIVACY_)
13394                 goto write_to_cam;
13395
13396         psta = rtw_get_stainfo(pstapriv, pparm->addr);
13397         if (!psta) {
13398                 DBG_871X_LEVEL(_drv_always_, "%s sta:"MAC_FMT" not found\n", __func__, MAC_ARG(pparm->addr));
13399                 ret = H2C_REJECTED;
13400                 goto exit;
13401         }
13402
13403         pmlmeinfo->enc_algo = pparm->algorithm;
13404         cam_id = rtw_camid_alloc(padapter, psta, 0);
13405         if (cam_id < 0)
13406                 goto exit;
13407
13408 write_to_cam:
13409         if(pparm->algorithm == _NO_PRIVACY_) {
13410                 while((cam_id = rtw_camid_search(padapter, pparm->addr, -1)) >= 0) {
13411                         DBG_871X_LEVEL(_drv_always_, "clear key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(pparm->addr), cam_id);
13412                         clear_cam_entry(padapter, cam_id);
13413                         rtw_camid_free(padapter,cam_id);
13414                 }
13415         } else {
13416                 DBG_871X_LEVEL(_drv_always_, "set pairwise key camid:%d, addr:"MAC_FMT", kid:%d, type:%s\n",
13417                         cam_id, MAC_ARG(pparm->addr), pparm->keyid, security_type_str(pparm->algorithm));
13418                 ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;
13419                 write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
13420         }
13421         ret = H2C_SUCCESS_RSP;
13422
13423 exit:
13424         return ret;
13425 }
13426
13427 u8 add_ba_hdl(_adapter *padapter, unsigned char *pbuf)
13428 {
13429         struct addBaReq_parm    *pparm = (struct addBaReq_parm *)pbuf;
13430         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13431         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13432
13433         struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr);
13434
13435         if(!psta)
13436                 return  H2C_SUCCESS;
13437
13438 #ifdef CONFIG_80211N_HT
13439         if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
13440                 ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
13441         {
13442                 //pmlmeinfo->ADDBA_retry_count = 0;
13443                 //pmlmeinfo->candidate_tid_bitmap |= (0x1 << pparm->tid);
13444                 //psta->htpriv.candidate_tid_bitmap |= BIT(pparm->tid);
13445                 issue_addba_req(padapter, pparm->addr, (u8)pparm->tid);
13446                 //_set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO);
13447                 _set_timer(&psta->addba_retry_timer, ADDBA_TO);
13448         }
13449 #ifdef CONFIG_TDLS
13450         else if((psta->tdls_sta_state & TDLS_LINKED_STATE)&&
13451                 (psta->htpriv.ht_option==_TRUE) &&
13452                 (psta->htpriv.ampdu_enable==_TRUE) )
13453         {
13454                 issue_addba_req(padapter, pparm->addr, (u8)pparm->tid);
13455                 _set_timer(&psta->addba_retry_timer, ADDBA_TO);
13456         }
13457 #endif //CONFIG
13458         else
13459         {
13460                 psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
13461         }
13462 #endif //CONFIG_80211N_HT
13463         return  H2C_SUCCESS;
13464 }
13465
13466
13467 u8 chk_bmc_sleepq_cmd(_adapter* padapter)
13468 {
13469         struct cmd_obj *ph2c;
13470         struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
13471         u8 res = _SUCCESS;
13472
13473 _func_enter_;
13474
13475         if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
13476         {
13477                 res= _FAIL;
13478                 goto exit;
13479         }
13480
13481         init_h2fwcmd_w_parm_no_parm_rsp(ph2c, GEN_CMD_CODE(_ChkBMCSleepq));
13482
13483         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
13484
13485 exit:
13486
13487 _func_exit_;
13488
13489         return res;
13490 }
13491
13492 u8 set_tx_beacon_cmd(_adapter* padapter)
13493 {
13494         struct cmd_obj  *ph2c;
13495         struct Tx_Beacon_param  *ptxBeacon_parm;
13496         struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
13497         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13498         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13499         u8      res = _SUCCESS;
13500         int len_diff = 0;
13501
13502 _func_enter_;
13503
13504         if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
13505         {
13506                 res= _FAIL;
13507                 goto exit;
13508         }
13509
13510         if ((ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param))) == NULL)
13511         {
13512                 rtw_mfree((unsigned char *)ph2c, sizeof(struct  cmd_obj));
13513                 res= _FAIL;
13514                 goto exit;
13515         }
13516
13517         _rtw_memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
13518
13519         len_diff = update_hidden_ssid(
13520                 ptxBeacon_parm->network.IEs+_BEACON_IE_OFFSET_
13521                 , ptxBeacon_parm->network.IELength-_BEACON_IE_OFFSET_
13522                 , pmlmeinfo->hidden_ssid_mode
13523         );
13524         ptxBeacon_parm->network.IELength += len_diff;
13525
13526         init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
13527
13528         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
13529
13530
13531 exit:
13532
13533 _func_exit_;
13534
13535         return res;
13536 }
13537
13538
13539 u8 mlme_evt_hdl(_adapter *padapter, unsigned char *pbuf)
13540 {
13541         u8 evt_code, evt_seq;
13542         u16 evt_sz;
13543         uint    *peventbuf;
13544         void (*event_callback)(_adapter *dev, u8 *pbuf);
13545         struct evt_priv *pevt_priv = &(padapter->evtpriv);
13546
13547         if (pbuf == NULL)
13548                 goto _abort_event_;
13549
13550         peventbuf = (uint*)pbuf;
13551         evt_sz = (u16)(*peventbuf&0xffff);
13552         evt_seq = (u8)((*peventbuf>>24)&0x7f);
13553         evt_code = (u8)((*peventbuf>>16)&0xff);
13554
13555
13556         #ifdef CHECK_EVENT_SEQ
13557         // checking event sequence...
13558         if (evt_seq != (ATOMIC_READ(&pevt_priv->event_seq) & 0x7f) )
13559         {
13560                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("Evetn Seq Error! %d vs %d\n", (evt_seq & 0x7f), (ATOMIC_READ(&pevt_priv->event_seq) & 0x7f)));
13561
13562                 pevt_priv->event_seq = (evt_seq+1)&0x7f;
13563
13564                 goto _abort_event_;
13565         }
13566         #endif
13567
13568         // checking if event code is valid
13569         if (evt_code >= MAX_C2HEVT)
13570         {
13571                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nEvent Code(%d) mismatch!\n", evt_code));
13572                 goto _abort_event_;
13573         }
13574
13575         // checking if event size match the event parm size
13576         if ((wlanevents[evt_code].parmsize != 0) &&
13577                         (wlanevents[evt_code].parmsize != evt_sz))
13578         {
13579
13580                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
13581                         evt_code, wlanevents[evt_code].parmsize, evt_sz));
13582                 goto _abort_event_;
13583
13584         }
13585
13586         ATOMIC_INC(&pevt_priv->event_seq);
13587
13588         peventbuf += 2;
13589
13590         if(peventbuf)
13591         {
13592                 event_callback = wlanevents[evt_code].event_callback;
13593                 event_callback(padapter, (u8*)peventbuf);
13594
13595                 pevt_priv->evt_done_cnt++;
13596         }
13597
13598
13599 _abort_event_:
13600
13601
13602         return H2C_SUCCESS;
13603
13604 }
13605
13606 u8 h2c_msg_hdl(_adapter *padapter, unsigned char *pbuf)
13607 {
13608         if(!pbuf)
13609                 return H2C_PARAMETERS_ERROR;
13610
13611         return H2C_SUCCESS;
13612 }
13613
13614 u8 chk_bmc_sleepq_hdl(_adapter *padapter, unsigned char *pbuf)
13615 {
13616 #ifdef CONFIG_AP_MODE
13617         _irqL irqL;
13618         struct sta_info *psta_bmc;
13619         _list   *xmitframe_plist, *xmitframe_phead;
13620         struct xmit_frame *pxmitframe=NULL;
13621         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
13622         struct sta_priv  *pstapriv = &padapter->stapriv;
13623
13624         //for BC/MC Frames
13625         psta_bmc = rtw_get_bcmc_stainfo(padapter);
13626         if(!psta_bmc)
13627                 return H2C_SUCCESS;
13628
13629         if((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len>0))
13630         {
13631 #ifndef CONFIG_PCI_HCI
13632                 rtw_msleep_os(10);// 10ms, ATIM(HIQ) Windows
13633 #endif
13634                 //_enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
13635                 _enter_critical_bh(&pxmitpriv->lock, &irqL);
13636
13637                 xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
13638                 xmitframe_plist = get_next(xmitframe_phead);
13639
13640                 while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
13641                 {
13642                         pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
13643
13644                         xmitframe_plist = get_next(xmitframe_plist);
13645
13646                         rtw_list_delete(&pxmitframe->list);
13647
13648                         psta_bmc->sleepq_len--;
13649                         if(psta_bmc->sleepq_len>0)
13650                                 pxmitframe->attrib.mdata = 1;
13651                         else
13652                                 pxmitframe->attrib.mdata = 0;
13653
13654                         pxmitframe->attrib.triggered=1;
13655
13656                         if (xmitframe_hiq_filter(pxmitframe) == _TRUE)
13657                                 pxmitframe->attrib.qsel = QSLT_HIGH;//HIQ
13658
13659                         #if 0
13660                         _exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
13661                         if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE)
13662                         {
13663                                 rtw_os_xmit_complete(padapter, pxmitframe);
13664                         }
13665                         _enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
13666                         #endif
13667                         rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
13668                 }
13669
13670                 //_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
13671                 _exit_critical_bh(&pxmitpriv->lock, &irqL);
13672
13673                 if (padapter->interface_type != RTW_PCIE) {
13674                         /* check hi queue and bmc_sleepq */
13675                         rtw_chk_hi_queue_cmd(padapter);
13676                 }
13677         }
13678 #endif
13679
13680         return H2C_SUCCESS;
13681 }
13682
13683 u8 tx_beacon_hdl(_adapter *padapter, unsigned char *pbuf)
13684 {
13685         if(send_beacon(padapter)==_FAIL)
13686         {
13687                 DBG_871X("issue_beacon, fail!\n");
13688                 return H2C_PARAMETERS_ERROR;
13689         }
13690
13691         /* tx bc/mc frames after update TIM */
13692         chk_bmc_sleepq_hdl(padapter, NULL);
13693
13694         return H2C_SUCCESS;
13695 }
13696
13697 void change_band_update_ie(_adapter *padapter, WLAN_BSSID_EX *pnetwork)
13698 {
13699         u8      network_type,rate_len, total_rate_len,remainder_rate_len;
13700         struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
13701         struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
13702         u8      erpinfo=0x4;
13703
13704         //DBG_871X("%s\n", __FUNCTION__);
13705
13706         if(pmlmeext->cur_channel >= 36)
13707         {
13708                 network_type = WIRELESS_11A;
13709                 total_rate_len = IEEE80211_NUM_OFDM_RATESLEN;
13710                 DBG_871X("%s(): change to 5G Band\n",__FUNCTION__);
13711                 rtw_remove_bcn_ie(padapter, pnetwork, _ERPINFO_IE_);
13712         }
13713         else
13714         {
13715                 network_type = WIRELESS_11BG;
13716                 total_rate_len = IEEE80211_CCK_RATE_LEN+IEEE80211_NUM_OFDM_RATESLEN;
13717                 DBG_871X("%s(): change to 2.4G Band\n",__FUNCTION__);
13718                 rtw_add_bcn_ie(padapter, pnetwork, _ERPINFO_IE_, &erpinfo, 1);
13719         }
13720
13721         rtw_set_supported_rate(pnetwork->SupportedRates, network_type);
13722
13723         UpdateBrateTbl(padapter, pnetwork->SupportedRates);
13724         rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
13725
13726         if(total_rate_len > 8)
13727         {
13728                 rate_len = 8;
13729                 remainder_rate_len = total_rate_len - 8;
13730         }
13731         else
13732         {
13733                 rate_len = total_rate_len;
13734                 remainder_rate_len = 0;
13735         }
13736
13737         rtw_add_bcn_ie(padapter, pnetwork, _SUPPORTEDRATES_IE_, pnetwork->SupportedRates, rate_len);
13738
13739         if(remainder_rate_len)
13740         {
13741                 rtw_add_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_, (pnetwork->SupportedRates+8), remainder_rate_len);
13742         }
13743         else
13744         {
13745                 rtw_remove_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_);
13746         }
13747 }
13748
13749 #ifdef CONFIG_CONCURRENT_MODE
13750 sint check_buddy_mlmeinfo_state(_adapter *padapter, u32 state)
13751 {
13752         PADAPTER pbuddy_adapter;
13753         struct mlme_ext_priv *pbuddy_mlmeext;
13754         struct mlme_ext_info *pbuddy_mlmeinfo;
13755
13756         if(padapter == NULL)
13757                 return _FALSE;
13758
13759         pbuddy_adapter = padapter->pbuddy_adapter;
13760
13761         if(pbuddy_adapter == NULL)
13762                 return _FALSE;
13763
13764
13765         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13766         pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
13767
13768         if((pbuddy_mlmeinfo->state&0x03) == state)
13769                 return _TRUE;
13770
13771         return _FALSE;
13772
13773 }
13774
13775 void concurrent_chk_joinbss_done(_adapter *padapter, int join_res)
13776 {
13777         struct mlme_ext_priv    *pmlmeext;
13778         struct mlme_ext_info    *pmlmeinfo;
13779         PADAPTER pbuddy_adapter;
13780         struct mlme_priv *pbuddy_mlmepriv;
13781         struct mlme_ext_priv *pbuddy_mlmeext;
13782         struct mlme_ext_info *pbuddy_mlmeinfo;
13783         WLAN_BSSID_EX *pbuddy_network_mlmeext;
13784         WLAN_BSSID_EX *pnetwork;
13785
13786         pmlmeext = &padapter->mlmeextpriv;
13787         pmlmeinfo = &(pmlmeext->mlmext_info);
13788
13789
13790         if(!rtw_buddy_adapter_up(padapter))
13791         {
13792                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
13793                 return;
13794         }
13795
13796         pbuddy_adapter = padapter->pbuddy_adapter;
13797         pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13798         pnetwork = (WLAN_BSSID_EX *)&pbuddy_mlmepriv->cur_network.network;
13799         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13800         pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
13801         pbuddy_network_mlmeext = &(pbuddy_mlmeinfo->network);
13802
13803         if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
13804                         check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
13805         {
13806                 //restart and update beacon
13807
13808                 DBG_871X("after join,primary adapter, CH=%d, BW=%d, offset=%d\n"
13809                         , pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
13810
13811
13812                 if(join_res >= 0)
13813                 {
13814                         u8 *p;
13815                         int     ie_len;
13816                         u8      change_band = _FALSE;
13817                         struct HT_info_element *pht_info=NULL;
13818
13819                         if((pmlmeext->cur_channel <= 14 && pbuddy_mlmeext->cur_channel >= 36) ||
13820                                 (pmlmeext->cur_channel >= 36 && pbuddy_mlmeext->cur_channel <= 14))
13821                                 change_band = _TRUE;
13822
13823                         p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
13824                         if( p && ie_len)
13825                         {
13826                                 pht_info = (struct HT_info_element *)(p+2);
13827                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK; //no secondary channel is present
13828                         }
13829
13830                         pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
13831 #ifdef CONFIG_80211AC_VHT
13832                         if(pbuddy_mlmeext->cur_bwmode  == CHANNEL_WIDTH_80)
13833                         {
13834                                 u8 *pvht_cap_ie, *pvht_op_ie;
13835                                 int vht_cap_ielen, vht_op_ielen;
13836
13837                                 pvht_cap_ie = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), EID_VHTCapability, &vht_cap_ielen, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
13838                                 pvht_op_ie = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), EID_VHTOperation, &vht_op_ielen, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
13839
13840                                 if(pmlmeext->cur_channel <= 14) // downgrade to 20/40Mhz
13841                                 {
13842                                         //modify vht cap ie
13843                                         if( pvht_cap_ie && vht_cap_ielen)
13844                                         {
13845                                                 SET_VHT_CAPABILITY_ELE_SHORT_GI80M(pvht_cap_ie+2, 0);
13846                                         }
13847
13848                                         //modify vht op ie
13849                                         if( pvht_op_ie && vht_op_ielen)
13850                                         {
13851                                                 SET_VHT_OPERATION_ELE_CHL_WIDTH(pvht_op_ie+2, 0); //change to 20/40Mhz
13852                                                 SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(pvht_op_ie+2, 0);
13853                                                 SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ2(pvht_op_ie+2, 0);
13854                                                 //SET_VHT_OPERATION_ELE_BASIC_MCS_SET(p+2, 0xFFFF);
13855                                                 pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
13856                                         }
13857                                 }
13858                                 else
13859                                 {
13860                                         u8      center_freq;
13861
13862                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_80;
13863
13864                                         if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
13865                                                 pmlmeext->cur_bwmode == CHANNEL_WIDTH_80)
13866                                         {
13867                                                 pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
13868                                         }
13869                                         else if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_20)
13870                                         {
13871                                                 pbuddy_mlmeext->cur_ch_offset =  rtw_get_offset_by_ch(pmlmeext->cur_channel);
13872                                         }
13873
13874                                         //modify ht info ie
13875                                         if(pht_info)
13876                                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK;
13877
13878                                         switch(pbuddy_mlmeext->cur_ch_offset)
13879                                         {
13880                                                 case HAL_PRIME_CHNL_OFFSET_LOWER:
13881                                                         if(pht_info)
13882                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;
13883                                                         //cur_bwmode = CHANNEL_WIDTH_40;
13884                                                         break;
13885                                                 case HAL_PRIME_CHNL_OFFSET_UPPER:
13886                                                         if(pht_info)
13887                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
13888                                                         //cur_bwmode = CHANNEL_WIDTH_40;
13889                                                         break;
13890                                                 case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
13891                                                 default:
13892                                                         if(pht_info)
13893                                                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK;
13894                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_20;
13895                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13896                                                         break;
13897                                         }
13898
13899                                         //modify vht op ie
13900                                         center_freq = rtw_get_center_ch(pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, HAL_PRIME_CHNL_OFFSET_LOWER);
13901                                         if( pvht_op_ie && vht_op_ielen)
13902                                                 SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(pvht_op_ie+2, center_freq);
13903
13904                                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13905
13906                                 }
13907
13908                         }
13909 #endif //CONFIG_80211AC_VHT
13910
13911                         if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40)
13912                         {
13913                                 p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
13914                                 if( p && ie_len)
13915                                 {
13916                                         pht_info = (struct HT_info_element *)(p+2);
13917                                         pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK; //no secondary channel is present
13918                                 }
13919
13920                                 if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
13921                                         pmlmeext->cur_bwmode == CHANNEL_WIDTH_80)
13922                                 {
13923                                         pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
13924
13925                                         //to update cur_ch_offset value in beacon
13926                                         if( pht_info )
13927                                         {
13928                                                 switch(pmlmeext->cur_ch_offset)
13929                                                 {
13930                                                         case HAL_PRIME_CHNL_OFFSET_LOWER:
13931                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;
13932                                                                 break;
13933                                                         case HAL_PRIME_CHNL_OFFSET_UPPER:
13934                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
13935                                                                 break;
13936                                                         case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
13937                                                         default:
13938                                                                 break;
13939                                                 }
13940
13941                                         }
13942
13943                                 }
13944                                 else if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_20)
13945                                 {
13946                                         pbuddy_mlmeext->cur_ch_offset =  rtw_get_offset_by_ch(pmlmeext->cur_channel);
13947
13948                                         switch(pbuddy_mlmeext->cur_ch_offset)
13949                                         {
13950                                                 case HAL_PRIME_CHNL_OFFSET_LOWER:
13951                                                         if(pht_info)
13952                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;
13953                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
13954                                                         break;
13955                                                 case HAL_PRIME_CHNL_OFFSET_UPPER:
13956                                                         if(pht_info)
13957                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
13958                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
13959                                                         break;
13960                                                 case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
13961                                                 default:
13962                                                         if(pht_info)
13963                                                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK;
13964                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_20;
13965                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13966                                                         break;
13967                                         }
13968
13969                                 }
13970
13971                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13972
13973                         }
13974                         else
13975                         {
13976                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
13977                         }
13978
13979
13980                         // to update channel value in beacon
13981                         pbuddy_network_mlmeext->Configuration.DSConfig = pmlmeext->cur_channel;
13982                         p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
13983                         if(p && ie_len>0)
13984                                 *(p + 2) = pmlmeext->cur_channel;
13985
13986                         p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
13987                         if( p && ie_len)
13988                         {
13989                                 pht_info = (struct HT_info_element *)(p+2);
13990                                 pht_info->primary_channel = pmlmeext->cur_channel;
13991                         }
13992
13993                         //buddy interface band is different from current interface, update ERP, support rate, ext support rate IE
13994                         if(change_band == _TRUE)
13995                                 change_band_update_ie(pbuddy_adapter, pbuddy_network_mlmeext);
13996                 }
13997                 else
13998                 {
13999                         // switch back to original channel/bwmode/ch_offset;
14000                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
14001                 }
14002
14003                 DBG_871X("after join, second adapter, CH=%d, BW=%d, offset=%d\n", pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
14004
14005                 DBG_871X("update pbuddy_adapter's beacon\n");
14006
14007                 _rtw_memcpy(pnetwork, pbuddy_network_mlmeext, sizeof(WLAN_BSSID_EX));
14008                 //update bmc rate to avoid bb cck hang
14009                 update_bmc_sta(pbuddy_adapter);
14010                 update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
14011
14012         }
14013         else if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) &&
14014                         check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
14015         {
14016                 if(join_res >= 0)
14017                 {
14018                         pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
14019                         if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
14020                                 pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80)
14021                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
14022                         else if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
14023                                 pmlmeext->cur_bwmode == CHANNEL_WIDTH_80)
14024                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
14025                         else
14026                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
14027                 }
14028                 else
14029                 {
14030                         // switch back to original channel/bwmode/ch_offset;
14031                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
14032                 }
14033         }
14034         else
14035         {
14036                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
14037         }
14038
14039 }
14040 #endif //CONFIG_CONCURRENT_MODE
14041
14042 int rtw_chk_start_clnt_join(_adapter *padapter, u8 *ch, u8 *bw, u8 *offset)
14043 {
14044         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
14045         unsigned char   cur_ch = pmlmeext->cur_channel;
14046         unsigned char   cur_bw = pmlmeext->cur_bwmode;
14047         unsigned char   cur_ch_offset = pmlmeext->cur_ch_offset;
14048         bool chbw_allow = _TRUE;
14049         bool connect_allow = _TRUE;
14050
14051 #ifdef CONFIG_CONCURRENT_MODE
14052         PADAPTER pbuddy_adapter;
14053         struct mlme_ext_priv *pbuddy_mlmeext;
14054         struct mlme_ext_info    *pbuddy_pmlmeinfo;
14055         struct mlme_priv *pbuddy_mlmepriv;
14056 #endif
14057
14058         if (!ch || !bw || !offset) {
14059                 connect_allow = _FALSE;
14060                 rtw_warn_on(1);
14061                 goto exit;
14062         }
14063
14064         if (cur_ch == 0) {
14065                 connect_allow = _FALSE;
14066                 DBG_871X_LEVEL(_drv_err_, FUNC_ADPT_FMT" cur_ch:%u\n"
14067                         , FUNC_ADPT_ARG(padapter), cur_ch);
14068                 rtw_warn_on(1);
14069                 goto exit;
14070         }
14071
14072 #ifdef CONFIG_CONCURRENT_MODE
14073         if (!rtw_buddy_adapter_up(padapter)) {
14074                 goto exit;
14075         }
14076
14077         pbuddy_adapter = padapter->pbuddy_adapter;
14078         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
14079         pbuddy_pmlmeinfo = &(pbuddy_mlmeext->mlmext_info);
14080         pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
14081
14082         if((pbuddy_pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)//for AP MODE
14083         {
14084                 DBG_871X("start_clnt_join: "ADPT_FMT"(ch=%d, bw=%d, ch_offset=%d)"
14085                         ", "ADPT_FMT" AP mode(ch=%d, bw=%d, ch_offset=%d)\n",
14086                         ADPT_ARG(padapter), pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset,
14087                         ADPT_ARG(pbuddy_adapter), pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
14088
14089                 if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel)
14090                 {
14091                         chbw_allow = _FALSE;
14092                 }
14093                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
14094                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
14095                         (pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset))
14096                 {
14097                         chbw_allow = _FALSE;
14098                 }
14099                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_20) &&
14100                         ((pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40)||
14101                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80)) )
14102                 {
14103                         cur_ch = pmlmeext->cur_channel;
14104                         cur_bw = pbuddy_mlmeext->cur_bwmode;
14105                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
14106                 }
14107
14108                 DBG_871X("start_clnt_join: connect_allow:%d, chbw_allow:%d\n", connect_allow, chbw_allow);
14109                 if (chbw_allow == _FALSE) {
14110                         #ifdef CONFIG_SPCT_CH_SWITCH
14111                         if (1) {
14112                                 rtw_ap_inform_ch_switch(pbuddy_adapter, pmlmeext->cur_channel , pmlmeext->cur_ch_offset);
14113                         } else
14114                         #endif
14115                         {
14116                                 //issue deauth to all stas if if2 is at ap mode
14117                                 rtw_sta_flush(pbuddy_adapter);
14118                         }
14119                         rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
14120                 }
14121         }
14122         else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED) == _TRUE &&
14123                 check_fwstate(pbuddy_mlmepriv, WIFI_STATION_STATE) == _TRUE) //for Client Mode/p2p client
14124         {
14125                 DBG_871X("start_clnt_join: "ADPT_FMT"(ch=%d, bw=%d, ch_offset=%d)"
14126                         ", "ADPT_FMT" STA mode(ch=%d, bw=%d, ch_offset=%d)\n",
14127                         ADPT_ARG(padapter), pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset,
14128                         ADPT_ARG(pbuddy_adapter), pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
14129
14130                 if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel)
14131                 {
14132                         chbw_allow = _FALSE;
14133                 }
14134                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_20) &&
14135                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40))
14136                 {
14137                         cur_bw = CHANNEL_WIDTH_40;
14138                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
14139                 }
14140                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_20) &&
14141                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80))
14142                 {
14143                         cur_bw = CHANNEL_WIDTH_80;
14144                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
14145                 }
14146                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
14147                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
14148                         (pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset))
14149                 {
14150                         chbw_allow = _FALSE;
14151                 }
14152                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
14153                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80))
14154                 {
14155                         if(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
14156                         {
14157                                 chbw_allow = _FALSE;
14158                         }
14159                         else
14160                         {
14161                                 cur_bw = CHANNEL_WIDTH_80;
14162                         }
14163                 }
14164                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_80) &&
14165                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40))
14166                 {
14167                         if(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
14168                         {
14169                                 chbw_allow = _FALSE;
14170                         }
14171                 }
14172                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_80) &&
14173                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80))
14174                 {
14175                         if(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
14176                         {
14177                                 chbw_allow = _FALSE;
14178                         }
14179                 }
14180
14181
14182                 connect_allow = chbw_allow;
14183
14184 #ifdef CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT
14185                 #if defined(CONFIG_P2P) && defined(CONFIG_IOCTL_CFG80211)
14186                 /* wlan0-sta mode has higher priority than p2p0-p2p client */
14187                 if (!rtw_p2p_chk_state(&(pbuddy_adapter->wdinfo), P2P_STATE_NONE)
14188                         && pbuddy_adapter->wdinfo.driver_interface == DRIVER_CFG80211)
14189                 {
14190                         connect_allow = _TRUE;
14191                 }
14192                 #endif /* CONFIG_P2P && CONFIG_IOCTL_CFG80211 */
14193 #else
14194                 connect_allow = _TRUE;
14195 #endif /* CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT */
14196
14197                 DBG_871X("start_clnt_join: connect_allow:%d, chbw_allow:%d\n", connect_allow, chbw_allow);
14198                 if (connect_allow == _TRUE && chbw_allow == _FALSE) {
14199                         /* disconnect buddy's connection */
14200                         rtw_disassoc_cmd(pbuddy_adapter, 500, _FALSE);
14201                         rtw_indicate_disconnect(pbuddy_adapter);
14202                         rtw_free_assoc_resources(pbuddy_adapter, 1);
14203                 }
14204         }
14205
14206 #endif /* CONFIG_CONCURRENT_MODE */
14207
14208 exit:
14209
14210         if (connect_allow == _TRUE) {
14211                 DBG_871X("start_join_set_ch_bw: ch=%d, bwmode=%d, ch_offset=%d\n", cur_ch, cur_bw, cur_ch_offset);
14212                 *ch = cur_ch;
14213                 *bw = cur_bw;
14214                 *offset = cur_ch_offset;
14215         }
14216
14217         return connect_allow == _TRUE ? _SUCCESS : _FAIL;
14218 }
14219
14220 /* Find union about ch, bw, ch_offset of all linked/linking interfaces */
14221 int rtw_get_ch_setting_union(_adapter *adapter, u8 *ch, u8 *bw, u8 *offset)
14222 {
14223         struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
14224         _adapter *iface;
14225         struct mlme_ext_priv *mlmeext;
14226         int i;
14227         u8 ch_ret = 0;
14228         u8 bw_ret = CHANNEL_WIDTH_20;
14229         u8 offset_ret = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
14230         int num = 0;
14231
14232         if (ch) *ch = 0;
14233         if (bw) *bw = CHANNEL_WIDTH_20;
14234         if (offset) *offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
14235
14236         for (i = 0; i<dvobj->iface_nums; i++) {
14237                 iface = dvobj->padapters[i];
14238                 mlmeext = &iface->mlmeextpriv;
14239
14240                 if (!check_fwstate(&iface->mlmepriv, _FW_LINKED|_FW_UNDER_LINKING))
14241                         continue;
14242
14243                 if (num == 0) {
14244                         ch_ret = mlmeext->cur_channel;
14245                         bw_ret = mlmeext->cur_bwmode;
14246                         offset_ret = mlmeext->cur_ch_offset;
14247                         num++;
14248                         continue;
14249                 }
14250
14251                 if (ch_ret != mlmeext->cur_channel) {
14252                         num = 0;
14253                         break;
14254                 }
14255
14256                 if (bw_ret < mlmeext->cur_bwmode) {
14257                         bw_ret = mlmeext->cur_bwmode;
14258                         offset_ret = mlmeext->cur_ch_offset;
14259                 } else if (bw_ret == mlmeext->cur_bwmode && offset_ret != mlmeext->cur_ch_offset) {
14260                         num = 0;
14261                         break;
14262                 }
14263
14264                 num++;
14265         }
14266
14267         if (num) {
14268                 if (ch) *ch = ch_ret;
14269                 if (bw) *bw = bw_ret;
14270                 if (offset) *offset = offset_ret;
14271         }
14272
14273         return num;
14274 }
14275
14276 u8 set_ch_hdl(_adapter *padapter, u8 *pbuf)
14277 {
14278         struct set_ch_parm *set_ch_parm;
14279         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
14280         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
14281
14282         if(!pbuf)
14283                 return H2C_PARAMETERS_ERROR;
14284
14285         set_ch_parm = (struct set_ch_parm *)pbuf;
14286
14287         DBG_871X(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
14288                 FUNC_NDEV_ARG(padapter->pnetdev),
14289                 set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
14290
14291         pmlmeext->cur_channel = set_ch_parm->ch;
14292         pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
14293         pmlmeext->cur_bwmode = set_ch_parm->bw;
14294
14295         set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
14296
14297         return  H2C_SUCCESS;
14298 }
14299
14300 u8 set_chplan_hdl(_adapter *padapter, unsigned char *pbuf)
14301 {
14302         struct SetChannelPlan_param *setChannelPlan_param;
14303         struct mlme_priv *mlme = &padapter->mlmepriv;
14304         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
14305
14306         if(!pbuf)
14307                 return H2C_PARAMETERS_ERROR;
14308
14309         setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
14310
14311         if(!rtw_is_channel_plan_valid(setChannelPlan_param->channel_plan)) {
14312                 return H2C_PARAMETERS_ERROR;
14313         }
14314
14315         mlme->ChannelPlan = setChannelPlan_param->channel_plan;
14316
14317         pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
14318         init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
14319
14320         rtw_hal_set_odm_var(padapter,HAL_ODM_REGULATION,NULL,_TRUE);
14321
14322 #ifdef CONFIG_IOCTL_CFG80211
14323         rtw_reg_notify_by_driver(padapter);
14324 #endif //CONFIG_IOCTL_CFG80211
14325
14326         return  H2C_SUCCESS;
14327 }
14328
14329 u8 led_blink_hdl(_adapter *padapter, unsigned char *pbuf)
14330 {
14331         struct LedBlink_param *ledBlink_param;
14332
14333         if(!pbuf)
14334                 return H2C_PARAMETERS_ERROR;
14335
14336         ledBlink_param = (struct LedBlink_param *)pbuf;
14337
14338         #ifdef CONFIG_LED_HANDLED_BY_CMD_THREAD
14339         BlinkHandler((PLED_DATA)ledBlink_param->pLed);
14340         #endif
14341
14342         return  H2C_SUCCESS;
14343 }
14344
14345 u8 set_csa_hdl(_adapter *padapter, unsigned char *pbuf)
14346 {
14347 #ifdef CONFIG_DFS
14348         struct SetChannelSwitch_param *setChannelSwitch_param;
14349         u8 new_ch_no;
14350         u8 gval8 = 0x00, sval8 = 0xff;
14351
14352         if(!pbuf)
14353                 return H2C_PARAMETERS_ERROR;
14354
14355         setChannelSwitch_param = (struct SetChannelSwitch_param *)pbuf;
14356         new_ch_no = setChannelSwitch_param->new_ch_no;
14357
14358         rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
14359
14360         rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &sval8);
14361
14362         DBG_871X("DFS detected! Swiching channel to %d!\n", new_ch_no);
14363         SelectChannel(padapter, new_ch_no);
14364
14365         rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
14366
14367         rtw_disassoc_cmd(padapter, 0, _FALSE);
14368         rtw_indicate_disconnect(padapter);
14369         rtw_free_assoc_resources(padapter, 1);
14370         rtw_free_network_queue(padapter, _TRUE);
14371
14372         if ( ((new_ch_no >= 52) && (new_ch_no <= 64)) ||((new_ch_no >= 100) && (new_ch_no <= 140)) ) {
14373                 DBG_871X("Switched to DFS band (ch %02x) again!!\n", new_ch_no);
14374         }
14375
14376         return  H2C_SUCCESS;
14377 #else
14378         return  H2C_REJECTED;
14379 #endif //CONFIG_DFS
14380
14381 }
14382
14383 u8 tdls_hdl(_adapter *padapter, unsigned char *pbuf)
14384 {
14385 #ifdef CONFIG_TDLS
14386         _irqL irqL;
14387         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
14388 #ifdef CONFIG_TDLS_CH_SW
14389         struct tdls_ch_switch *pchsw_info = &ptdlsinfo->chsw_info;
14390 #endif
14391         struct TDLSoption_param *TDLSoption;
14392         struct sta_info *ptdls_sta = NULL;
14393         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
14394         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
14395         u8 survey_channel, i, min, option;
14396         struct tdls_txmgmt txmgmt;
14397         u32 setchtime, resp_sleep = 0, wait_time;
14398         u8 zaddr[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
14399
14400         if (!pbuf)
14401                 return H2C_PARAMETERS_ERROR;
14402
14403         TDLSoption = (struct TDLSoption_param *)pbuf;
14404         option = TDLSoption->option;
14405
14406         if (!_rtw_memcmp(TDLSoption->addr, zaddr, ETH_ALEN)) {
14407                 ptdls_sta = rtw_get_stainfo( &(padapter->stapriv), TDLSoption->addr );
14408                 if (ptdls_sta == NULL) {
14409                         return H2C_REJECTED;
14410                 }
14411         } else {
14412                 if (!(option == TDLS_RS_RCR || option == TDLS_CH_SW_BACK))
14413                         return H2C_REJECTED;
14414         }
14415
14416         //_enter_critical_bh(&(ptdlsinfo->hdl_lock), &irqL);
14417         //DBG_871X("[%s] option:%d\n", __FUNCTION__, option);
14418
14419         switch (option) {
14420         case TDLS_ESTABLISHED:
14421         {
14422                 /* As long as TDLS handshake success, we should set RCR_CBSSID_DATA bit to 0 */
14423                 /* So we can receive all kinds of data frames. */
14424                 u8 sta_band = 0;
14425
14426                 //leave ALL PS when TDLS is established
14427                         rtw_pwr_wakeup(padapter);
14428
14429                 rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_WRCR, 0);
14430                 DBG_871X("Created Direct Link with "MAC_FMT"\n", MAC_ARG(ptdls_sta->hwaddr));
14431
14432                 pmlmeinfo->FW_sta_info[ptdls_sta->mac_id].psta = ptdls_sta;
14433                 /* Set TDLS sta rate. */
14434                 /* Update station supportRate */
14435                 rtw_hal_update_sta_rate_mask(padapter, ptdls_sta);
14436                 if (pmlmeext->cur_channel > 14) {
14437                         if (ptdls_sta->ra_mask & 0xffff000)
14438                                 sta_band |= WIRELESS_11_5N ;
14439
14440                         if (ptdls_sta->ra_mask & 0xff0)
14441                                 sta_band |= WIRELESS_11A;
14442
14443                         /* 5G band */
14444                         #ifdef CONFIG_80211AC_VHT
14445                         if (ptdls_sta->vhtpriv.vht_option)
14446                                 sta_band = WIRELESS_11_5AC;
14447                         #endif
14448
14449                 } else {
14450                         if (ptdls_sta->ra_mask & 0xffff000)
14451                                 sta_band |= WIRELESS_11_24N;
14452
14453                         if (ptdls_sta->ra_mask & 0xff0)
14454                                 sta_band |= WIRELESS_11G;
14455
14456                         if (ptdls_sta->ra_mask & 0x0f)
14457                                 sta_band |= WIRELESS_11B;
14458                 }
14459                 ptdls_sta->wireless_mode = sta_band;
14460                 ptdls_sta->raid = rtw_hal_networktype_to_raid(padapter,ptdls_sta);
14461                 set_sta_rate(padapter, ptdls_sta);
14462                 rtw_sta_media_status_rpt(padapter, ptdls_sta, 1);
14463                 /* Sta mode */
14464                 rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, ptdls_sta,_TRUE);
14465                 break;
14466         }
14467         case TDLS_ISSUE_PTI:
14468                 ptdls_sta->tdls_sta_state |= TDLS_WAIT_PTR_STATE;
14469                 issue_tdls_peer_traffic_indication(padapter, ptdls_sta);
14470                 _set_timer(&ptdls_sta->pti_timer, TDLS_PTI_TIME);
14471                 break;
14472 #ifdef CONFIG_TDLS_CH_SW
14473         case TDLS_CH_SW_RESP:
14474                 _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
14475                 txmgmt.status_code = 0;
14476                 _rtw_memcpy(txmgmt.peer, ptdls_sta->hwaddr, ETH_ALEN);
14477
14478                 issue_nulldata(padapter, NULL, 1, 0, 0);
14479
14480                 DBG_871X("issue tdls channel switch response\n");
14481                 issue_tdls_ch_switch_rsp(padapter, &txmgmt, _FALSE);
14482                 resp_sleep = 5;
14483                 rtw_msleep_os(resp_sleep);
14484
14485                 /* If we receive TDLS_CH_SW_REQ at off channel which it's target is AP's channel */
14486                 /* then we just SelectChannel to AP's channel*/
14487                 if (padapter->mlmeextpriv.cur_channel == pchsw_info->off_ch_num) {
14488                         SelectChannel(padapter, padapter->mlmeextpriv.cur_channel);
14489                         issue_nulldata(padapter, NULL, 0, 0, 0);
14490                         pchsw_info->ch_sw_state &= ~(TDLS_PEER_AT_OFF_STATE);
14491                         ATOMIC_SET(&pchsw_info->chsw_on, _FALSE);
14492                         break;
14493                 }
14494
14495                 _set_timer(&ptdls_sta->delay_timer, pmlmeinfo->bcn_interval - 40);
14496
14497                 /* Continue following actions */
14498
14499         case TDLS_CH_SW:
14500                 issue_nulldata(padapter, NULL, 1, 0, 0);
14501                 _set_timer(&ptdls_sta->ch_sw_timer, (u32)(ptdls_sta->ch_switch_timeout)/1000);
14502
14503                 setchtime = rtw_systime_to_ms(rtw_get_current_time());
14504                 SelectChannel(padapter, pchsw_info->off_ch_num);
14505                 setchtime = rtw_systime_to_ms(rtw_get_current_time()) - setchtime;
14506                 setchtime += resp_sleep;
14507
14508                 if (pmlmeext->cur_channel != rtw_get_oper_ch(padapter))
14509                         issue_nulldata(padapter, NULL, 0, 0, 0);
14510                 pchsw_info->ch_sw_state &= ~(TDLS_PEER_AT_OFF_STATE);
14511
14512                 if ((u32)ptdls_sta->ch_switch_time/1000 > setchtime)
14513                         wait_time = (u32)ptdls_sta->ch_switch_time/1000 - setchtime;
14514                 else
14515                         wait_time = 0;
14516
14517                 if (wait_time > 0)
14518                         rtw_msleep_os(wait_time);
14519
14520                 issue_nulldata_to_TDLS_peer_STA(ptdls_sta->padapter, ptdls_sta->hwaddr, 0, 0, 0);
14521                 issue_nulldata_to_TDLS_peer_STA(ptdls_sta->padapter, ptdls_sta->hwaddr, 0, 0, 0);
14522
14523                 break;
14524         case TDLS_CH_SW_BACK:
14525                 pchsw_info->ch_sw_state &= ~(TDLS_PEER_AT_OFF_STATE | TDLS_WAIT_CH_RSP_STATE);
14526                 ATOMIC_SET(&pchsw_info->chsw_on, _FALSE);
14527                 SelectChannel(padapter, padapter->mlmeextpriv.cur_channel);
14528                 issue_nulldata(padapter, NULL, 0, 0, 0);
14529                 break;
14530 #endif
14531         case TDLS_RS_RCR:
14532                 rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_RS_RCR, 0);
14533                 DBG_871X("wirte REG_RCR, set bit6 on\n");
14534                 break;
14535         case TDLS_TEAR_STA:
14536 #ifdef CONFIG_TDLS_CH_SW
14537                 if (_rtw_memcmp(TDLSoption->addr, pchsw_info->addr, ETH_ALEN) == _TRUE) {
14538                         pchsw_info->ch_sw_state &= ~(TDLS_CH_SW_INITIATOR_STATE |
14539                                                                                 TDLS_CH_SWITCH_ON_STATE |
14540                                                                                 TDLS_PEER_AT_OFF_STATE);
14541                         ATOMIC_SET(&pchsw_info->chsw_on, _FALSE);
14542                         _rtw_memset(pchsw_info->addr, 0x00, ETH_ALEN);
14543                 }
14544 #endif
14545                 rtw_sta_media_status_rpt(padapter, ptdls_sta, 0);
14546                 free_tdls_sta(padapter, ptdls_sta);
14547                 break;
14548         }
14549
14550         //_exit_critical_bh(&(ptdlsinfo->hdl_lock), &irqL);
14551
14552         return H2C_SUCCESS;
14553 #else
14554         return H2C_REJECTED;
14555 #endif /* CONFIG_TDLS */
14556
14557 }
14558
14559 u8 run_in_thread_hdl(_adapter *padapter, u8 *pbuf)
14560 {
14561         struct RunInThread_param *p;
14562
14563
14564         if (NULL == pbuf)
14565                 return H2C_PARAMETERS_ERROR;
14566         p = (struct RunInThread_param*)pbuf;
14567
14568         if (p->func)
14569                 p->func(p->context);
14570
14571         return H2C_SUCCESS;
14572 }
14573