drivers/net/wireless/rtl8723bs/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 };
  48
  49 #ifdef _CONFIG_NATIVEAP_MLME_
  50 struct mlme_handler mlme_ap_tbl[]={
  51         {WIFI_ASSOCREQ,         "OnAssocReq",   &OnAssocReq},
  52         {WIFI_ASSOCRSP,         "OnAssocRsp",   &OnAssocRsp},
  53         {WIFI_REASSOCREQ,       "OnReAssocReq", &OnAssocReq},
  54         {WIFI_REASSOCRSP,       "OnReAssocRsp", &OnAssocRsp},
  55         {WIFI_PROBEREQ,         "OnProbeReq",   &OnProbeReq},
  56         {WIFI_PROBERSP,         "OnProbeRsp",           &OnProbeRsp},
  57
  58         /*----------------------------------------------------------
  59                                         below 2 are reserved
  60         -----------------------------------------------------------*/
  61         {0,                                     "DoReserved",           &DoReserved},
  62         {0,                                     "DoReserved",           &DoReserved},
  63         {WIFI_BEACON,           "OnBeacon",             &OnBeacon},
  64         {WIFI_ATIM,                     "OnATIM",               &OnAtim},
  65         {WIFI_DISASSOC,         "OnDisassoc",           &OnDisassoc},
  66         {WIFI_AUTH,                     "OnAuth",               &OnAuth},
  67         {WIFI_DEAUTH,           "OnDeAuth",             &OnDeAuth},
  68         {WIFI_ACTION,           "OnAction",             &OnAction},
  69 };
  70 #endif
  71
  72 struct action_handler OnAction_tbl[]={
  73         {RTW_WLAN_CATEGORY_SPECTRUM_MGMT,        "ACTION_SPECTRUM_MGMT", on_action_spct},
  74         {RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos},
  75         {RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls},
  76         {RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
  77         {RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
  78         {RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
  79         {RTW_WLAN_CATEGORY_FT, "ACTION_FT",     &DoReserved},
  80         {RTW_WLAN_CATEGORY_HT,  "ACTION_HT",    &OnAction_ht},
  81 #ifdef CONFIG_IEEE80211W
  82         {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &OnAction_sa_query},
  83 #else
  84         {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved},
  85 #endif //CONFIG_IEEE80211W
  86         //add for CONFIG_IEEE80211W
  87         {RTW_WLAN_CATEGORY_UNPROTECTED_WNM, "ACTION_UNPROTECTED_WNM", &DoReserved},
  88         {RTW_WLAN_CATEGORY_SELF_PROTECTED, "ACTION_SELF_PROTECTED", &DoReserved},
  89         {RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm},
  90         {RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p},
  91 };
  92
  93
  94 u8      null_addr[ETH_ALEN]= {0,0,0,0,0,0};
  95
  96 /**************************************************
  97 OUI definitions for the vendor specific IE
  98 ***************************************************/
  99 unsigned char   RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
 100 unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
 101 unsigned char   WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
 102 unsigned char   P2P_OUI[] = {0x50,0x6F,0x9A,0x09};
 103 unsigned char   WFD_OUI[] = {0x50,0x6F,0x9A,0x0A};
 104
 105 unsigned char   WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
 106 unsigned char   WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
 107
 108 unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
 109 unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
 110
 111 extern unsigned char REALTEK_96B_IE[];
 112
 113 /********************************************************
 114 MCS rate definitions
 115 *********************************************************/
 116 #ifdef CONFIG_DISABLE_MCS13TO15
 117 unsigned char   MCS_rate_2R_MCS13TO15_OFF[16] = {0xff, 0x1f, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 118 unsigned char   MCS_rate_2R[16] = {0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 119 #else //CONFIG_DISABLE_MCS13TO15
 120 unsigned char   MCS_rate_2R[16] = {0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 121 #endif //CONFIG_DISABLE_MCS13TO15
 122 unsigned char   MCS_rate_1R[16] = {0xff, 0x00, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 123
 124 /********************************************************
 125 ChannelPlan definitions
 126 *********************************************************/
 127 /*static RT_CHANNEL_PLAN        DefaultChannelPlan[RT_CHANNEL_DOMAIN_MAX] = {
 128         {{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
 129         {{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
 130         {{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
 131         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x03, RT_CHANNEL_DOMAIN_SPAIN
 132         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x04, RT_CHANNEL_DOMAIN_FRANCE
 133         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x05, RT_CHANNEL_DOMAIN_MKK
 134         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x06, RT_CHANNEL_DOMAIN_MKK1
 135         {{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
 136         {{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
 137         {{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},                                                                                                                                                                // 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN
 138         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13
 139         {{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
 140         {{1,2,3,4,5,6,7,8,9,10,11,12,13,149,153,157,161,165},18},                                                                                                                               // 0x0C, RT_CHANNEL_DOMAIN_CHINA
 141         {{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
 142         {{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
 143         {{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
 144         {{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
 145         {{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
 146         {{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
 147         {{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
 148         {{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
 149 };*/
 150
 151 static RT_CHANNEL_PLAN_2G       RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
 152         {{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
 153         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},           // 0x01, RT_CHANNEL_DOMAIN_2G_ETSI1
 154         {{1,2,3,4,5,6,7,8,9,10,11},11},                 // 0x02, RT_CHANNEL_DOMAIN_2G_FCC1
 155         {{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},        // 0x03, RT_CHANNEL_DOMAIN_2G_MIKK1
 156         {{10,11,12,13},4},                                              // 0x04, RT_CHANNEL_DOMAIN_2G_ETSI2
 157         {{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
 158         {{},0},                                                         // 0x06, RT_CHANNEL_DOMAIN_2G_NULL
 159 };
 160
 161 static RT_CHANNEL_PLAN_5G       RTW_ChannelPlan5G[RT_CHANNEL_DOMAIN_5G_MAX] = {
 162         {{},0},                                                                                                                                                                 // 0x00, RT_CHANNEL_DOMAIN_5G_NULL
 163         {{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
 164         {{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
 165         {{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
 166         {{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
 167         {{36,40,44,48,149,153,157,161,165},9},                                                                                                          // 0x05, RT_CHANNEL_DOMAIN_5G_FCC2
 168         {{36,40,44,48,52,56,60,64,149,153,157,161,165},13},                                                                                     // 0x06, RT_CHANNEL_DOMAIN_5G_FCC3
 169         {{36,40,44,48,52,56,60,64,149,153,157,161},12},                                                                                         // 0x07, RT_CHANNEL_DOMAIN_5G_FCC4
 170         {{149,153,157,161,165},5},                                                                                                                                      // 0x08, RT_CHANNEL_DOMAIN_5G_FCC5
 171         {{36,40,44,48,52,56,60,64},8},                                                                                                                          // 0x09, RT_CHANNEL_DOMAIN_5G_FCC6
 172         {{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
 173         {{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
 174         {{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
 175         {{36,40,44,48,52,56,60,64},8},                                                                                                                          // 0x0D, RT_CHANNEL_DOMAIN_5G_MKK2
 176         {{100,104,108,112,116,120,124,128,132,136,140},11},                                                                                     // 0x0E, RT_CHANNEL_DOMAIN_5G_MKK3
 177         {{56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},15},                                                                // 0x0F, RT_CHANNEL_DOMAIN_5G_NCC1
 178         {{56,60,64,149,153,157,161,165},8},                                                                                                                     // 0x10, RT_CHANNEL_DOMAIN_5G_NCC2
 179         {{149,153,157,161,165},5},                                                                                                                                      // 0x11, RT_CHANNEL_DOMAIN_5G_NCC3
 180         {{36,40,44,48},4},                                                                                                                                                      // 0x12, RT_CHANNEL_DOMAIN_5G_ETSI4
 181         {{36,40,44,48,52,56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},20},                                 // 0x13, RT_CHANNEL_DOMAIN_5G_ETSI5
 182         {{149,153,157,161},4},                                                                                                                                          // 0x14, RT_CHANNEL_DOMAIN_5G_FCC8
 183         {{36,40,44,48,52,56,60,64},8},                                                                                                                          // 0x15, RT_CHANNEL_DOMAIN_5G_ETSI6
 184         {{36,40,44,48,52,56,60,64,149,153,157,161,165},13},                                                                                     // 0x16, RT_CHANNEL_DOMAIN_5G_ETSI7
 185         {{36,40,44,48,149,153,157,161,165},9},                                                                                                          // 0x17, RT_CHANNEL_DOMAIN_5G_ETSI8
 186         {{100,104,108,112,116,120,124,128,132,136,140},11},                                                                                     // 0x18, RT_CHANNEL_DOMAIN_5G_ETSI9
 187         {{149,153,157,161,165},5},                                                                                                                                      // 0x19, RT_CHANNEL_DOMAIN_5G_ETSI10
 188         {{36,40,44,48,52,56,60,64,132,136,140,149,153,157,161,165},16},                                                                 // 0x1A, RT_CHANNEL_DOMAIN_5G_ETSI11
 189         {{52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},17},                                                 // 0x1B, RT_CHANNEL_DOMAIN_5G_NCC4
 190         {{149,153,157,161},4},                                                                                                                                          // 0x1C, RT_CHANNEL_DOMAIN_5G_ETSI12
 191         {{36,40,44,48,100,104,108,112,116,132,136,140,149,153,157,161,165},17},                                                 // 0x1D, RT_CHANNEL_DOMAIN_5G_FCC9
 192         {{36,40,44,48,100,104,108,112,116,132,136,140},12},                                                                                     // 0x1E, RT_CHANNEL_DOMAIN_5G_ETSI13
 193         {{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
 194
 195         //===== Driver self defined for old channel plan Compatible ,Remember to modify if have new channel plan definition =====
 196         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},21},                             // 0x20, RT_CHANNEL_DOMAIN_5G_FCC
 197         {{36,40,44,48},4},                                                                                                                                                      // 0x21, RT_CHANNEL_DOMAIN_5G_JAPAN_NO_DFS
 198         {{36,40,44,48,149,153,157,161},8},                                                                                                                      // 0x22, RT_CHANNEL_DOMAIN_5G_FCC4_NO_DFS
 199 };
 200
 201 static RT_CHANNEL_PLAN_MAP      RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
 202         //===== 0x00 ~ 0x1F , Old Define =====
 203         {0x02,0x20},    //0x00, RT_CHANNEL_DOMAIN_FCC
 204         {0x02,0x0A},    //0x01, RT_CHANNEL_DOMAIN_IC
 205         {0x01,0x01},    //0x02, RT_CHANNEL_DOMAIN_ETSI
 206         {0x01,0x00},    //0x03, RT_CHANNEL_DOMAIN_SPAIN
 207         {0x01,0x00},    //0x04, RT_CHANNEL_DOMAIN_FRANCE
 208         {0x03,0x00},    //0x05, RT_CHANNEL_DOMAIN_MKK
 209         {0x03,0x00},    //0x06, RT_CHANNEL_DOMAIN_MKK1
 210         {0x01,0x09},    //0x07, RT_CHANNEL_DOMAIN_ISRAEL
 211         {0x03,0x09},    //0x08, RT_CHANNEL_DOMAIN_TELEC
 212         {0x03,0x00},    //0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN
 213         {0x00,0x00},    //0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13
 214         {0x02,0x0F},    //0x0B, RT_CHANNEL_DOMAIN_TAIWAN
 215         {0x01,0x08},    //0x0C, RT_CHANNEL_DOMAIN_CHINA
 216         {0x02,0x06},    //0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO
 217         {0x02,0x0B},    //0x0E, RT_CHANNEL_DOMAIN_KOREA
 218         {0x02,0x09},    //0x0F, RT_CHANNEL_DOMAIN_TURKEY
 219         {0x01,0x01},    //0x10, RT_CHANNEL_DOMAIN_JAPAN
 220         {0x02,0x05},    //0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS
 221         {0x01,0x21},    //0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS
 222         {0x00,0x04},    //0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G
 223         {0x02,0x10},    //0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS
 224         {0x00,0x21},    //0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS
 225         {0x00,0x22},    //0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS
 226         {0x03,0x21},    //0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS
 227         {0x06,0x08},    //0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS
 228         {0x02,0x08},    //0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS
 229         {0x00,0x00},    //0x1A,
 230         {0x00,0x00},    //0x1B,
 231         {0x00,0x00},    //0x1C,
 232         {0x00,0x00},    //0x1D,
 233         {0x00,0x00},    //0x1E,
 234         {0x06,0x04},    //0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G
 235         //===== 0x20 ~ 0x7F ,New Define =====
 236         {0x00,0x00},    //0x20, RT_CHANNEL_DOMAIN_WORLD_NULL
 237         {0x01,0x00},    //0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL
 238         {0x02,0x00},    //0x22, RT_CHANNEL_DOMAIN_FCC1_NULL
 239         {0x03,0x00},    //0x23, RT_CHANNEL_DOMAIN_MKK1_NULL
 240         {0x04,0x00},    //0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL
 241         {0x02,0x04},    //0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1
 242         {0x00,0x01},    //0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1
 243         {0x03,0x0C},    //0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1
 244         {0x00,0x0B},    //0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1
 245         {0x00,0x05},    //0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2
 246         {0x00,0x00},    //0x2A,
 247         {0x00,0x00},    //0x2B,
 248         {0x00,0x00},    //0x2C,
 249         {0x00,0x00},    //0x2D,
 250         {0x00,0x00},    //0x2E,
 251         {0x00,0x00},    //0x2F,
 252         {0x00,0x06},    //0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3
 253         {0x00,0x07},    //0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4
 254         {0x00,0x08},    //0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5
 255         {0x00,0x09},    //0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6
 256         {0x02,0x0A},    //0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7
 257         {0x00,0x02},    //0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2
 258         {0x00,0x03},    //0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3
 259         {0x03,0x0D},    //0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2
 260         {0x03,0x0E},    //0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3
 261         {0x02,0x0F},    //0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1
 262         {0x00,0x00},    //0x3A,
 263         {0x00,0x00},    //0x3B,
 264         {0x00,0x00},    //0x3C,
 265         {0x00,0x00},    //0x3D,
 266         {0x00,0x00},    //0x3E,
 267         {0x00,0x00},    //0x3F,
 268         {0x02,0x10},    //0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2
 269         {0x05,0x00},    //0x41, RT_CHANNEL_DOMAIN_GLOBAL_NULL
 270         {0x01,0x12},    //0x42, RT_CHANNEL_DOMAIN_ETSI1_ETSI4
 271         {0x02,0x05},    //0x43, RT_CHANNEL_DOMAIN_FCC1_FCC2
 272         {0x02,0x11},    //0x44, RT_CHANNEL_DOMAIN_FCC1_NCC3
 273         {0x00,0x13},    //0x45, RT_CHANNEL_DOMAIN_WORLD_ETSI5
 274         {0x02,0x14},    //0x46, RT_CHANNEL_DOMAIN_FCC1_FCC8
 275         {0x00,0x15},    //0x47, RT_CHANNEL_DOMAIN_WORLD_ETSI6
 276         {0x00,0x16},    //0x48, RT_CHANNEL_DOMAIN_WORLD_ETSI7
 277         {0x00,0x17},    //0x49, RT_CHANNEL_DOMAIN_WORLD_ETSI8
 278         {0x00,0x18},    //0x50, RT_CHANNEL_DOMAIN_WORLD_ETSI9
 279         {0x00,0x19},    //0x51, RT_CHANNEL_DOMAIN_WORLD_ETSI10
 280         {0x00,0x1A},    //0x52, RT_CHANNEL_DOMAIN_WORLD_ETSI11
 281         {0x02,0x1B},    //0x53, RT_CHANNEL_DOMAIN_FCC1_NCC4
 282         {0x00,0x1C},    //0x54, RT_CHANNEL_DOMAIN_WORLD_ETSI12
 283         {0x02,0x1D},    //0x55, RT_CHANNEL_DOMAIN_FCC1_FCC9
 284         {0x00,0x1E},    //0x56, RT_CHANNEL_DOMAIN_WORLD_ETSI13
 285         {0x02,0x1F},    //0x57, RT_CHANNEL_DOMAIN_FCC1_FCC10
 286 };
 287
 288 static RT_CHANNEL_PLAN_MAP      RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03,0x02}; //use the conbination for max channel numbers
 289
 290 /*
 291  * Search the @param ch in given @param ch_set
 292  * @ch_set: the given channel set
 293  * @ch: the given channel number
 294  *
 295  * return the index of channel_num in channel_set, -1 if not found
 296  */
 297 int rtw_ch_set_search_ch(RT_CHANNEL_INFO *ch_set, const u32 ch)
 298 {
 299         int i;
 300         for(i=0;ch_set[i].ChannelNum!=0;i++){
 301                 if(ch == ch_set[i].ChannelNum)
 302                         break;
 303         }
 304
 305         if(i >= ch_set[i].ChannelNum)
 306                 return -1;
 307         return i;
 308 }
 309
 310 /*
 311  * Check the @param ch is fit with setband setting of @param adapter
 312  * @adapter: the given adapter
 313  * @ch: the given channel number
 314  *
 315  * return _TRUE when check valid, _FALSE not valid
 316  */
 317 bool rtw_mlme_band_check(_adapter *adapter, const u32 ch)
 318 {
 319         if (adapter->setband == GHZ24_50 /* 2.4G and 5G */
 320                 || (adapter->setband == GHZ_24 && ch < 35) /* 2.4G only */
 321                 || (adapter->setband == GHZ_50 && ch > 35) /* 5G only */
 322         ) {
 323                 return _TRUE;
 324         }
 325         return _FALSE;
 326 }
 327
 328 /****************************************************************************
 329
 330 Following are the initialization functions for WiFi MLME
 331
 332 *****************************************************************************/
 333
 334 int init_hw_mlme_ext(_adapter *padapter)
 335 {
 336         struct  mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 337
 338         //set_opmode_cmd(padapter, infra_client_with_mlme);//removed
 339
 340         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 341
 342         return _SUCCESS;
 343 }
 344
 345 static void init_mlme_ext_priv_value(_adapter* padapter)
 346 {
 347         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 348         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 349
 350 #ifdef CONFIG_TDLS
 351         u8 i;
 352 #endif
 353
 354         //unsigned char default_channel_set[MAX_CHANNEL_NUM] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 0, 0};
 355         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};
 356         unsigned char   mixed_basicrate[NumRates] ={_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_, _12M_RATE_, _24M_RATE_, 0xff,};
 357
 358         ATOMIC_SET(&pmlmeext->event_seq, 0);
 359         pmlmeext->mgnt_seq = 0;//reset to zero when disconnect at client mode
 360 #ifdef CONFIG_IEEE80211W
 361         pmlmeext->sa_query_seq = 0;
 362         pmlmeext->mgnt_80211w_IPN=0;
 363         pmlmeext->mgnt_80211w_IPN_rx=0;
 364 #endif //CONFIG_IEEE80211W
 365         pmlmeext->cur_channel = padapter->registrypriv.channel;
 366         pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
 367         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 368
 369         pmlmeext->retry = 0;
 370
 371         pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
 372
 373         //_rtw_memcpy(pmlmeext->channel_set, DefaultChannelPlan[padapter->mlmepriv.ChannelPlan].Channel, DefaultChannelPlan[padapter->mlmepriv.ChannelPlan].Len);
 374         //_rtw_memcpy(pmlmeext->channel_set, default_channel_set, MAX_CHANNEL_NUM);
 375         _rtw_memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
 376         _rtw_memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
 377
 378         if(pmlmeext->cur_channel > 14)
 379                 pmlmeext->tx_rate = IEEE80211_OFDM_RATE_6MB;
 380         else
 381                 pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
 382
 383         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
 384         pmlmeext->sitesurvey_res.channel_idx = 0;
 385         pmlmeext->sitesurvey_res.bss_cnt = 0;
 386         pmlmeext->scan_abort = _FALSE;
 387
 388         pmlmeinfo->state = WIFI_FW_NULL_STATE;
 389         pmlmeinfo->reauth_count = 0;
 390         pmlmeinfo->reassoc_count = 0;
 391         pmlmeinfo->link_count = 0;
 392         pmlmeinfo->auth_seq = 0;
 393         pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
 394         pmlmeinfo->key_index = 0;
 395         pmlmeinfo->iv = 0;
 396
 397         pmlmeinfo->enc_algo = _NO_PRIVACY_;
 398         pmlmeinfo->authModeToggle = 0;
 399
 400         _rtw_memset(pmlmeinfo->chg_txt, 0, 128);
 401
 402         pmlmeinfo->slotTime = SHORT_SLOT_TIME;
 403         pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
 404
 405         pmlmeinfo->dialogToken = 0;
 406
 407         pmlmeext->action_public_rxseq = 0xffff;
 408         pmlmeext->action_public_dialog_token = 0xff;
 409 }
 410
 411 static int has_channel(RT_CHANNEL_INFO *channel_set,
 412                                            u8 chanset_size,
 413                                            u8 chan) {
 414         int i;
 415
 416         for (i = 0; i < chanset_size; i++) {
 417                 if (channel_set[i].ChannelNum == chan) {
 418                         return 1;
 419                 }
 420         }
 421
 422         return 0;
 423 }
 424
 425 static void init_channel_list(_adapter *padapter, RT_CHANNEL_INFO *channel_set,
 426                                                           u8 chanset_size,
 427                                                           struct p2p_channels *channel_list) {
 428
 429         struct p2p_oper_class_map op_class[] = {
 430                 { IEEE80211G,  81,   1,  13,  1, BW20 },
 431                 { IEEE80211G,  82,  14,  14,  1, BW20 },
 432 #if 0 /* Do not enable HT40 on 2 GHz */
 433                 { IEEE80211G,  83,   1,   9,  1, BW40PLUS },
 434                 { IEEE80211G,  84,   5,  13,  1, BW40MINUS },
 435 #endif
 436                 { IEEE80211A, 115,  36,  48,  4, BW20 },
 437                 { IEEE80211A, 116,  36,  44,  8, BW40PLUS },
 438                 { IEEE80211A, 117,  40,  48,  8, BW40MINUS },
 439                 { IEEE80211A, 124, 149, 161,  4, BW20 },
 440                 { IEEE80211A, 125, 149, 169,  4, BW20 },
 441                 { IEEE80211A, 126, 149, 157,  8, BW40PLUS },
 442                 { IEEE80211A, 127, 153, 161,  8, BW40MINUS },
 443                 { -1, 0, 0, 0, 0, BW20 }
 444         };
 445
 446         int cla, op;
 447
 448         cla = 0;
 449
 450         for (op = 0; op_class[op].op_class; op++) {
 451                 u8 ch;
 452                 struct p2p_oper_class_map *o = &op_class[op];
 453                 struct p2p_reg_class *reg = NULL;
 454
 455                 for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
 456                         if (!has_channel(channel_set, chanset_size, ch)) {
 457                                 continue;
 458                         }
 459
 460                         if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
 461                                 continue;
 462
 463                         if ((0 < (padapter->registrypriv.bw_mode & 0xf0)) &&
 464                                 ((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
 465                                 continue;
 466
 467                         if (reg == NULL) {
 468                                 reg = &channel_list->reg_class[cla];
 469                                 cla++;
 470                                 reg->reg_class = o->op_class;
 471                                 reg->channels = 0;
 472                         }
 473                         reg->channel[reg->channels] = ch;
 474                         reg->channels++;
 475                 }
 476         }
 477         channel_list->reg_classes = cla;
 478
 479 }
 480
 481 static u8 init_channel_set(_adapter* padapter, u8 ChannelPlan, RT_CHANNEL_INFO *channel_set)
 482 {
 483         u8      index,chanset_size = 0;
 484         u8      b5GBand = _FALSE, b2_4GBand = _FALSE;
 485         u8      Index2G = 0, Index5G=0;
 486
 487         _rtw_memset(channel_set, 0, sizeof(RT_CHANNEL_INFO)*MAX_CHANNEL_NUM);
 488
 489         if(ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE)
 490         {
 491                 DBG_871X("ChannelPlan ID %x error !!!!!\n",ChannelPlan);
 492                 return chanset_size;
 493         }
 494
 495         if(IsSupported24G(padapter->registrypriv.wireless_mode))
 496         {
 497                 b2_4GBand = _TRUE;
 498                 if(RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
 499                         Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
 500                 else
 501                         Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
 502         }
 503
 504         if(IsSupported5G(padapter->registrypriv.wireless_mode))
 505         {
 506                 b5GBand = _TRUE;
 507                 if(RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
 508                         Index5G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index5G;
 509                 else
 510                         Index5G = RTW_ChannelPlanMap[ChannelPlan].Index5G;
 511         }
 512
 513         if(b2_4GBand)
 514         {
 515                 for(index=0;index<RTW_ChannelPlan2G[Index2G].Len;index++)
 516                 {
 517                         channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
 518
 519                         if(     (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||//Channel 1~11 is active, and 12~14 is passive
 520                                 (RT_CHANNEL_DOMAIN_GLOBAL_NULL == ChannelPlan)  )
 521                         {
 522                                 if(channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
 523                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 524                                 else if((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
 525                                         channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
 526                         }
 527                         else if(RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
 528                                 RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan ||
 529                                 RT_CHANNEL_DOMAIN_2G_WORLD == Index2G)// channel 12~13, passive scan
 530                         {
 531                                 if(channel_set[chanset_size].ChannelNum <= 11)
 532                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 533                                 else
 534                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 535                         }
 536                         else
 537                         {
 538                                 channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 539                         }
 540
 541                         chanset_size++;
 542                 }
 543         }
 544
 545         if(b5GBand)
 546         {
 547                 for(index=0;index<RTW_ChannelPlan5G[Index5G].Len;index++)
 548                 {
 549 #ifdef CONFIG_DFS
 550                         channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
 551                         if ( channel_set[chanset_size].ChannelNum <= 48
 552                                 || channel_set[chanset_size].ChannelNum >= 149 )
 553                         {
 554                                 if(RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)//passive scan for all 5G channels
 555                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 556                                 else
 557                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 558                         }
 559                         else
 560                         {
 561                                 channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 562                         }
 563                         chanset_size++;
 564 #else /* CONFIG_DFS */
 565                         if ( RTW_ChannelPlan5G[Index5G].Channel[index] <= 48
 566                                 || RTW_ChannelPlan5G[Index5G].Channel[index] >= 149 ) {
 567                                 channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
 568                                 if(RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)//passive scan for all 5G channels
 569                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 570                                 else
 571                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 572                                 DBG_871X("%s(): channel_set[%d].ChannelNum = %d\n", __FUNCTION__, chanset_size, channel_set[chanset_size].ChannelNum);
 573                                 chanset_size++;
 574                         }
 575 #endif /* CONFIG_DFS */
 576                 }
 577         }
 578
 579         DBG_871X("%s ChannelPlan ID %x Chan num:%d  \n",__FUNCTION__,ChannelPlan,chanset_size);
 580         return chanset_size;
 581 }
 582
 583 int     init_mlme_ext_priv(_adapter* padapter)
 584 {
 585         int     res = _SUCCESS;
 586         struct registry_priv* pregistrypriv = &padapter->registrypriv;
 587         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 588         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
 589         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 590
 591         // We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc().
 592         //_rtw_memset((u8 *)pmlmeext, 0, sizeof(struct mlme_ext_priv));
 593
 594         pmlmeext->padapter = padapter;
 595
 596         //fill_fwpriv(padapter, &(pmlmeext->fwpriv));
 597
 598         init_mlme_ext_priv_value(padapter);
 599         pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
 600
 601         init_mlme_ext_timer(padapter);
 602
 603 #ifdef CONFIG_AP_MODE
 604         init_mlme_ap_info(padapter);
 605 #endif
 606
 607         pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan,pmlmeext->channel_set);
 608         init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
 609         pmlmeext->last_scan_time = 0;
 610         pmlmeext->chan_scan_time = SURVEY_TO;
 611         pmlmeext->mlmeext_init = _TRUE;
 612
 613
 614 #ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
 615         pmlmeext->active_keep_alive_check = _TRUE;
 616 #else
 617         pmlmeext->active_keep_alive_check = _FALSE;
 618 #endif
 619
 620 #ifdef DBG_FIXED_CHAN
 621         pmlmeext->fixed_chan = 0xFF;
 622 #endif
 623
 624         return res;
 625
 626 }
 627
 628 void free_mlme_ext_priv (struct mlme_ext_priv *pmlmeext)
 629 {
 630         _adapter *padapter = pmlmeext->padapter;
 631
 632         if (!padapter)
 633                 return;
 634
 635         if (padapter->bDriverStopped == _TRUE)
 636         {
 637                 _cancel_timer_ex(&pmlmeext->survey_timer);
 638                 _cancel_timer_ex(&pmlmeext->link_timer);
 639                 //_cancel_timer_ex(&pmlmeext->ADDBA_timer);
 640         }
 641 }
 642
 643 static u8 cmp_pkt_chnl_diff(_adapter *padapter,u8* pframe,uint packet_len)
 644 {       // if the channel is same, return 0. else return channel differential
 645         uint len;
 646         u8 channel;
 647         u8 *p;
 648         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_, _DSSET_IE_, &len, packet_len - _BEACON_IE_OFFSET_);
 649         if (p)
 650         {
 651                 channel = *(p + 2);
 652                 if(padapter->mlmeextpriv.cur_channel >= channel)
 653                 {
 654                         return (padapter->mlmeextpriv.cur_channel - channel);
 655                 }
 656                 else
 657                 {
 658                         return (channel-padapter->mlmeextpriv.cur_channel);
 659                 }
 660         }
 661         else
 662         {
 663                 return 0;
 664         }
 665 }
 666
 667 static void _mgt_dispatcher(_adapter *padapter, struct mlme_handler *ptable, union recv_frame *precv_frame)
 668 {
 669         u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
 670         u8 *pframe = precv_frame->u.hdr.rx_data;
 671
 672           if(ptable->func)
 673         {
 674          //receive the frames that ra(a1) is my address or ra(a1) is bc address.
 675                 if (!_rtw_memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
 676                         !_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
 677                 {
 678                         return;
 679                 }
 680
 681                 ptable->func(padapter, precv_frame);
 682         }
 683
 684 }
 685
 686 void mgt_dispatcher(_adapter *padapter, union recv_frame *precv_frame)
 687 {
 688         int index;
 689         struct mlme_handler *ptable;
 690 #ifdef CONFIG_AP_MODE
 691         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 692 #endif //CONFIG_AP_MODE
 693         u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
 694         u8 *pframe = precv_frame->u.hdr.rx_data;
 695         struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));
 696
 697         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 698                  ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
 699                   GetFrameType(pframe), GetFrameSubType(pframe)));
 700
 701 #if 0
 702         {
 703                 u8 *pbuf;
 704                 pbuf = GetAddr1Ptr(pframe);
 705                 DBG_871X("A1-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
 706                 pbuf = GetAddr2Ptr(pframe);
 707                 DBG_871X("A2-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
 708                 pbuf = GetAddr3Ptr(pframe);
 709                 DBG_871X("A3-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
 710         }
 711 #endif
 712
 713         if (GetFrameType(pframe) != WIFI_MGT_TYPE)
 714         {
 715                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
 716                 return;
 717         }
 718
 719         //receive the frames that ra(a1) is my address or ra(a1) is bc address.
 720         if (!_rtw_memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
 721                 !_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
 722         {
 723                 return;
 724         }
 725
 726         ptable = mlme_sta_tbl;
 727
 728         index = GetFrameSubType(pframe) >> 4;
 729
 730 #ifdef CONFIG_TDLS
 731         if((index << 4)==WIFI_ACTION){
 732                 //category==public (4), action==TDLS_DISCOVERY_RESPONSE
 733                 if(*(pframe+24)==0x04 && *(pframe+25)==TDLS_DISCOVERY_RESPONSE){
 734                         DBG_871X("recv tdls discovery response frame\n");
 735                         On_TDLS_Dis_Rsp(padapter, precv_frame);
 736                 }
 737         }
 738 #endif //CONFIG_TDLS
 739
 740         if (index > 13)
 741         {
 742                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Currently we do not support reserved sub-fr-type=%d\n", index));
 743                 return;
 744         }
 745         ptable += index;
 746
 747 #if 1
 748         if (psta != NULL)
 749         {
 750                 if (GetRetry(pframe))
 751                 {
 752                         if (precv_frame->u.hdr.attrib.seq_num == psta->RxMgmtFrameSeqNum)
 753                         {
 754                                 /* drop the duplicate management frame */
 755                                 DBG_871X("Drop duplicate management frame with seq_num = %d.\n", precv_frame->u.hdr.attrib.seq_num);
 756                                 return;
 757                         }
 758                 }
 759                 psta->RxMgmtFrameSeqNum = precv_frame->u.hdr.attrib.seq_num;
 760         }
 761 #else
 762
 763         if(GetRetry(pframe))
 764         {
 765                 //RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("drop due to decache!\n"));
 766                 //return;
 767         }
 768 #endif
 769
 770 #ifdef CONFIG_AP_MODE
 771         switch (GetFrameSubType(pframe))
 772         {
 773                 case WIFI_AUTH:
 774                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 775                                 ptable->func = &OnAuth;
 776                         else
 777                                 ptable->func = &OnAuthClient;
 778                         //pass through
 779                 case WIFI_ASSOCREQ:
 780                 case WIFI_REASSOCREQ:
 781                         _mgt_dispatcher(padapter, ptable, precv_frame);
 782 #ifdef CONFIG_HOSTAPD_MLME
 783                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 784                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 785 #endif
 786                         break;
 787                 case WIFI_PROBEREQ:
 788                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 789                         {
 790 #ifdef CONFIG_HOSTAPD_MLME
 791                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 792 #else
 793                                 _mgt_dispatcher(padapter, ptable, precv_frame);
 794 #endif
 795                         }
 796                         else
 797                                 _mgt_dispatcher(padapter, ptable, precv_frame);
 798                         break;
 799                 case WIFI_BEACON:
 800                         _mgt_dispatcher(padapter, ptable, precv_frame);
 801                         break;
 802                 case WIFI_ACTION:
 803                         //if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 804                         _mgt_dispatcher(padapter, ptable, precv_frame);
 805                         break;
 806                 default:
 807                         _mgt_dispatcher(padapter, ptable, precv_frame);
 808                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 809                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 810                         break;
 811         }
 812 #else
 813
 814         _mgt_dispatcher(padapter, ptable, precv_frame);
 815
 816 #endif
 817
 818 }
 819
 820 #ifdef CONFIG_P2P
 821 u32 p2p_listen_state_process(_adapter *padapter, unsigned char *da)
 822 {
 823         bool response = _TRUE;
 824
 825 #ifdef CONFIG_IOCTL_CFG80211
 826         if( padapter->wdinfo.driver_interface == DRIVER_CFG80211 )
 827         {
 828                 if(padapter->cfg80211_wdinfo.is_ro_ch == _FALSE
 829                         || rtw_get_oper_ch(padapter) != padapter->wdinfo.listen_channel
 830                         || adapter_wdev_data(padapter)->p2p_enabled == _FALSE
 831                         || padapter->mlmepriv.wps_probe_resp_ie == NULL
 832                         || padapter->mlmepriv.p2p_probe_resp_ie == NULL
 833                 )
 834                 {
 835 #ifdef CONFIG_DEBUG_CFG80211
 836                         DBG_871X("DON'T issue_probersp_p2p: p2p_enabled:%d, wps_probe_resp_ie:%p, p2p_probe_resp_ie:%p, ",
 837                                 adapter_wdev_data(padapter)->p2p_enabled,
 838                                 padapter->mlmepriv.wps_probe_resp_ie,
 839                                 padapter->mlmepriv.p2p_probe_resp_ie);
 840                         DBG_871X("is_ro_ch:%d, op_ch:%d, p2p_listen_channel:%d\n",
 841                                 padapter->cfg80211_wdinfo.is_ro_ch,
 842                                 rtw_get_oper_ch(padapter),
 843                                 padapter->wdinfo.listen_channel);
 844 #endif
 845                         response = _FALSE;
 846                 }
 847         }
 848         else
 849 #endif //CONFIG_IOCTL_CFG80211
 850         if( padapter->wdinfo.driver_interface == DRIVER_WEXT )
 851         {
 852                 //      do nothing if the device name is empty
 853                 if ( !padapter->wdinfo.device_name_len )
 854                 {
 855                         response        = _FALSE;
 856                 }
 857         }
 858
 859         if (response == _TRUE)
 860                 issue_probersp_p2p( padapter, da);
 861
 862         return _SUCCESS;
 863 }
 864 #endif //CONFIG_P2P
 865
 866
 867 /****************************************************************************
 868
 869 Following are the callback functions for each subtype of the management frames
 870
 871 *****************************************************************************/
 872
 873 unsigned int OnProbeReq(_adapter *padapter, union recv_frame *precv_frame)
 874 {
 875         unsigned int    ielen;
 876         unsigned char   *p;
 877         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 878         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 879         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 880         WLAN_BSSID_EX   *cur = &(pmlmeinfo->network);
 881         u8 *pframe = precv_frame->u.hdr.rx_data;
 882         uint len = precv_frame->u.hdr.len;
 883         u8 is_valid_p2p_probereq = _FALSE;
 884
 885 #ifdef CONFIG_ATMEL_RC_PATCH
 886         u8 *target_ie=NULL, *wps_ie=NULL;
 887         u8 *start;
 888         uint search_len = 0, wps_ielen = 0, target_ielen = 0;
 889         struct sta_info *psta;
 890         struct sta_priv *pstapriv = &padapter->stapriv;
 891 #endif
 892
 893
 894 #ifdef CONFIG_P2P
 895         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
 896         struct rx_pkt_attrib    *pattrib = &precv_frame->u.hdr.attrib;
 897         u8 wifi_test_chk_rate = 1;
 898
 899         if (    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
 900                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) &&
 901                 !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
 902                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
 903                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)
 904            )
 905         {
 906                 //      Commented by Albert 2011/03/17
 907                 //      mcs_rate = 0 -> CCK 1M rate
 908                 //      mcs_rate = 1 -> CCK 2M rate
 909                 //      mcs_rate = 2 -> CCK 5.5M rate
 910                 //      mcs_rate = 3 -> CCK 11M rate
 911                 //      In the P2P mode, the driver should not support the CCK rate
 912
 913                 //      Commented by Kurt 2012/10/16
 914                 //      IOT issue: Google Nexus7 use 1M rate to send p2p_probe_req after GO nego completed and Nexus7 is client
 915 #ifdef CONFIG_WIFI_TEST
 916                 if ( pattrib->data_rate <= 3 )
 917                 {
 918                         wifi_test_chk_rate = 0;
 919                 }
 920 #endif //CONFIG_WIFI_TEST
 921
 922                 if( wifi_test_chk_rate == 1 )
 923                 {
 924                         if((is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len)) == _TRUE)
 925                         {
 926                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE))
 927                                 {
 928                                         // FIXME
 929                                         if( padapter->wdinfo.driver_interface == DRIVER_WEXT )
 930                                                 report_survey_event(padapter, precv_frame);
 931
 932                                         p2p_listen_state_process( padapter,  get_sa(pframe));
 933
 934                                         return _SUCCESS;
 935                                 }
 936
 937                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
 938                                 {
 939                                         goto _continue;
 940                                 }
 941                         }
 942                 }
 943         }
 944
 945 _continue:
 946 #endif //CONFIG_P2P
 947
 948         if(check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 949         {
 950                 return _SUCCESS;
 951         }
 952
 953         if(check_fwstate(pmlmepriv, _FW_LINKED) == _FALSE &&
 954                 check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE)==_FALSE)
 955         {
 956                 return _SUCCESS;
 957         }
 958
 959
 960         //DBG_871X("+OnProbeReq\n");
 961
 962
 963 #ifdef CONFIG_ATMEL_RC_PATCH
 964                 if ((wps_ie = rtw_get_wps_ie(
 965                         pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_,
 966                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_,
 967                          NULL, &wps_ielen))) {
 968
 969                         target_ie = rtw_get_wps_attr_content( wps_ie, wps_ielen, WPS_ATTR_MANUFACTURER, NULL, &target_ielen);
 970                 }
 971                 if ((target_ie && (target_ielen == 4)) && (_TRUE ==_rtw_memcmp((void *)target_ie, "Ozmo",4 ))) {
 972                         //psta->flag_atmel_rc = 1;
 973                         unsigned char *sa_addr = get_sa(pframe);
 974                         printk("%s: Find Ozmo RC -- %02x:%02x:%02x:%02x:%02x:%02x  \n\n",
 975                                 __func__, *sa_addr, *(sa_addr+1), *(sa_addr+2), *(sa_addr+3), *(sa_addr+4), *(sa_addr+5));
 976                         _rtw_memcpy(  pstapriv->atmel_rc_pattern, get_sa(pframe), ETH_ALEN);
 977                 }
 978 #endif
 979
 980
 981 #ifdef CONFIG_AUTO_AP_MODE
 982         if(check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE &&
 983                         pmlmepriv->cur_network.join_res == _TRUE)
 984         {
 985                 _irqL   irqL;
 986                 struct sta_info *psta;
 987                 u8 *mac_addr, *peer_addr;
 988                 struct sta_priv *pstapriv = &padapter->stapriv;
 989                 u8 RC_OUI[4]={0x00,0xE0,0x4C,0x0A};
 990                 //EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2]
 991
 992                 p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _VENDOR_SPECIFIC_IE_, (int *)&ielen,
 993                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
 994
 995                 if(!p || ielen !=14)
 996                         goto _non_rc_device;
 997
 998                 if(!_rtw_memcmp(p+2, RC_OUI, sizeof(RC_OUI)))
 999                         goto _non_rc_device;
1000
1001                 if(!_rtw_memcmp(p+6, get_sa(pframe), ETH_ALEN))
1002                 {
1003                         DBG_871X("%s, do rc pairing ("MAC_FMT"), but mac addr mismatch!("MAC_FMT")\n", __FUNCTION__,
1004                                 MAC_ARG(get_sa(pframe)), MAC_ARG(p+6));
1005
1006                         goto _non_rc_device;
1007                 }
1008
1009                 DBG_871X("%s, got the pairing device("MAC_FMT")\n", __FUNCTION__,  MAC_ARG(get_sa(pframe)));
1010
1011                 //new a station
1012                 psta = rtw_get_stainfo(pstapriv, get_sa(pframe));
1013                 if (psta == NULL)
1014                 {
1015                         // allocate a new one
1016                         DBG_871X("going to alloc stainfo for rc="MAC_FMT"\n",  MAC_ARG(get_sa(pframe)));
1017                         psta = rtw_alloc_stainfo(pstapriv, get_sa(pframe));
1018                         if (psta == NULL)
1019                         {
1020                                 //TODO:
1021                                 DBG_871X(" Exceed the upper limit of supported clients...\n");
1022                                 return _SUCCESS;
1023                         }
1024
1025                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1026                         if (rtw_is_list_empty(&psta->asoc_list))
1027                         {
1028                                 psta->expire_to = pstapriv->expire_to;
1029                                 rtw_list_insert_tail(&psta->asoc_list, &pstapriv->asoc_list);
1030                                 pstapriv->asoc_list_cnt++;
1031                         }
1032                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1033
1034                         //generate pairing ID
1035                         mac_addr = myid(&(padapter->eeprompriv));
1036                         peer_addr = psta->hwaddr;
1037                         psta->pid = (u16)(((mac_addr[4]<<8) + mac_addr[5]) + ((peer_addr[4]<<8) + peer_addr[5]));
1038
1039                         //update peer stainfo
1040                         psta->isrc = _TRUE;
1041                         //psta->aid = 0;
1042                         //psta->mac_id = 2;
1043
1044                         /* get a unique AID */
1045                         if (psta->aid > 0) {
1046                                 DBG_871X("old AID %d\n", psta->aid);
1047                         } else {
1048                                 for (psta->aid = 1; psta->aid <= NUM_STA; psta->aid++)
1049                                         if (pstapriv->sta_aid[psta->aid - 1] == NULL)
1050                                                 break;
1051
1052                                 if (psta->aid > pstapriv->max_num_sta) {
1053                                         psta->aid = 0;
1054                                         DBG_871X("no room for more AIDs\n");
1055                                         return _SUCCESS;
1056                                 } else {
1057                                         pstapriv->sta_aid[psta->aid - 1] = psta;
1058                                         DBG_871X("allocate new AID = (%d)\n", psta->aid);
1059                                 }
1060                         }
1061
1062                         psta->qos_option = 1;
1063                         psta->bw_mode = CHANNEL_WIDTH_20;
1064                         psta->ieee8021x_blocked = _FALSE;
1065 #ifdef CONFIG_80211N_HT
1066                         psta->htpriv.ht_option = _TRUE;
1067                         psta->htpriv.ampdu_enable = _FALSE;
1068                         psta->htpriv.sgi_20m = _FALSE;
1069                         psta->htpriv.sgi_40m = _FALSE;
1070                         psta->htpriv.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
1071                         psta->htpriv.agg_enable_bitmap = 0x0;//reset
1072                         psta->htpriv.candidate_tid_bitmap = 0x0;//reset
1073 #endif
1074
1075                         rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, _TRUE);
1076
1077                         _rtw_memset((void*)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
1078
1079                         _enter_critical_bh(&psta->lock, &irqL);
1080                         psta->state |= _FW_LINKED;
1081                         _exit_critical_bh(&psta->lock, &irqL);
1082
1083                         report_add_sta_event(padapter, psta->hwaddr, psta->aid);
1084
1085                 }
1086
1087                 issue_probersp(padapter, get_sa(pframe), _FALSE);
1088
1089                 return _SUCCESS;
1090
1091         }
1092
1093 _non_rc_device:
1094
1095         return _SUCCESS;
1096
1097 #endif //CONFIG_AUTO_AP_MODE
1098
1099
1100 #ifdef CONFIG_CONCURRENT_MODE
1101         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
1102                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
1103         {
1104                 //don't process probe req
1105                 return _SUCCESS;
1106         }
1107 #endif
1108
1109         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
1110                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
1111
1112
1113         //check (wildcard) SSID
1114         if (p != NULL)
1115         {
1116                 if(is_valid_p2p_probereq == _TRUE)
1117                 {
1118                         goto _issue_probersp;
1119                 }
1120
1121                 if ( (ielen != 0 && _FALSE ==_rtw_memcmp((void *)(p+2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength))
1122                         || (ielen == 0 && pmlmeinfo->hidden_ssid_mode)
1123                 )
1124                 {
1125                         return _SUCCESS;
1126                 }
1127
1128 _issue_probersp:
1129                 if(((check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE &&
1130                         pmlmepriv->cur_network.join_res == _TRUE)) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
1131                 {
1132                         //DBG_871X("+issue_probersp during ap mode\n");
1133                         issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
1134                 }
1135
1136         }
1137
1138         return _SUCCESS;
1139
1140 }
1141
1142 unsigned int OnProbeRsp(_adapter *padapter, union recv_frame *precv_frame)
1143 {
1144         struct sta_info         *psta;
1145         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1146         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1147         struct sta_priv         *pstapriv = &padapter->stapriv;
1148         u8      *pframe = precv_frame->u.hdr.rx_data;
1149 #ifdef CONFIG_P2P
1150         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
1151 #endif
1152
1153
1154 #ifdef CONFIG_P2P
1155         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
1156         {
1157                 if ( _TRUE == pwdinfo->tx_prov_disc_info.benable )
1158                 {
1159                         if( _rtw_memcmp( pwdinfo->tx_prov_disc_info.peerIFAddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
1160                         {
1161                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT))
1162                                 {
1163                                         pwdinfo->tx_prov_disc_info.benable = _FALSE;
1164                                         issue_p2p_provision_request( padapter,
1165                                                                                                 pwdinfo->tx_prov_disc_info.ssid.Ssid,
1166                                                                                                 pwdinfo->tx_prov_disc_info.ssid.SsidLength,
1167                                                                                                 pwdinfo->tx_prov_disc_info.peerDevAddr );
1168                                 }
1169                                 else if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
1170                                 {
1171                                         pwdinfo->tx_prov_disc_info.benable = _FALSE;
1172                                         issue_p2p_provision_request( padapter,
1173                                                                                                 NULL,
1174                                                                                                 0,
1175                                                                                                 pwdinfo->tx_prov_disc_info.peerDevAddr );
1176                                 }
1177                         }
1178                 }
1179                 return _SUCCESS;
1180         }
1181         else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
1182         {
1183                 if ( _TRUE == pwdinfo->nego_req_info.benable )
1184                 {
1185                         DBG_871X( "[%s] P2P State is GONEGO ING!\n", __FUNCTION__ );
1186                         if( _rtw_memcmp( pwdinfo->nego_req_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
1187                         {
1188                                 pwdinfo->nego_req_info.benable = _FALSE;
1189                                 issue_p2p_GO_request( padapter, pwdinfo->nego_req_info.peerDevAddr);
1190                         }
1191                 }
1192         }
1193         else if( rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ ) )
1194         {
1195                 if ( _TRUE == pwdinfo->invitereq_info.benable )
1196                 {
1197                         DBG_871X( "[%s] P2P_STATE_TX_INVITE_REQ!\n", __FUNCTION__ );
1198                         if( _rtw_memcmp( pwdinfo->invitereq_info.peer_macaddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
1199                         {
1200                                 pwdinfo->invitereq_info.benable = _FALSE;
1201                                 issue_p2p_invitation_request( padapter, pwdinfo->invitereq_info.peer_macaddr );
1202                         }
1203                 }
1204         }
1205 #endif
1206
1207
1208         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
1209         {
1210                 report_survey_event(padapter, precv_frame);
1211 #ifdef CONFIG_CONCURRENT_MODE
1212                 report_survey_event(padapter->pbuddy_adapter, precv_frame);
1213 #endif
1214 #ifdef CONFIG_DUALMAC_CONCURRENT
1215                 dc_report_survey_event(padapter, precv_frame);
1216 #endif
1217                 return _SUCCESS;
1218         }
1219
1220         #if 0 //move to validate_recv_mgnt_frame
1221         if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
1222         {
1223                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
1224                 {
1225                         if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
1226                         {
1227                                 psta->sta_stats.rx_mgnt_pkts++;
1228                         }
1229                 }
1230         }
1231         #endif
1232
1233         return _SUCCESS;
1234
1235 }
1236
1237 unsigned int OnBeacon(_adapter *padapter, union recv_frame *precv_frame)
1238 {
1239         int cam_idx;
1240         struct sta_info *psta;
1241         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1242         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1243         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1244         struct sta_priv *pstapriv = &padapter->stapriv;
1245         u8 *pframe = precv_frame->u.hdr.rx_data;
1246         uint len = precv_frame->u.hdr.len;
1247         WLAN_BSSID_EX *pbss;
1248         int ret = _SUCCESS;
1249         u8 *p = NULL;
1250         u32 ielen = 0;
1251
1252 #ifdef CONFIG_ATTEMPT_TO_FIX_AP_BEACON_ERROR
1253         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_);
1254         if ((p != NULL) && (ielen > 0))
1255         {
1256                 if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D))
1257                 {
1258                         /* Invalid value 0x2D is detected in Extended Supported Rates (ESR) IE. Try to fix the IE length to avoid failed Beacon parsing. */
1259                         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)));
1260                         *(p + 1) = ielen - 1;
1261                 }
1262         }
1263 #endif
1264
1265         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
1266         {
1267                 report_survey_event(padapter, precv_frame);
1268 #ifdef CONFIG_CONCURRENT_MODE
1269                 report_survey_event(padapter->pbuddy_adapter, precv_frame);
1270 #endif
1271
1272 #ifdef CONFIG_DUALMAC_CONCURRENT
1273                 dc_report_survey_event(padapter, precv_frame);
1274 #endif
1275
1276                 return _SUCCESS;
1277         }
1278
1279         if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
1280         {
1281                 if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
1282                 {
1283                         //we should update current network before auth, or some IE is wrong
1284                         pbss = (WLAN_BSSID_EX*)rtw_malloc(sizeof(WLAN_BSSID_EX));
1285                         if (pbss) {
1286                                 if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
1287                                         update_network(&(pmlmepriv->cur_network.network), pbss, padapter, _TRUE);
1288                                         rtw_get_bcn_info(&(pmlmepriv->cur_network));
1289                                 }
1290                                 rtw_mfree((u8*)pbss, sizeof(WLAN_BSSID_EX));
1291                         }
1292
1293                         //check the vendor of the assoc AP
1294                         pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct rtw_ieee80211_hdr_3addr), len-sizeof(struct rtw_ieee80211_hdr_3addr));
1295
1296                         //update TSF Value
1297                         update_TSF(pmlmeext, pframe, len);
1298
1299                         //reset for adaptive_early_32k
1300                         pmlmeext->adaptive_tsf_done = _FALSE;
1301                         pmlmeext->DrvBcnEarly = 0xff;
1302                         pmlmeext->DrvBcnTimeOut = 0xff;
1303                         pmlmeext->bcn_cnt = 0;
1304                         _rtw_memset(pmlmeext->bcn_delay_cnt, 0, sizeof(pmlmeext->bcn_delay_cnt));
1305                         _rtw_memset(pmlmeext->bcn_delay_ratio, 0, sizeof(pmlmeext->bcn_delay_ratio));
1306
1307
1308                         //start auth
1309                         start_clnt_auth(padapter);
1310
1311                         return _SUCCESS;
1312                 }
1313
1314                 if(((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
1315                 {
1316                         if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
1317                         {
1318                                 #ifdef CONFIG_PATCH_JOIN_WRONG_CHANNEL
1319                                 //Merge from 8712 FW code
1320                                 if (cmp_pkt_chnl_diff(padapter,pframe,len) != 0)
1321                                 {            // join wrong channel, deauth and reconnect
1322                                         issue_deauth(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
1323
1324                                         report_del_sta_event(padapter,(&(pmlmeinfo->network))->MacAddress, WLAN_REASON_JOIN_WRONG_CHANNEL);
1325                                         pmlmeinfo->state &= (~WIFI_FW_ASSOC_SUCCESS);
1326                                         return _SUCCESS;
1327                                 }
1328                                 #endif //CONFIG_PATCH_JOIN_WRONG_CHANNEL
1329
1330                                 ret = rtw_check_bcn_info(padapter, pframe, len);
1331                                 if (!ret) {
1332                                                 DBG_871X_LEVEL(_drv_always_, "ap has changed, disconnect now\n ");
1333                                                 receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 0);
1334                                                 return _SUCCESS;
1335                                 }
1336                                 //update WMM, ERP in the beacon
1337                                 //todo: the timer is used instead of the number of the beacon received
1338                                 if ((sta_rx_pkts(psta) & 0xf) == 0)
1339                                 {
1340                                         //DBG_871X("update_bcn_info\n");
1341                                         update_beacon_info(padapter, pframe, len, psta);
1342                                 }
1343
1344                                 adaptive_early_32k(pmlmeext, pframe, len);
1345
1346 #ifdef CONFIG_DFS
1347                                 process_csa_ie(padapter, pframe, len);  //channel switch announcement
1348 #endif //CONFIG_DFS
1349
1350 #ifdef CONFIG_P2P_PS
1351                                 process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
1352 #endif //CONFIG_P2P_PS
1353
1354                                 #if 0 //move to validate_recv_mgnt_frame
1355                                 psta->sta_stats.rx_mgnt_pkts++;
1356                                 #endif
1357                         }
1358                 }
1359                 else if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
1360                 {
1361                         if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
1362                         {
1363                                 //update WMM, ERP in the beacon
1364                                 //todo: the timer is used instead of the number of the beacon received
1365                                 if ((sta_rx_pkts(psta) & 0xf) == 0)
1366                                 {
1367                                         //DBG_871X("update_bcn_info\n");
1368                                         update_beacon_info(padapter, pframe, len, psta);
1369                                 }
1370
1371                                 #if 0 //move to validate_recv_mgnt_frame
1372                                 psta->sta_stats.rx_mgnt_pkts++;
1373                                 #endif
1374                         }
1375                         else
1376                         {
1377                                 //allocate a new CAM entry for IBSS station
1378                                 if ((cam_idx = allocate_fw_sta_entry(padapter)) == NUM_STA)
1379                                 {
1380                                         goto _END_ONBEACON_;
1381                                 }
1382
1383                                 //get supported rate
1384                                 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)
1385                                 {
1386                                         pmlmeinfo->FW_sta_info[cam_idx].status = 0;
1387                                         goto _END_ONBEACON_;
1388                                 }
1389
1390                                 //update TSF Value
1391                                 update_TSF(pmlmeext, pframe, len);
1392
1393                                 //report sta add event
1394                                 report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
1395                         }
1396                 }
1397         }
1398
1399 _END_ONBEACON_:
1400
1401         return _SUCCESS;
1402
1403 }
1404
1405 unsigned int OnAuth(_adapter *padapter, union recv_frame *precv_frame)
1406 {
1407 #ifdef CONFIG_AP_MODE
1408         _irqL irqL;
1409         unsigned int    auth_mode, seq, ie_len;
1410         unsigned char   *sa, *p;
1411         u16     algorithm;
1412         int     status;
1413         static struct sta_info stat;
1414         struct  sta_info        *pstat=NULL;
1415         struct  sta_priv *pstapriv = &padapter->stapriv;
1416         struct security_priv *psecuritypriv = &padapter->securitypriv;
1417         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1418         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1419         u8 *pframe = precv_frame->u.hdr.rx_data;
1420         uint len = precv_frame->u.hdr.len;
1421         u8      offset = 0;
1422
1423
1424 #ifdef CONFIG_CONCURRENT_MODE
1425         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
1426                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
1427         {
1428                 //don't process auth request;
1429                 return _SUCCESS;
1430         }
1431 #endif //CONFIG_CONCURRENT_MODE
1432
1433         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
1434                 return _FAIL;
1435
1436         DBG_871X("+OnAuth\n");
1437
1438         sa = GetAddr2Ptr(pframe);
1439
1440         auth_mode = psecuritypriv->dot11AuthAlgrthm;
1441
1442         if (GetPrivacy(pframe))
1443         {
1444                 u8      *iv;
1445                 struct rx_pkt_attrib     *prxattrib = &(precv_frame->u.hdr.attrib);
1446
1447                 prxattrib->hdrlen = WLAN_HDR_A3_LEN;
1448                 prxattrib->encrypt = _WEP40_;
1449
1450                 iv = pframe+prxattrib->hdrlen;
1451                 prxattrib->key_index = ((iv[3]>>6)&0x3);
1452
1453                 prxattrib->iv_len = 4;
1454                 prxattrib->icv_len = 4;
1455
1456                 rtw_wep_decrypt(padapter, (u8 *)precv_frame);
1457
1458                 offset = 4;
1459         }
1460
1461         algorithm = le16_to_cpu(*(u16*)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset));
1462         seq     = le16_to_cpu(*(u16*)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2));
1463
1464         DBG_871X("auth alg=%x, seq=%X\n", algorithm, seq);
1465
1466         if (auth_mode == 2 &&
1467                         psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
1468                         psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
1469                 auth_mode = 0;
1470
1471         if ((algorithm > 0 && auth_mode == 0) ||        // rx a shared-key auth but shared not enabled
1472                 (algorithm == 0 && auth_mode == 1) )    // rx a open-system auth but shared-key is enabled
1473         {
1474                 DBG_871X("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n",
1475                         algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
1476
1477                 status = _STATS_NO_SUPP_ALG_;
1478
1479                 goto auth_fail;
1480         }
1481
1482 #if 0 //ACL control
1483         phead = &priv->wlan_acl_list;
1484         plist = phead->next;
1485         //check sa
1486         if (acl_mode == 1)              // 1: positive check, only those on acl_list can be connected.
1487                 res = FAIL;
1488         else
1489                 res = SUCCESS;
1490
1491         while(plist != phead)
1492         {
1493                 paclnode = list_entry(plist, struct rtw_wlan_acl_node, list);
1494                 plist = plist->next;
1495                 if (!memcmp((void *)sa, paclnode->addr, 6)) {
1496                         if (paclnode->mode & 2) { // deny
1497                                 res = FAIL;
1498                                 break;
1499                         }
1500                         else {
1501                                 res = SUCCESS;
1502                                 break;
1503                         }
1504                 }
1505         }
1506
1507         if (res != SUCCESS) {
1508                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,"auth abort because ACL!\n");
1509                 return FAIL;
1510         }
1511 #else
1512         if(rtw_access_ctrl(padapter, sa) == _FALSE)
1513         {
1514                 status = _STATS_UNABLE_HANDLE_STA_;
1515                 goto auth_fail;
1516         }
1517 #endif
1518
1519         pstat = rtw_get_stainfo(pstapriv, sa);
1520         if (pstat == NULL)
1521         {
1522
1523                 // allocate a new one
1524                 DBG_871X("going to alloc stainfo for sa="MAC_FMT"\n",  MAC_ARG(sa));
1525                 pstat = rtw_alloc_stainfo(pstapriv, sa);
1526                 if (pstat == NULL)
1527                 {
1528                         DBG_871X(" Exceed the upper limit of supported clients...\n");
1529                         status = _STATS_UNABLE_HANDLE_STA_;
1530                         goto auth_fail;
1531                 }
1532
1533                 pstat->state = WIFI_FW_AUTH_NULL;
1534                 pstat->auth_seq = 0;
1535
1536                 //pstat->flags = 0;
1537                 //pstat->capability = 0;
1538         }
1539         else
1540         {
1541
1542                 _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1543                 if(rtw_is_list_empty(&pstat->asoc_list)==_FALSE)
1544                 {
1545                         rtw_list_delete(&pstat->asoc_list);
1546                         pstapriv->asoc_list_cnt--;
1547                         if (pstat->expire_to > 0)
1548                         {
1549                                 //TODO: STA re_auth within expire_to
1550                         }
1551                 }
1552                 _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1553
1554                 if (seq==1) {
1555                         //TODO: STA re_auth and auth timeout
1556                 }
1557         }
1558
1559         _enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
1560         if (rtw_is_list_empty(&pstat->auth_list))
1561         {
1562
1563                 rtw_list_insert_tail(&pstat->auth_list, &pstapriv->auth_list);
1564                 pstapriv->auth_list_cnt++;
1565         }
1566         _exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
1567
1568         if (pstat->auth_seq == 0)
1569                 pstat->expire_to = pstapriv->auth_to;
1570
1571
1572         if ((pstat->auth_seq + 1) != seq)
1573         {
1574                 DBG_871X("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1575                         seq, pstat->auth_seq+1);
1576                 status = _STATS_OUT_OF_AUTH_SEQ_;
1577                 goto auth_fail;
1578         }
1579
1580         if (algorithm==0 && (auth_mode == 0 || auth_mode == 2 || auth_mode == 3))
1581         {
1582                 if (seq == 1)
1583                 {
1584                         pstat->state &= ~WIFI_FW_AUTH_NULL;
1585                         pstat->state |= WIFI_FW_AUTH_SUCCESS;
1586                         pstat->expire_to = pstapriv->assoc_to;
1587                         pstat->authalg = algorithm;
1588                 }
1589                 else
1590                 {
1591                         DBG_871X("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1592                                 seq, pstat->auth_seq+1);
1593                         status = _STATS_OUT_OF_AUTH_SEQ_;
1594                         goto auth_fail;
1595                 }
1596         }
1597         else // shared system or auto authentication
1598         {
1599                 if (seq == 1)
1600                 {
1601                         //prepare for the challenging txt...
1602
1603                         //get_random_bytes((void *)pstat->chg_txt, 128);//TODO:
1604                         _rtw_memset((void *)pstat->chg_txt, 78, 128);
1605
1606                         pstat->state &= ~WIFI_FW_AUTH_NULL;
1607                         pstat->state |= WIFI_FW_AUTH_STATE;
1608                         pstat->authalg = algorithm;
1609                         pstat->auth_seq = 2;
1610                 }
1611                 else if (seq == 3)
1612                 {
1613                         //checking for challenging txt...
1614                         DBG_871X("checking for challenging txt...\n");
1615
1616                         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len,
1617                                         len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
1618
1619                         if((p==NULL) || (ie_len<=0))
1620                         {
1621                                 DBG_871X("auth rejected because challenge failure!(1)\n");
1622                                 status = _STATS_CHALLENGE_FAIL_;
1623                                 goto auth_fail;
1624                         }
1625
1626                         if (_rtw_memcmp((void *)(p + 2), pstat->chg_txt, 128))
1627                         {
1628                                 pstat->state &= (~WIFI_FW_AUTH_STATE);
1629                                 pstat->state |= WIFI_FW_AUTH_SUCCESS;
1630                                 // challenging txt is correct...
1631                                 pstat->expire_to =  pstapriv->assoc_to;
1632                         }
1633                         else
1634                         {
1635                                 DBG_871X("auth rejected because challenge failure!\n");
1636                                 status = _STATS_CHALLENGE_FAIL_;
1637                                 goto auth_fail;
1638                         }
1639                 }
1640                 else
1641                 {
1642                         DBG_871X("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1643                                 seq, pstat->auth_seq+1);
1644                         status = _STATS_OUT_OF_AUTH_SEQ_;
1645                         goto auth_fail;
1646                 }
1647         }
1648
1649
1650         // Now, we are going to issue_auth...
1651         pstat->auth_seq = seq + 1;
1652
1653 #ifdef CONFIG_NATIVEAP_MLME
1654         issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
1655 #endif
1656
1657         if (pstat->state & WIFI_FW_AUTH_SUCCESS)
1658                 pstat->auth_seq = 0;
1659
1660
1661         return _SUCCESS;
1662
1663 auth_fail:
1664
1665         if(pstat)
1666                 rtw_free_stainfo(padapter , pstat);
1667
1668         pstat = &stat;
1669         _rtw_memset((char *)pstat, '\0', sizeof(stat));
1670         pstat->auth_seq = 2;
1671         _rtw_memcpy(pstat->hwaddr, sa, 6);
1672
1673 #ifdef CONFIG_NATIVEAP_MLME
1674         issue_auth(padapter, pstat, (unsigned short)status);
1675 #endif
1676
1677 #endif
1678         return _FAIL;
1679
1680 }
1681
1682 unsigned int OnAuthClient(_adapter *padapter, union recv_frame *precv_frame)
1683 {
1684         unsigned int    seq, len, status, algthm, offset;
1685         unsigned char   *p;
1686         unsigned int    go2asoc = 0;
1687         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1688         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1689         u8 *pframe = precv_frame->u.hdr.rx_data;
1690         uint pkt_len = precv_frame->u.hdr.len;
1691
1692         DBG_871X("%s\n", __FUNCTION__);
1693
1694         //check A1 matches or not
1695         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
1696                 return _SUCCESS;
1697
1698         if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
1699                 return _SUCCESS;
1700
1701         offset = (GetPrivacy(pframe))? 4: 0;
1702
1703         algthm  = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset));
1704         seq     = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2));
1705         status  = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 4));
1706
1707         if (status != 0)
1708         {
1709                 DBG_871X("clnt auth fail, status: %d\n", status);
1710                 if(status == 13)//&& pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto)
1711                 {
1712                         if(pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1713                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
1714                         else
1715                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
1716                         //pmlmeinfo->reauth_count = 0;
1717                 }
1718
1719                 set_link_timer(pmlmeext, 1);
1720                 goto authclnt_fail;
1721         }
1722
1723         if (seq == 2)
1724         {
1725                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1726                 {
1727                          // legendary shared system
1728                         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
1729                                 pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
1730
1731                         if (p == NULL)
1732                         {
1733                                 //DBG_871X("marc: no challenge text?\n");
1734                                 goto authclnt_fail;
1735                         }
1736
1737                         _rtw_memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
1738                         pmlmeinfo->auth_seq = 3;
1739                         issue_auth(padapter, NULL, 0);
1740                         set_link_timer(pmlmeext, REAUTH_TO);
1741
1742                         return _SUCCESS;
1743                 }
1744                 else
1745                 {
1746                         // open system
1747                         go2asoc = 1;
1748                 }
1749         }
1750         else if (seq == 4)
1751         {
1752                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1753                 {
1754                         go2asoc = 1;
1755                 }
1756                 else
1757                 {
1758                         goto authclnt_fail;
1759                 }
1760         }
1761         else
1762         {
1763                 // this is also illegal
1764                 //DBG_871X("marc: clnt auth failed due to illegal seq=%x\n", seq);
1765                 goto authclnt_fail;
1766         }
1767
1768         if (go2asoc)
1769         {
1770                 DBG_871X_LEVEL(_drv_always_, "auth success, start assoc\n");
1771                 start_clnt_assoc(padapter);
1772                 return _SUCCESS;
1773         }
1774
1775 authclnt_fail:
1776
1777         //pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE);
1778
1779         return _FAIL;
1780
1781 }
1782
1783 unsigned int OnAssocReq(_adapter *padapter, union recv_frame *precv_frame)
1784 {
1785 #ifdef CONFIG_AP_MODE
1786         _irqL irqL;
1787         u16 capab_info, listen_interval;
1788         struct rtw_ieee802_11_elems elems;
1789         struct sta_info *pstat;
1790         unsigned char           reassoc, *p, *pos, *wpa_ie;
1791         unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
1792         int             i, ie_len, wpa_ie_len, left;
1793         unsigned char           supportRate[16];
1794         int                                     supportRateNum;
1795         unsigned short          status = _STATS_SUCCESSFUL_;
1796         unsigned short          frame_type, ie_offset=0;
1797         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1798         struct security_priv *psecuritypriv = &padapter->securitypriv;
1799         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
1800         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1801         WLAN_BSSID_EX   *cur = &(pmlmeinfo->network);
1802         struct sta_priv *pstapriv = &padapter->stapriv;
1803         u8 *pframe = precv_frame->u.hdr.rx_data;
1804         uint pkt_len = precv_frame->u.hdr.len;
1805 #ifdef CONFIG_P2P
1806         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
1807         u8 p2p_status_code = P2P_STATUS_SUCCESS;
1808         u8 *p2pie;
1809         u32 p2pielen = 0;
1810 #ifdef CONFIG_WFD
1811         u8      wfd_ie[ 128 ] = { 0x00 };
1812         u32     wfd_ielen = 0;
1813 #endif // CONFIG_WFD
1814 #endif //CONFIG_P2P
1815
1816 #ifdef CONFIG_CONCURRENT_MODE
1817         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
1818                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
1819         {
1820                 //don't process assoc request;
1821                 return _SUCCESS;
1822         }
1823 #endif //CONFIG_CONCURRENT_MODE
1824
1825         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
1826                 return _FAIL;
1827
1828         frame_type = GetFrameSubType(pframe);
1829         if (frame_type == WIFI_ASSOCREQ)
1830         {
1831                 reassoc = 0;
1832                 ie_offset = _ASOCREQ_IE_OFFSET_;
1833         }
1834         else // WIFI_REASSOCREQ
1835         {
1836                 reassoc = 1;
1837                 ie_offset = _REASOCREQ_IE_OFFSET_;
1838         }
1839
1840
1841         if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
1842                 DBG_871X("handle_assoc(reassoc=%d) - too short payload (len=%lu)"
1843                        "\n", reassoc, (unsigned long)pkt_len);
1844                 return _FAIL;
1845         }
1846
1847         pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
1848         if (pstat == (struct sta_info *)NULL)
1849         {
1850                 status = _RSON_CLS2_;
1851                 goto asoc_class2_error;
1852         }
1853
1854         capab_info = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN);
1855         //capab_info = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
1856         //listen_interval = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN+2));
1857         listen_interval = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN+2);
1858
1859         left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
1860         pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
1861
1862
1863         DBG_871X("%s\n", __FUNCTION__);
1864
1865         // check if this stat has been successfully authenticated/assocated
1866         if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS))
1867         {
1868                 if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS))
1869                 {
1870                         status = _RSON_CLS2_;
1871                         goto asoc_class2_error;
1872                 }
1873                 else
1874                 {
1875                         pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
1876                         pstat->state |= WIFI_FW_ASSOC_STATE;
1877                 }
1878         }
1879         else
1880         {
1881                 pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
1882                 pstat->state |= WIFI_FW_ASSOC_STATE;
1883         }
1884
1885
1886 #if 0// todo:tkip_countermeasures
1887         if (hapd->tkip_countermeasures) {
1888                 resp = WLAN_REASON_MICHAEL_MIC_FAILURE;
1889                 goto fail;
1890         }
1891 #endif
1892
1893         pstat->capability = capab_info;
1894
1895 #if 0//todo:
1896         //check listen_interval
1897         if (listen_interval > hapd->conf->max_listen_interval) {
1898                 hostapd_logger(hapd, mgmt->sa, HOSTAPD_MODULE_IEEE80211,
1899                                HOSTAPD_LEVEL_DEBUG,
1900                                "Too large Listen Interval (%d)",
1901                                listen_interval);
1902                 resp = WLAN_STATUS_ASSOC_DENIED_LISTEN_INT_TOO_LARGE;
1903                 goto fail;
1904         }
1905
1906         pstat->listen_interval = listen_interval;
1907 #endif
1908
1909         //now parse all ieee802_11 ie to point to elems
1910         if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
1911             !elems.ssid) {
1912                 DBG_871X("STA " MAC_FMT " sent invalid association request\n",
1913                        MAC_ARG(pstat->hwaddr));
1914                 status = _STATS_FAILURE_;
1915                 goto OnAssocReqFail;
1916         }
1917
1918
1919         // now we should check all the fields...
1920         // checking SSID
1921         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
1922                 pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1923         if (p == NULL)
1924         {
1925                 status = _STATS_FAILURE_;
1926         }
1927
1928         if (ie_len == 0) // broadcast ssid, however it is not allowed in assocreq
1929                 status = _STATS_FAILURE_;
1930         else
1931         {
1932                 // check if ssid match
1933                 if (!_rtw_memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
1934                         status = _STATS_FAILURE_;
1935
1936                 if (ie_len != cur->Ssid.SsidLength)
1937                         status = _STATS_FAILURE_;
1938         }
1939
1940         if(_STATS_SUCCESSFUL_ != status)
1941                 goto OnAssocReqFail;
1942
1943         // check if the supported rate is ok
1944         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1945         if (p == NULL) {
1946                 DBG_871X("Rx a sta assoc-req which supported rate is empty!\n");
1947                 // use our own rate set as statoin used
1948                 //_rtw_memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN);
1949                 //supportRateNum = AP_BSSRATE_LEN;
1950
1951                 status = _STATS_FAILURE_;
1952                 goto OnAssocReqFail;
1953         }
1954         else {
1955                 _rtw_memcpy(supportRate, p+2, ie_len);
1956                 supportRateNum = ie_len;
1957
1958                 p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_ , &ie_len,
1959                                 pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1960                 if (p !=  NULL) {
1961
1962                         if(supportRateNum<=sizeof(supportRate))
1963                         {
1964                                 _rtw_memcpy(supportRate+supportRateNum, p+2, ie_len);
1965                                 supportRateNum += ie_len;
1966                         }
1967                 }
1968         }
1969
1970         //todo: mask supportRate between AP & STA -> move to update raid
1971         //get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0);
1972
1973         //update station supportRate
1974         pstat->bssratelen = supportRateNum;
1975         _rtw_memcpy(pstat->bssrateset, supportRate, supportRateNum);
1976         UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
1977
1978         //check RSN/WPA/WPS
1979         pstat->dot8021xalg = 0;
1980         pstat->wpa_psk = 0;
1981         pstat->wpa_group_cipher = 0;
1982         pstat->wpa2_group_cipher = 0;
1983         pstat->wpa_pairwise_cipher = 0;
1984         pstat->wpa2_pairwise_cipher = 0;
1985         _rtw_memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
1986         if((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
1987
1988                 int group_cipher=0, pairwise_cipher=0;
1989
1990                 wpa_ie = elems.rsn_ie;
1991                 wpa_ie_len = elems.rsn_ie_len;
1992
1993                 if(rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS)
1994                 {
1995                         pstat->dot8021xalg = 1;//psk,  todo:802.1x
1996                         pstat->wpa_psk |= BIT(1);
1997
1998                         pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
1999                         pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
2000
2001                         if(!pstat->wpa2_group_cipher)
2002                                 status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
2003
2004                         if(!pstat->wpa2_pairwise_cipher)
2005                                 status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
2006                 }
2007                 else
2008                 {
2009                         status = WLAN_STATUS_INVALID_IE;
2010                 }
2011
2012         } else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
2013
2014                 int group_cipher=0, pairwise_cipher=0;
2015
2016                 wpa_ie = elems.wpa_ie;
2017                 wpa_ie_len = elems.wpa_ie_len;
2018
2019                 if(rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS)
2020                 {
2021                         pstat->dot8021xalg = 1;//psk,  todo:802.1x
2022                         pstat->wpa_psk |= BIT(0);
2023
2024                         pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
2025                         pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
2026
2027                         if(!pstat->wpa_group_cipher)
2028                                 status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
2029
2030                         if(!pstat->wpa_pairwise_cipher)
2031                                 status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
2032
2033                 }
2034                 else
2035                 {
2036                         status = WLAN_STATUS_INVALID_IE;
2037                 }
2038
2039         } else {
2040                 wpa_ie = NULL;
2041                 wpa_ie_len = 0;
2042         }
2043
2044         if(_STATS_SUCCESSFUL_ != status)
2045                 goto OnAssocReqFail;
2046
2047         pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
2048         //if (hapd->conf->wps_state && wpa_ie == NULL) { //todo: to check ap if supporting WPS
2049         if(wpa_ie == NULL) {
2050                 if (elems.wps_ie) {
2051                         DBG_871X("STA included WPS IE in "
2052                                    "(Re)Association Request - assume WPS is "
2053                                    "used\n");
2054                         pstat->flags |= WLAN_STA_WPS;
2055                         //wpabuf_free(sta->wps_ie);
2056                         //sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4,
2057                         //                              elems.wps_ie_len - 4);
2058                 } else {
2059                         DBG_871X("STA did not include WPA/RSN IE "
2060                                    "in (Re)Association Request - possible WPS "
2061                                    "use\n");
2062                         pstat->flags |= WLAN_STA_MAYBE_WPS;
2063                 }
2064
2065
2066                 // AP support WPA/RSN, and sta is going to do WPS, but AP is not ready
2067                 // that the selected registrar of AP is _FLASE
2068                 if((psecuritypriv->wpa_psk >0)
2069                         && (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS)))
2070                 {
2071                         if(pmlmepriv->wps_beacon_ie)
2072                         {
2073                                 u8 selected_registrar = 0;
2074
2075                                 rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR , &selected_registrar, NULL);
2076
2077                                 if(!selected_registrar)
2078                                 {
2079                                         DBG_871X("selected_registrar is _FALSE , or AP is not ready to do WPS\n");
2080
2081                                         status = _STATS_UNABLE_HANDLE_STA_;
2082
2083                                         goto OnAssocReqFail;
2084                                 }
2085                         }
2086                 }
2087
2088         }
2089         else
2090         {
2091                 int copy_len;
2092
2093                 if(psecuritypriv->wpa_psk == 0)
2094                 {
2095                         DBG_871X("STA " MAC_FMT ": WPA/RSN IE in association "
2096                         "request, but AP don't support WPA/RSN\n", MAC_ARG(pstat->hwaddr));
2097
2098                         status = WLAN_STATUS_INVALID_IE;
2099
2100                         goto OnAssocReqFail;
2101
2102                 }
2103
2104                 if (elems.wps_ie) {
2105                         DBG_871X("STA included WPS IE in "
2106                                    "(Re)Association Request - WPS is "
2107                                    "used\n");
2108                         pstat->flags |= WLAN_STA_WPS;
2109                         copy_len=0;
2110                 }
2111                 else
2112                 {
2113                         copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)):(wpa_ie_len+2);
2114                 }
2115
2116
2117                 if(copy_len>0)
2118                         _rtw_memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
2119
2120         }
2121
2122
2123         // check if there is WMM IE & support WWM-PS
2124         pstat->flags &= ~WLAN_STA_WME;
2125         pstat->qos_option = 0;
2126         pstat->qos_info = 0;
2127         pstat->has_legacy_ac = _TRUE;
2128         pstat->uapsd_vo = 0;
2129         pstat->uapsd_vi = 0;
2130         pstat->uapsd_be = 0;
2131         pstat->uapsd_bk = 0;
2132         if (pmlmepriv->qospriv.qos_option)
2133         {
2134                 p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
2135                 for (;;)
2136                 {
2137                         p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
2138                         if (p != NULL) {
2139                                 if (_rtw_memcmp(p+2, WMM_IE, 6)) {
2140
2141                                         pstat->flags |= WLAN_STA_WME;
2142
2143                                         pstat->qos_option = 1;
2144                                         pstat->qos_info = *(p+8);
2145
2146                                         pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
2147
2148                                         if((pstat->qos_info&0xf) !=0xf)
2149                                                 pstat->has_legacy_ac = _TRUE;
2150                                         else
2151                                                 pstat->has_legacy_ac = _FALSE;
2152
2153                                         if(pstat->qos_info&0xf)
2154                                         {
2155                                                 if(pstat->qos_info&BIT(0))
2156                                                         pstat->uapsd_vo = BIT(0)|BIT(1);
2157                                                 else
2158                                                         pstat->uapsd_vo = 0;
2159
2160                                                 if(pstat->qos_info&BIT(1))
2161                                                         pstat->uapsd_vi = BIT(0)|BIT(1);
2162                                                 else
2163                                                         pstat->uapsd_vi = 0;
2164
2165                                                 if(pstat->qos_info&BIT(2))
2166                                                         pstat->uapsd_bk = BIT(0)|BIT(1);
2167                                                 else
2168                                                         pstat->uapsd_bk = 0;
2169
2170                                                 if(pstat->qos_info&BIT(3))
2171                                                         pstat->uapsd_be = BIT(0)|BIT(1);
2172                                                 else
2173                                                         pstat->uapsd_be = 0;
2174
2175                                         }
2176
2177                                         break;
2178                                 }
2179                         }
2180                         else {
2181                                 break;
2182                         }
2183                         p = p + ie_len + 2;
2184                 }
2185         }
2186
2187
2188 #ifdef CONFIG_80211N_HT
2189         /* save HT capabilities in the sta object */
2190         _rtw_memset(&pstat->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));
2191         if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap))
2192         {
2193                 pstat->flags |= WLAN_STA_HT;
2194
2195                 pstat->flags |= WLAN_STA_WME;
2196
2197                 _rtw_memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap));
2198
2199         } else
2200                 pstat->flags &= ~WLAN_STA_HT;
2201
2202
2203         if((pmlmepriv->htpriv.ht_option == _FALSE) && (pstat->flags&WLAN_STA_HT))
2204         {
2205                 status = _STATS_FAILURE_;
2206                 goto OnAssocReqFail;
2207         }
2208
2209
2210         if ((pstat->flags & WLAN_STA_HT) &&
2211                     ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
2212                       (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP)))
2213         {
2214                 DBG_871X("HT: " MAC_FMT " tried to "
2215                                    "use TKIP with HT association\n", MAC_ARG(pstat->hwaddr));
2216
2217                 //status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY;
2218                 //goto OnAssocReqFail;
2219         }
2220 #endif /* CONFIG_80211N_HT */
2221
2222 #ifdef CONFIG_80211AC_VHT
2223         _rtw_memset(&pstat->vhtpriv, 0, sizeof(struct vht_priv));
2224         if (elems.vht_capabilities && elems.vht_capabilities_len == 12) {
2225                 pstat->flags |= WLAN_STA_VHT;
2226
2227                 _rtw_memcpy(pstat->vhtpriv.vht_cap, elems.vht_capabilities, 12);
2228
2229                 if (elems.vht_op_mode_notify && elems.vht_op_mode_notify_len == 1) {
2230                         _rtw_memcpy(&pstat->vhtpriv.vht_op_mode_notify, elems.vht_op_mode_notify, 1);
2231                 }
2232         }
2233         else {
2234                 pstat->flags &= ~WLAN_STA_VHT;
2235         }
2236
2237         if((pmlmepriv->vhtpriv.vht_option == _FALSE) && (pstat->flags&WLAN_STA_VHT))
2238         {
2239                 status = _STATS_FAILURE_;
2240                 goto OnAssocReqFail;
2241         }
2242 #endif /* CONFIG_80211AC_VHT */
2243
2244        //
2245        //if (hapd->iface->current_mode->mode == HOSTAPD_MODE_IEEE80211G)//?
2246         pstat->flags |= WLAN_STA_NONERP;
2247         for (i = 0; i < pstat->bssratelen; i++) {
2248                 if ((pstat->bssrateset[i] & 0x7f) > 22) {
2249                         pstat->flags &= ~WLAN_STA_NONERP;
2250                         break;
2251                 }
2252         }
2253
2254         if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
2255                 pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
2256         else
2257                 pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
2258
2259
2260
2261         if (status != _STATS_SUCCESSFUL_)
2262                 goto OnAssocReqFail;
2263
2264 #ifdef CONFIG_P2P
2265         pstat->is_p2p_device = _FALSE;
2266         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2267         {
2268                 if( (p2pie=rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , NULL, &p2pielen)))
2269                 {
2270                         pstat->is_p2p_device = _TRUE;
2271                         if((p2p_status_code=(u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat))>0)
2272                         {
2273                                 pstat->p2p_status_code = p2p_status_code;
2274                                 status = _STATS_CAP_FAIL_;
2275                                 goto OnAssocReqFail;
2276                         }
2277                 }
2278 #ifdef CONFIG_WFD
2279                 if(rtw_get_wfd_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , wfd_ie, &wfd_ielen ))
2280                 {
2281                         u8      attr_content[ 10 ] = { 0x00 };
2282                         u32     attr_contentlen = 0;
2283
2284                         DBG_8192C( "[%s] WFD IE Found!!\n", __FUNCTION__ );
2285                         rtw_get_wfd_attr_content( wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
2286                         if ( attr_contentlen )
2287                         {
2288                                 pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16( attr_content + 2 );
2289                                 DBG_8192C( "[%s] Peer PORT NUM = %d\n", __FUNCTION__, pwdinfo->wfd_info->peer_rtsp_ctrlport );
2290                         }
2291                 }
2292 #endif
2293         }
2294         pstat->p2p_status_code = p2p_status_code;
2295 #endif //CONFIG_P2P
2296
2297         //TODO: identify_proprietary_vendor_ie();
2298         // Realtek proprietary IE
2299         // identify if this is Broadcom sta
2300         // identify if this is ralink sta
2301         // Customer proprietary IE
2302
2303
2304
2305         /* get a unique AID */
2306         if (pstat->aid > 0) {
2307                 DBG_871X("  old AID %d\n", pstat->aid);
2308         } else {
2309                 for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
2310                         if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
2311                                 break;
2312
2313                 //if (pstat->aid > NUM_STA) {
2314                 if (pstat->aid > pstapriv->max_num_sta) {
2315
2316                         pstat->aid = 0;
2317
2318                         DBG_871X("  no room for more AIDs\n");
2319
2320                         status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
2321
2322                         goto OnAssocReqFail;
2323
2324
2325                 } else {
2326                         pstapriv->sta_aid[pstat->aid - 1] = pstat;
2327                         DBG_871X("allocate new AID = (%d)\n", pstat->aid);
2328                 }
2329         }
2330
2331
2332         pstat->state &= (~WIFI_FW_ASSOC_STATE);
2333         pstat->state |= WIFI_FW_ASSOC_SUCCESS;
2334
2335         _enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
2336         if (!rtw_is_list_empty(&pstat->auth_list))
2337         {
2338                 rtw_list_delete(&pstat->auth_list);
2339                 pstapriv->auth_list_cnt--;
2340         }
2341         _exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
2342
2343         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2344         if (rtw_is_list_empty(&pstat->asoc_list))
2345         {
2346                 pstat->expire_to = pstapriv->expire_to;
2347                 rtw_list_insert_tail(&pstat->asoc_list, &pstapriv->asoc_list);
2348                 pstapriv->asoc_list_cnt++;
2349         }
2350         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2351
2352         // now the station is qualified to join our BSS...
2353         if(pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_==status))
2354         {
2355 #ifdef CONFIG_NATIVEAP_MLME
2356                 //.1 bss_cap_update & sta_info_update
2357                 bss_cap_update_on_sta_join(padapter, pstat);
2358                 sta_info_update(padapter, pstat);
2359
2360                 //.2 issue assoc rsp before notify station join event.
2361                 if (frame_type == WIFI_ASSOCREQ)
2362                         issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
2363                 else
2364                         issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
2365
2366 #ifdef CONFIG_IOCTL_CFG80211
2367                 _enter_critical_bh(&pstat->lock, &irqL);
2368                 if(pstat->passoc_req)
2369                 {
2370                         rtw_mfree(pstat->passoc_req, pstat->assoc_req_len);
2371                         pstat->passoc_req = NULL;
2372                         pstat->assoc_req_len = 0;
2373                 }
2374
2375                 pstat->passoc_req =  rtw_zmalloc(pkt_len);
2376                 if(pstat->passoc_req)
2377                 {
2378                         _rtw_memcpy(pstat->passoc_req, pframe, pkt_len);
2379                         pstat->assoc_req_len = pkt_len;
2380                 }
2381                 _exit_critical_bh(&pstat->lock, &irqL);
2382 #endif //CONFIG_IOCTL_CFG80211
2383
2384                 //.3-(1) report sta add event
2385                 report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
2386
2387 #endif //CONFIG_NATIVEAP_MLME
2388         }
2389
2390         return _SUCCESS;
2391
2392 asoc_class2_error:
2393
2394 #ifdef CONFIG_NATIVEAP_MLME
2395         issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
2396 #endif
2397
2398         return _FAIL;
2399
2400 OnAssocReqFail:
2401
2402
2403 #ifdef CONFIG_NATIVEAP_MLME
2404         pstat->aid = 0;
2405         if (frame_type == WIFI_ASSOCREQ)
2406                 issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
2407         else
2408                 issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
2409 #endif
2410
2411
2412 #endif /* CONFIG_AP_MODE */
2413
2414         return _FAIL;
2415
2416 }
2417
2418 unsigned int OnAssocRsp(_adapter *padapter, union recv_frame *precv_frame)
2419 {
2420         uint i;
2421         int res;
2422         unsigned short  status;
2423         PNDIS_802_11_VARIABLE_IEs       pIE;
2424         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2425         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2426         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2427         //WLAN_BSSID_EX                 *cur_network = &(pmlmeinfo->network);
2428         u8 *pframe = precv_frame->u.hdr.rx_data;
2429         uint pkt_len = precv_frame->u.hdr.len;
2430         PNDIS_802_11_VARIABLE_IEs       pWapiIE = NULL;
2431
2432         DBG_871X("%s\n", __FUNCTION__);
2433
2434         //check A1 matches or not
2435         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
2436                 return _SUCCESS;
2437
2438         if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
2439                 return _SUCCESS;
2440
2441         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
2442                 return _SUCCESS;
2443
2444         _cancel_timer_ex(&pmlmeext->link_timer);
2445
2446         //status
2447         if ((status = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 2))) > 0)
2448         {
2449                 DBG_871X("assoc reject, status code: %d\n", status);
2450                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
2451                 res = -4;
2452                 goto report_assoc_result;
2453         }
2454
2455         //get capabilities
2456         pmlmeinfo->capability = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2457
2458         //set slot time
2459         pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10))? 9: 20;
2460
2461         //AID
2462         res = pmlmeinfo->aid = (int)(le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
2463
2464         //following are moved to join event callback function
2465         //to handle HT, WMM, rate adaptive, update MAC reg
2466         //for not to handle the synchronous IO in the tasklet
2467         for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;)
2468         {
2469                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + i);
2470
2471                 switch (pIE->ElementID)
2472                 {
2473                         case _VENDOR_SPECIFIC_IE_:
2474                                 if (_rtw_memcmp(pIE->data, WMM_PARA_OUI, 6))    //WMM
2475                                 {
2476                                         WMM_param_handler(padapter, pIE);
2477                                 }
2478 #if defined(CONFIG_P2P) && defined(CONFIG_WFD)
2479                                 else if ( _rtw_memcmp(pIE->data, WFD_OUI, 4))           //WFD
2480                                 {
2481                                         DBG_871X( "[%s] Found WFD IE\n", __FUNCTION__ );
2482                                         WFD_info_handler( padapter, pIE );
2483                                 }
2484 #endif
2485                                 break;
2486
2487 #ifdef CONFIG_WAPI_SUPPORT
2488                         case _WAPI_IE_:
2489                                 pWapiIE = pIE;
2490                                 break;
2491 #endif
2492
2493                         case _HT_CAPABILITY_IE_:        //HT caps
2494                                 HT_caps_handler(padapter, pIE);
2495                                 break;
2496
2497                         case _HT_EXTRA_INFO_IE_:        //HT info
2498                                 HT_info_handler(padapter, pIE);
2499                                 break;
2500
2501 #ifdef CONFIG_80211AC_VHT
2502                         case EID_VHTCapability:
2503                                 VHT_caps_handler(padapter, pIE);
2504                                 break;
2505
2506                         case EID_VHTOperation:
2507                                 VHT_operation_handler(padapter, pIE);
2508                                 break;
2509 #endif
2510
2511                         case _ERPINFO_IE_:
2512                                 ERP_IE_handler(padapter, pIE);
2513
2514                         default:
2515                                 break;
2516                 }
2517
2518                 i += (pIE->Length + 2);
2519         }
2520
2521 #ifdef CONFIG_WAPI_SUPPORT
2522         rtw_wapi_on_assoc_ok(padapter, pIE);
2523 #endif
2524
2525         pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
2526         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
2527
2528         //Update Basic Rate Table for spec, 2010-12-28 , by thomas
2529         UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);
2530
2531 report_assoc_result:
2532         if (res > 0) {
2533                 rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
2534         } else {
2535                 rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
2536         }
2537
2538         report_join_res(padapter, res);
2539
2540         return _SUCCESS;
2541 }
2542
2543 unsigned int OnDeAuth(_adapter *padapter, union recv_frame *precv_frame)
2544 {
2545         unsigned short  reason;
2546         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2547         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2548         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2549         u8 *pframe = precv_frame->u.hdr.rx_data;
2550 #ifdef CONFIG_P2P
2551         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
2552 #endif //CONFIG_P2P
2553
2554         //check A3
2555         if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
2556                 return _SUCCESS;
2557
2558 #ifdef CONFIG_P2P
2559         if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
2560         {
2561                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
2562                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
2563         }
2564 #endif //CONFIG_P2P
2565
2566         reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2567
2568         DBG_871X("%s Reason code(%d)\n", __FUNCTION__,reason);
2569
2570         rtw_lock_rx_suspend_timeout(8000);
2571
2572 #ifdef CONFIG_AP_MODE
2573         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
2574         {
2575                 _irqL irqL;
2576                 struct sta_info *psta;
2577                 struct sta_priv *pstapriv = &padapter->stapriv;
2578
2579                 //_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2580                 //rtw_free_stainfo(padapter, psta);
2581                 //_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2582
2583                 DBG_871X_LEVEL(_drv_always_, "ap recv deauth reason code(%d) sta:%pM\n",
2584                                 reason, GetAddr2Ptr(pframe));
2585
2586                 psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2587                 if(psta)
2588                 {
2589                         u8 updated = _FALSE;
2590
2591                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2592                         if(rtw_is_list_empty(&psta->asoc_list)==_FALSE)
2593                         {
2594                                 rtw_list_delete(&psta->asoc_list);
2595                                 pstapriv->asoc_list_cnt--;
2596                                 updated = ap_free_sta(padapter, psta, _FALSE, reason);
2597
2598                         }
2599                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2600
2601                         associated_clients_update(padapter, updated);
2602                 }
2603
2604
2605                 return _SUCCESS;
2606         }
2607         else
2608 #endif
2609         {
2610                 int     ignore_received_deauth = 0;
2611
2612                 //      Commented by Albert 20130604
2613                 //      Before sending the auth frame to start the STA/GC mode connection with AP/GO,
2614                 //      we will send the deauth first.
2615                 //      However, the Win8.1 with BRCM Wi-Fi will send the deauth with reason code 6 to us after receieving our deauth.
2616                 //      Added the following code to avoid this case.
2617                 if ( ( pmlmeinfo->state & WIFI_FW_AUTH_STATE ) ||
2618                         ( pmlmeinfo->state & WIFI_FW_ASSOC_STATE ) )
2619                 {
2620                         if ( reason == WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA )
2621                         {
2622                                 ignore_received_deauth = 1;
2623                         } else if (WLAN_REASON_PREV_AUTH_NOT_VALID == reason) {
2624                                 // TODO: 802.11r
2625                                 ignore_received_deauth = 1;
2626                         }
2627                 }
2628
2629                 DBG_871X_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM, ignore = %d\n",
2630                                 reason, GetAddr3Ptr(pframe), ignore_received_deauth);
2631
2632                 if ( 0 == ignore_received_deauth )
2633                 {
2634                         receive_disconnect(padapter, GetAddr3Ptr(pframe) ,reason);
2635                 }
2636         }
2637         pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE;
2638         return _SUCCESS;
2639
2640 }
2641
2642 unsigned int OnDisassoc(_adapter *padapter, union recv_frame *precv_frame)
2643 {
2644         unsigned short  reason;
2645         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2646         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2647         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2648         u8 *pframe = precv_frame->u.hdr.rx_data;
2649 #ifdef CONFIG_P2P
2650         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
2651 #endif //CONFIG_P2P
2652
2653         //check A3
2654         if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
2655                 return _SUCCESS;
2656
2657 #ifdef CONFIG_P2P
2658         if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
2659         {
2660                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
2661                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
2662         }
2663 #endif //CONFIG_P2P
2664
2665         reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2666
2667         DBG_871X("%s Reason code(%d)\n", __FUNCTION__,reason);
2668
2669         rtw_lock_rx_suspend_timeout(8000);
2670
2671 #ifdef CONFIG_AP_MODE
2672         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
2673         {
2674                 _irqL irqL;
2675                 struct sta_info *psta;
2676                 struct sta_priv *pstapriv = &padapter->stapriv;
2677
2678                 //_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2679                 //rtw_free_stainfo(padapter, psta);
2680                 //_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2681
2682                 DBG_871X_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
2683                                 reason, GetAddr2Ptr(pframe));
2684
2685                 psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2686                 if(psta)
2687                 {
2688                         u8 updated = _FALSE;
2689
2690                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2691                         if(rtw_is_list_empty(&psta->asoc_list)==_FALSE)
2692                         {
2693                                 rtw_list_delete(&psta->asoc_list);
2694                                 pstapriv->asoc_list_cnt--;
2695                                 updated = ap_free_sta(padapter, psta, _FALSE, reason);
2696
2697                         }
2698                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2699
2700                         associated_clients_update(padapter, updated);
2701                 }
2702
2703                 return _SUCCESS;
2704         }
2705         else
2706 #endif
2707         {
2708                 DBG_871X_LEVEL(_drv_always_, "sta recv disassoc reason code(%d) sta:%pM\n",
2709                                 reason, GetAddr3Ptr(pframe));
2710
2711                 receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
2712         }
2713         pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE;
2714         return _SUCCESS;
2715
2716 }
2717
2718 unsigned int OnAtim(_adapter *padapter, union recv_frame *precv_frame)
2719 {
2720         DBG_871X("%s\n", __FUNCTION__);
2721         return _SUCCESS;
2722 }
2723
2724 unsigned int on_action_spct_ch_switch(_adapter *padapter, struct sta_info *psta, u8 *ies, uint ies_len)
2725 {
2726         unsigned int ret = _FAIL;
2727         struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
2728         struct mlme_ext_info    *pmlmeinfo = &(mlmeext->mlmext_info);
2729
2730         if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
2731                 ret = _SUCCESS;
2732                 goto exit;
2733         }
2734
2735         if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
2736
2737                 int ch_switch_mode = -1, ch = -1, ch_switch_cnt = -1;
2738                 int ch_offset = -1;
2739                 u8 bwmode;
2740                 struct ieee80211_info_element *ie;
2741
2742                 DBG_871X(FUNC_NDEV_FMT" from "MAC_FMT"\n",
2743                         FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(psta->hwaddr));
2744
2745                 for_each_ie(ie, ies, ies_len) {
2746                         if (ie->id == WLAN_EID_CHANNEL_SWITCH) {
2747                                 ch_switch_mode = ie->data[0];
2748                                 ch = ie->data[1];
2749                                 ch_switch_cnt = ie->data[2];
2750                                 DBG_871X("ch_switch_mode:%d, ch:%d, ch_switch_cnt:%d\n",
2751                                         ch_switch_mode, ch, ch_switch_cnt);
2752                         }
2753                         else if (ie->id == WLAN_EID_SECONDARY_CHANNEL_OFFSET) {
2754                                 ch_offset = secondary_ch_offset_to_hal_ch_offset(ie->data[0]);
2755                                 DBG_871X("ch_offset:%d\n", ch_offset);
2756                         }
2757                 }
2758
2759                 if (ch == -1)
2760                         return _SUCCESS;
2761
2762                 if (ch_offset == -1)
2763                         bwmode = mlmeext->cur_bwmode;
2764                 else
2765                         bwmode = (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) ?
2766                                 CHANNEL_WIDTH_20 : CHANNEL_WIDTH_40;
2767
2768                 ch_offset = (ch_offset == -1) ? mlmeext->cur_ch_offset : ch_offset;
2769
2770                 /* todo:
2771                  * 1. the decision of channel switching
2772                  * 2. things after channel switching
2773                  */
2774
2775                 ret = rtw_set_ch_cmd(padapter, ch, bwmode, ch_offset, _TRUE);
2776         }
2777
2778 exit:
2779         return ret;
2780 }
2781
2782 unsigned int on_action_spct(_adapter *padapter, union recv_frame *precv_frame)
2783 {
2784         unsigned int ret = _FAIL;
2785         struct sta_info *psta = NULL;
2786         struct sta_priv *pstapriv = &padapter->stapriv;
2787         u8 *pframe = precv_frame->u.hdr.rx_data;
2788         uint frame_len = precv_frame->u.hdr.len;
2789         u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
2790         u8 category;
2791         u8 action;
2792
2793         DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
2794
2795         psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2796
2797         if (!psta)
2798                 goto exit;
2799
2800         category = frame_body[0];
2801         if(category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
2802                 goto exit;
2803
2804         action = frame_body[1];
2805         switch (action) {
2806         case RTW_WLAN_ACTION_SPCT_MSR_REQ:
2807         case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
2808         case RTW_WLAN_ACTION_SPCT_TPC_REQ:
2809         case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
2810                 break;
2811         case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
2812                 #ifdef CONFIG_SPCT_CH_SWITCH
2813                 ret = on_action_spct_ch_switch(padapter, psta, &frame_body[2],
2814                         frame_len-(frame_body-pframe)-2);
2815                 #endif
2816                 break;
2817         default:
2818                 break;
2819         }
2820
2821 exit:
2822         return ret;
2823 }
2824
2825 unsigned int OnAction_qos(_adapter *padapter, union recv_frame *precv_frame)
2826 {
2827         return _SUCCESS;
2828 }
2829
2830 unsigned int OnAction_dls(_adapter *padapter, union recv_frame *precv_frame)
2831 {
2832         return _SUCCESS;
2833 }
2834
2835 unsigned int OnAction_back(_adapter *padapter, union recv_frame *precv_frame)
2836 {
2837         u8 *addr;
2838         struct sta_info *psta=NULL;
2839         struct recv_reorder_ctrl *preorder_ctrl;
2840         unsigned char           *frame_body;
2841         unsigned char           category, action;
2842         unsigned short  tid, status, reason_code = 0;
2843         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2844         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2845         u8 *pframe = precv_frame->u.hdr.rx_data;
2846         struct sta_priv *pstapriv = &padapter->stapriv;
2847 #ifdef CONFIG_80211N_HT
2848         //check RA matches or not
2849         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))//for if1, sta/ap mode
2850                 return _SUCCESS;
2851
2852 /*
2853         //check A1 matches or not
2854         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
2855                 return _SUCCESS;
2856 */
2857         DBG_871X("%s\n", __FUNCTION__);
2858
2859         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
2860                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
2861                         return _SUCCESS;
2862
2863         addr = GetAddr2Ptr(pframe);
2864         psta = rtw_get_stainfo(pstapriv, addr);
2865
2866         if(psta==NULL)
2867                 return _SUCCESS;
2868
2869         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
2870
2871         category = frame_body[0];
2872         if (category == RTW_WLAN_CATEGORY_BACK)// representing Block Ack
2873         {
2874 #ifdef CONFIG_TDLS
2875                 if((psta->tdls_sta_state & TDLS_LINKED_STATE) &&
2876                         (psta->htpriv.ht_option==_TRUE) &&
2877                         (psta->htpriv.ampdu_enable==_TRUE) )
2878                 {
2879                         //do nothing; just don't want to return _SUCCESS;
2880                 }
2881                 else
2882 #endif //CONFIG_TDLS
2883                 if (!pmlmeinfo->HT_enable)
2884                 {
2885                         return _SUCCESS;
2886                 }
2887
2888                 action = frame_body[1];
2889                 DBG_871X("%s, action=%d\n", __FUNCTION__, action);
2890                 switch (action)
2891                 {
2892                         case RTW_WLAN_ACTION_ADDBA_REQ: //ADDBA request
2893
2894                                 _rtw_memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
2895                                 //process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), GetAddr3Ptr(pframe));
2896                                 process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), addr);
2897
2898                                 if(pmlmeinfo->bAcceptAddbaReq == _TRUE)
2899                                 {
2900                                         issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0);
2901                                 }
2902                                 else
2903                                 {
2904                                         issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);//reject ADDBA Req
2905                                 }
2906
2907                                 break;
2908
2909                         case RTW_WLAN_ACTION_ADDBA_RESP: //ADDBA response
2910
2911                                 //status = frame_body[3] | (frame_body[4] << 8); //endian issue
2912                                 status = RTW_GET_LE16(&frame_body[3]);
2913                                 tid = ((frame_body[5] >> 2) & 0x7);
2914
2915                                 if (status == 0)
2916                                 {       //successful
2917                                         DBG_871X("agg_enable for TID=%d\n", tid);
2918                                         psta->htpriv.agg_enable_bitmap |= 1 << tid;
2919                                         psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
2920                                 }
2921                                 else
2922                                 {
2923                                         psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
2924                                 }
2925
2926                                 if(psta->state & WIFI_STA_ALIVE_CHK_STATE)
2927                                 {
2928                                         DBG_871X("%s alive check - rx ADDBA response\n", __func__);
2929                                         psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
2930                                         psta->expire_to = pstapriv->expire_to;
2931                                         psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
2932                                 }
2933
2934                                 //DBG_871X("marc: ADDBA RSP: %x\n", pmlmeinfo->agg_enable_bitmap);
2935                                 break;
2936
2937                         case RTW_WLAN_ACTION_DELBA: //DELBA
2938                                 if ((frame_body[3] & BIT(3)) == 0)
2939                                 {
2940                                         psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
2941                                         psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
2942
2943                                         //reason_code = frame_body[4] | (frame_body[5] << 8);
2944                                         reason_code = RTW_GET_LE16(&frame_body[4]);
2945                                 }
2946                                 else if((frame_body[3] & BIT(3)) == BIT(3))
2947                                 {
2948                                         tid = (frame_body[3] >> 4) & 0x0F;
2949
2950                                         preorder_ctrl =  &psta->recvreorder_ctrl[tid];
2951                                         preorder_ctrl->enable = _FALSE;
2952                                         preorder_ctrl->indicate_seq = 0xffff;
2953                                         #ifdef DBG_RX_SEQ
2954                                         DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
2955                                                 preorder_ctrl->indicate_seq);
2956                                         #endif
2957                                 }
2958
2959                                 DBG_871X("%s(): DELBA: %x(%x)\n", __FUNCTION__,pmlmeinfo->agg_enable_bitmap, reason_code);
2960                                 //todo: how to notify the host while receiving DELETE BA
2961                                 break;
2962
2963                         default:
2964                                 break;
2965                 }
2966         }
2967 #endif //CONFIG_80211N_HT
2968         return _SUCCESS;
2969 }
2970
2971 #ifdef CONFIG_P2P
2972
2973 static int get_reg_classes_full_count(struct p2p_channels channel_list) {
2974         int cnt = 0;
2975         int i;
2976
2977         for (i = 0; i < channel_list.reg_classes; i++) {
2978                 cnt += channel_list.reg_class[i].channels;
2979         }
2980
2981         return cnt;
2982 }
2983
2984 static void get_channel_cnt_24g_5gl_5gh(  struct mlme_ext_priv *pmlmeext, u8* p24g_cnt, u8* p5gl_cnt, u8* p5gh_cnt )
2985 {
2986         int     i = 0;
2987
2988         *p24g_cnt = 0;
2989         *p5gl_cnt = 0;
2990         *p5gh_cnt = 0;
2991
2992         for( i = 0; i < pmlmeext->max_chan_nums; i++ )
2993         {
2994                 if ( pmlmeext->channel_set[ i ].ChannelNum <= 14 )
2995                 {
2996                         (*p24g_cnt)++;
2997                 }
2998                 else if ( ( pmlmeext->channel_set[ i ].ChannelNum > 14 ) && ( pmlmeext->channel_set[ i ].ChannelNum <= 48 ) )
2999                 {
3000                         //      Just include the channel 36, 40, 44, 48 channels for 5G low
3001                         (*p5gl_cnt)++;
3002                 }
3003                 else if ( ( pmlmeext->channel_set[ i ].ChannelNum >= 149 ) && ( pmlmeext->channel_set[ i ].ChannelNum <= 161 ) )
3004                 {
3005                         //      Just include the channel 149, 153, 157, 161 channels for 5G high
3006                         (*p5gh_cnt)++;
3007                 }
3008         }
3009 }
3010
3011 void issue_p2p_GO_request(_adapter *padapter, u8* raddr)
3012 {
3013
3014         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3015         u8                      action = P2P_PUB_ACTION_ACTION;
3016         u32                     p2poui = cpu_to_be32(P2POUI);
3017         u8                      oui_subtype = P2P_GO_NEGO_REQ;
3018         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
3019         u8                      wpsielen = 0, p2pielen = 0, i;
3020         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
3021         u16                     len_channellist_attr = 0;
3022 #ifdef CONFIG_WFD
3023         u32                                     wfdielen = 0;
3024 #endif //CONFIG_WFD
3025
3026         struct xmit_frame                       *pmgntframe;
3027         struct pkt_attrib                       *pattrib;
3028         unsigned char                                   *pframe;
3029         struct rtw_ieee80211_hdr        *pwlanhdr;
3030         unsigned short                          *fctrl;
3031         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3032         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3033         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3034         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
3035
3036
3037         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
3038         {
3039                 return;
3040         }
3041
3042         DBG_871X( "[%s] In\n", __FUNCTION__ );
3043         //update attribute
3044         pattrib = &pmgntframe->attrib;
3045         update_mgntframe_attrib(padapter, pattrib);
3046
3047         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3048
3049         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3050         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3051
3052         fctrl = &(pwlanhdr->frame_ctl);
3053         *(fctrl) = 0;
3054
3055         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
3056         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
3057         _rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
3058
3059         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3060         pmlmeext->mgnt_seq++;
3061         SetFrameSubType(pframe, WIFI_ACTION);
3062
3063         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
3064         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3065
3066         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
3067         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
3068         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
3069         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3070         pwdinfo->negotiation_dialog_token = 1;  //      Initialize the dialog value
3071         pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &(pattrib->pktlen));
3072
3073
3074
3075         //      WPS Section
3076         wpsielen = 0;
3077         //      WPS OUI
3078         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
3079         wpsielen += 4;
3080
3081         //      WPS version
3082         //      Type:
3083         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
3084         wpsielen += 2;
3085
3086         //      Length:
3087         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
3088         wpsielen += 2;
3089
3090         //      Value:
3091         wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
3092
3093         //      Device Password ID
3094         //      Type:
3095         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
3096         wpsielen += 2;
3097
3098         //      Length:
3099         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
3100         wpsielen += 2;
3101
3102         //      Value:
3103
3104         if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN )
3105         {
3106                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_USER_SPEC );
3107         }
3108         else if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN )
3109         {
3110                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );
3111         }
3112         else if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC )
3113         {
3114                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_PBC );
3115         }
3116
3117         wpsielen += 2;
3118
3119         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
3120
3121
3122         //      P2P IE Section.
3123
3124         //      P2P OUI
3125         p2pielen = 0;
3126         p2pie[ p2pielen++ ] = 0x50;
3127         p2pie[ p2pielen++ ] = 0x6F;
3128         p2pie[ p2pielen++ ] = 0x9A;
3129         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
3130
3131         //      Commented by Albert 20110306
3132         //      According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes
3133         //      1. P2P Capability
3134         //      2. Group Owner Intent
3135         //      3. Configuration Timeout
3136         //      4. Listen Channel
3137         //      5. Extended Listen Timing
3138         //      6. Intended P2P Interface Address
3139         //      7. Channel List
3140         //      8. P2P Device Info
3141         //      9. Operating Channel
3142
3143
3144         //      P2P Capability
3145         //      Type:
3146         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
3147
3148         //      Length:
3149         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3150         p2pielen += 2;
3151
3152         //      Value:
3153         //      Device Capability Bitmap, 1 byte
3154         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
3155
3156         //      Group Capability Bitmap, 1 byte
3157         if ( pwdinfo->persistent_supported )
3158         {
3159                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
3160         }
3161         else
3162         {
3163                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
3164         }
3165
3166
3167         //      Group Owner Intent
3168         //      Type:
3169         p2pie[ p2pielen++ ] = P2P_ATTR_GO_INTENT;
3170
3171         //      Length:
3172         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3173         p2pielen += 2;
3174
3175         //      Value:
3176         //      Todo the tie breaker bit.
3177         p2pie[ p2pielen++ ] = ( ( pwdinfo->intent << 1 ) | BIT(0) );
3178
3179         //      Configuration Timeout
3180         //      Type:
3181         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
3182
3183         //      Length:
3184         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3185         p2pielen += 2;
3186
3187         //      Value:
3188         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
3189         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
3190
3191
3192         //      Listen Channel
3193         //      Type:
3194         p2pie[ p2pielen++ ] = P2P_ATTR_LISTEN_CH;
3195
3196         //      Length:
3197         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
3198         p2pielen += 2;
3199
3200         //      Value:
3201         //      Country String
3202         p2pie[ p2pielen++ ] = 'X';
3203         p2pie[ p2pielen++ ] = 'X';
3204
3205         //      The third byte should be set to 0x04.
3206         //      Described in the "Operating Channel Attribute" section.
3207         p2pie[ p2pielen++ ] = 0x04;
3208
3209         //      Operating Class
3210         p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
3211
3212         //      Channel Number
3213         p2pie[ p2pielen++ ] = pwdinfo->listen_channel;  //      listening channel number
3214
3215
3216         //      Extended Listen Timing ATTR
3217         //      Type:
3218         p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
3219
3220         //      Length:
3221         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
3222         p2pielen += 2;
3223
3224         //      Value:
3225         //      Availability Period
3226         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
3227         p2pielen += 2;
3228
3229         //      Availability Interval
3230         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
3231         p2pielen += 2;
3232
3233
3234         //      Intended P2P Interface Address
3235         //      Type:
3236         p2pie[ p2pielen++ ] = P2P_ATTR_INTENTED_IF_ADDR;
3237
3238         //      Length:
3239         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
3240         p2pielen += 2;
3241
3242         //      Value:
3243         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
3244         p2pielen += ETH_ALEN;
3245
3246
3247         //      Channel List
3248         //      Type:
3249         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
3250
3251         // Length:
3252         // Country String(3)
3253         // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
3254         // + number of channels in all classes
3255         len_channellist_attr = 3
3256            + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
3257            + get_reg_classes_full_count(pmlmeext->channel_list);
3258
3259 #ifdef CONFIG_CONCURRENT_MODE
3260         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3261         {
3262                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
3263         }
3264         else
3265         {
3266                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3267         }
3268 #else
3269
3270         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3271
3272 #endif
3273         p2pielen += 2;
3274
3275         //      Value:
3276         //      Country String
3277         p2pie[ p2pielen++ ] = 'X';
3278         p2pie[ p2pielen++ ] = 'X';
3279
3280         //      The third byte should be set to 0x04.
3281         //      Described in the "Operating Channel Attribute" section.
3282         p2pie[ p2pielen++ ] = 0x04;
3283
3284         //      Channel Entry List
3285
3286 #ifdef CONFIG_CONCURRENT_MODE
3287         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3288         {
3289                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
3290                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3291
3292                 //      Operating Class
3293                 if ( pbuddy_mlmeext->cur_channel > 14 )
3294                 {
3295                         if ( pbuddy_mlmeext->cur_channel >= 149 )
3296                         {
3297                                 p2pie[ p2pielen++ ] = 0x7c;
3298                         }
3299                         else
3300                         {
3301                                 p2pie[ p2pielen++ ] = 0x73;
3302                         }
3303                 }
3304                 else
3305                 {
3306                         p2pie[ p2pielen++ ] = 0x51;
3307                 }
3308
3309                 //      Number of Channels
3310                 //      Just support 1 channel and this channel is AP's channel
3311                 p2pie[ p2pielen++ ] = 1;
3312
3313                 //      Channel List
3314                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
3315         }
3316         else
3317         {
3318                 int i,j;
3319                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3320                         //      Operating Class
3321                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3322
3323                         //      Number of Channels
3324                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3325
3326                         //      Channel List
3327                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3328                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3329                         }
3330                 }
3331         }
3332 #else // CONFIG_CONCURRENT_MODE
3333         {
3334                 int i,j;
3335                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3336                         //      Operating Class
3337                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3338
3339                         //      Number of Channels
3340                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3341
3342                         //      Channel List
3343                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3344                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3345                         }
3346                 }
3347         }
3348 #endif // CONFIG_CONCURRENT_MODE
3349
3350         //      Device Info
3351         //      Type:
3352         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
3353
3354         //      Length:
3355         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
3356         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
3357         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
3358         p2pielen += 2;
3359
3360         //      Value:
3361         //      P2P Device Address
3362         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
3363         p2pielen += ETH_ALEN;
3364
3365         //      Config Method
3366         //      This field should be big endian. Noted by P2P specification.
3367
3368         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
3369
3370         p2pielen += 2;
3371
3372         //      Primary Device Type
3373         //      Category ID
3374         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
3375         p2pielen += 2;
3376
3377         //      OUI
3378         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
3379         p2pielen += 4;
3380
3381         //      Sub Category ID
3382         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
3383         p2pielen += 2;
3384
3385         //      Number of Secondary Device Types
3386         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
3387
3388         //      Device Name
3389         //      Type:
3390         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
3391         p2pielen += 2;
3392
3393         //      Length:
3394         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
3395         p2pielen += 2;
3396
3397         //      Value:
3398         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len );
3399         p2pielen += pwdinfo->device_name_len;
3400
3401
3402         //      Operating Channel
3403         //      Type:
3404         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
3405
3406         //      Length:
3407         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
3408         p2pielen += 2;
3409
3410         //      Value:
3411         //      Country String
3412         p2pie[ p2pielen++ ] = 'X';
3413         p2pie[ p2pielen++ ] = 'X';
3414
3415         //      The third byte should be set to 0x04.
3416         //      Described in the "Operating Channel Attribute" section.
3417         p2pie[ p2pielen++ ] = 0x04;
3418
3419         //      Operating Class
3420         if ( pwdinfo->operating_channel <= 14 )
3421         {
3422                 //      Operating Class
3423                 p2pie[ p2pielen++ ] = 0x51;
3424         }
3425         else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
3426         {
3427                 //      Operating Class
3428                 p2pie[ p2pielen++ ] = 0x73;
3429         }
3430         else
3431         {
3432                 //      Operating Class
3433                 p2pie[ p2pielen++ ] = 0x7c;
3434         }
3435
3436         //      Channel Number
3437         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
3438
3439         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
3440
3441 #ifdef CONFIG_WFD
3442         wfdielen = build_nego_req_wfd_ie(pwdinfo, pframe);
3443         pframe += wfdielen;
3444         pattrib->pktlen += wfdielen;
3445 #endif //CONFIG_WFD
3446
3447         pattrib->last_txcmdsz = pattrib->pktlen;
3448
3449         dump_mgntframe(padapter, pmgntframe);
3450
3451         return;
3452
3453 }
3454
3455
3456 void issue_p2p_GO_response(_adapter *padapter, u8* raddr, u8* frame_body,uint len, u8 result)
3457 {
3458
3459         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3460         u8                      action = P2P_PUB_ACTION_ACTION;
3461         u32                     p2poui = cpu_to_be32(P2POUI);
3462         u8                      oui_subtype = P2P_GO_NEGO_RESP;
3463         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
3464         u8                      p2pielen = 0, i;
3465         uint                    wpsielen = 0;
3466         u16                     wps_devicepassword_id = 0x0000;
3467         uint                    wps_devicepassword_id_len = 0;
3468         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh;
3469         u16                     len_channellist_attr = 0;
3470
3471         struct xmit_frame                       *pmgntframe;
3472         struct pkt_attrib                       *pattrib;
3473         unsigned char                                   *pframe;
3474         struct rtw_ieee80211_hdr        *pwlanhdr;
3475         unsigned short                          *fctrl;
3476         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3477         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3478         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3479         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
3480
3481 #ifdef CONFIG_WFD
3482         u32                                     wfdielen = 0;
3483 #endif //CONFIG_WFD
3484
3485         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
3486         {
3487                 return;
3488         }
3489
3490         DBG_871X( "[%s] In, result = %d\n", __FUNCTION__,  result );
3491         //update attribute
3492         pattrib = &pmgntframe->attrib;
3493         update_mgntframe_attrib(padapter, pattrib);
3494
3495         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3496
3497         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3498         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3499
3500         fctrl = &(pwlanhdr->frame_ctl);
3501         *(fctrl) = 0;
3502
3503         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
3504         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
3505         _rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
3506
3507         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3508         pmlmeext->mgnt_seq++;
3509         SetFrameSubType(pframe, WIFI_ACTION);
3510
3511         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
3512         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3513
3514         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
3515         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
3516         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
3517         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3518         pwdinfo->negotiation_dialog_token = frame_body[7];      //      The Dialog Token of provisioning discovery request frame.
3519         pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
3520
3521         //      Commented by Albert 20110328
3522         //      Try to get the device password ID from the WPS IE of group negotiation request frame
3523         //      WiFi Direct test plan 5.1.15
3524         rtw_get_wps_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
3525         rtw_get_wps_attr_content( wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len);
3526         wps_devicepassword_id = be16_to_cpu( wps_devicepassword_id );
3527
3528         _rtw_memset( wpsie, 0x00, 255 );
3529         wpsielen = 0;
3530
3531         //      WPS Section
3532         wpsielen = 0;
3533         //      WPS OUI
3534         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
3535         wpsielen += 4;
3536
3537         //      WPS version
3538         //      Type:
3539         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
3540         wpsielen += 2;
3541
3542         //      Length:
3543         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
3544         wpsielen += 2;
3545
3546         //      Value:
3547         wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
3548
3549         //      Device Password ID
3550         //      Type:
3551         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
3552         wpsielen += 2;
3553
3554         //      Length:
3555         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
3556         wpsielen += 2;
3557
3558         //      Value:
3559         if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
3560         {
3561                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );
3562         }
3563         else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
3564         {
3565                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_USER_SPEC );
3566         }
3567         else
3568         {
3569                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_PBC );
3570         }
3571         wpsielen += 2;
3572
3573         //      Commented by Kurt 20120113
3574         //      If some device wants to do p2p handshake without sending prov_disc_req
3575         //      We have to get peer_req_cm from here.
3576         if(_rtw_memcmp( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3) )
3577         {
3578                 if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
3579                 {
3580                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3 );
3581                 }
3582                 else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
3583                 {
3584                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3 );
3585                 }
3586                 else
3587                 {
3588                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3 );
3589                 }
3590         }
3591
3592         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
3593
3594
3595         //      P2P IE Section.
3596
3597         //      P2P OUI
3598         p2pielen = 0;
3599         p2pie[ p2pielen++ ] = 0x50;
3600         p2pie[ p2pielen++ ] = 0x6F;
3601         p2pie[ p2pielen++ ] = 0x9A;
3602         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
3603
3604         //      Commented by Albert 20100908
3605         //      According to the P2P Specification, the group negoitation response frame should contain 9 P2P attributes
3606         //      1. Status
3607         //      2. P2P Capability
3608         //      3. Group Owner Intent
3609         //      4. Configuration Timeout
3610         //      5. Operating Channel
3611         //      6. Intended P2P Interface Address
3612         //      7. Channel List
3613         //      8. Device Info
3614         //      9. Group ID     ( Only GO )
3615
3616
3617         //      ToDo:
3618
3619         //      P2P Status
3620         //      Type:
3621         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
3622
3623         //      Length:
3624         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3625         p2pielen += 2;
3626
3627         //      Value:
3628         p2pie[ p2pielen++ ] = result;
3629
3630         //      P2P Capability
3631         //      Type:
3632         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
3633
3634         //      Length:
3635         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3636         p2pielen += 2;
3637
3638         //      Value:
3639         //      Device Capability Bitmap, 1 byte
3640
3641         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
3642         {
3643                 //      Commented by Albert 2011/03/08
3644                 //      According to the P2P specification
3645                 //      if the sending device will be client, the P2P Capability should be reserved of group negotation response frame
3646                 p2pie[ p2pielen++ ] = 0;
3647         }
3648         else
3649         {
3650                 //      Be group owner or meet the error case
3651                 p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
3652         }
3653
3654         //      Group Capability Bitmap, 1 byte
3655         if ( pwdinfo->persistent_supported )
3656         {
3657                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
3658         }
3659         else
3660         {
3661                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
3662         }
3663
3664         //      Group Owner Intent
3665         //      Type:
3666         p2pie[ p2pielen++ ] = P2P_ATTR_GO_INTENT;
3667
3668         //      Length:
3669         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3670         p2pielen += 2;
3671
3672         //      Value:
3673         if ( pwdinfo->peer_intent & 0x01 )
3674         {
3675                 //      Peer's tie breaker bit is 1, our tie breaker bit should be 0
3676                 p2pie[ p2pielen++ ] = ( pwdinfo->intent << 1 );
3677         }
3678         else
3679         {
3680                 //      Peer's tie breaker bit is 0, our tie breaker bit should be 1
3681                 p2pie[ p2pielen++ ] = ( ( pwdinfo->intent << 1 ) | BIT(0) );
3682         }
3683
3684
3685         //      Configuration Timeout
3686         //      Type:
3687         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
3688
3689         //      Length:
3690         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3691         p2pielen += 2;
3692
3693         //      Value:
3694         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
3695         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
3696
3697         //      Operating Channel
3698         //      Type:
3699         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
3700
3701         //      Length:
3702         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
3703         p2pielen += 2;
3704
3705         //      Value:
3706         //      Country String
3707         p2pie[ p2pielen++ ] = 'X';
3708         p2pie[ p2pielen++ ] = 'X';
3709
3710         //      The third byte should be set to 0x04.
3711         //      Described in the "Operating Channel Attribute" section.
3712         p2pie[ p2pielen++ ] = 0x04;
3713
3714         //      Operating Class
3715         if ( pwdinfo->operating_channel <= 14 )
3716         {
3717                 //      Operating Class
3718                 p2pie[ p2pielen++ ] = 0x51;
3719         }
3720         else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
3721         {
3722                 //      Operating Class
3723                 p2pie[ p2pielen++ ] = 0x73;
3724         }
3725         else
3726         {
3727                 //      Operating Class
3728                 p2pie[ p2pielen++ ] = 0x7c;
3729         }
3730
3731         //      Channel Number
3732         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
3733
3734         //      Intended P2P Interface Address
3735         //      Type:
3736         p2pie[ p2pielen++ ] = P2P_ATTR_INTENTED_IF_ADDR;
3737
3738         //      Length:
3739         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
3740         p2pielen += 2;
3741
3742         //      Value:
3743         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
3744         p2pielen += ETH_ALEN;
3745
3746         //      Channel List
3747         //      Type:
3748         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
3749
3750         // Country String(3)
3751         // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
3752         // + number of channels in all classes
3753         len_channellist_attr = 3
3754            + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
3755            + get_reg_classes_full_count(pmlmeext->channel_list);
3756
3757 #ifdef CONFIG_CONCURRENT_MODE
3758         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3759         {
3760                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
3761         }
3762         else
3763         {
3764                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3765         }
3766 #else
3767
3768         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3769
3770  #endif
3771         p2pielen += 2;
3772
3773         //      Value:
3774         //      Country String
3775         p2pie[ p2pielen++ ] = 'X';
3776         p2pie[ p2pielen++ ] = 'X';
3777
3778         //      The third byte should be set to 0x04.
3779         //      Described in the "Operating Channel Attribute" section.
3780         p2pie[ p2pielen++ ] = 0x04;
3781
3782         //      Channel Entry List
3783
3784 #ifdef CONFIG_CONCURRENT_MODE
3785         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3786         {
3787                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
3788                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3789
3790                 //      Operating Class
3791                 if ( pbuddy_mlmeext->cur_channel > 14 )
3792                 {
3793                         if ( pbuddy_mlmeext->cur_channel >= 149 )
3794                         {
3795                                 p2pie[ p2pielen++ ] = 0x7c;
3796                         }
3797                         else
3798                         {
3799                                 p2pie[ p2pielen++ ] = 0x73;
3800                         }
3801                 }
3802                 else
3803                 {
3804                         p2pie[ p2pielen++ ] = 0x51;
3805                 }
3806
3807                 //      Number of Channels
3808                 //      Just support 1 channel and this channel is AP's channel
3809                 p2pie[ p2pielen++ ] = 1;
3810
3811                 //      Channel List
3812                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
3813         }
3814         else
3815         {
3816                 int i, j;
3817                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3818                         //      Operating Class
3819                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3820
3821                         //      Number of Channels
3822                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3823
3824                         //      Channel List
3825                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3826                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3827                         }
3828                 }
3829         }
3830 #else // CONFIG_CONCURRENT_MODE
3831         {
3832                 int i, j;
3833                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3834                         //      Operating Class
3835                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3836
3837                         //      Number of Channels
3838                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3839
3840                         //      Channel List
3841                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3842                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3843                         }
3844                 }
3845         }
3846 #endif // CONFIG_CONCURRENT_MODE
3847
3848
3849         //      Device Info
3850         //      Type:
3851         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
3852
3853         //      Length:
3854         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
3855         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
3856         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
3857         p2pielen += 2;
3858
3859         //      Value:
3860         //      P2P Device Address
3861         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
3862         p2pielen += ETH_ALEN;
3863
3864         //      Config Method
3865         //      This field should be big endian. Noted by P2P specification.
3866
3867         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
3868
3869         p2pielen += 2;
3870
3871         //      Primary Device Type
3872         //      Category ID
3873         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
3874         p2pielen += 2;
3875
3876         //      OUI
3877         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
3878         p2pielen += 4;
3879
3880         //      Sub Category ID
3881         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
3882         p2pielen += 2;
3883
3884         //      Number of Secondary Device Types
3885         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
3886
3887         //      Device Name
3888         //      Type:
3889         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
3890         p2pielen += 2;
3891
3892         //      Length:
3893         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
3894         p2pielen += 2;
3895
3896         //      Value:
3897         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len );
3898         p2pielen += pwdinfo->device_name_len;
3899
3900         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
3901         {
3902                 //      Group ID Attribute
3903                 //      Type:
3904                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
3905
3906                 //      Length:
3907                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + pwdinfo->nego_ssidlen );
3908                 p2pielen += 2;
3909
3910                 //      Value:
3911                 //      p2P Device Address
3912                 _rtw_memcpy( p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN );
3913                 p2pielen += ETH_ALEN;
3914
3915                 //      SSID
3916                 _rtw_memcpy( p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
3917                 p2pielen += pwdinfo->nego_ssidlen;
3918
3919         }
3920
3921         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
3922
3923 #ifdef CONFIG_WFD
3924         wfdielen = build_nego_resp_wfd_ie(pwdinfo, pframe);
3925         pframe += wfdielen;
3926         pattrib->pktlen += wfdielen;
3927 #endif //CONFIG_WFD
3928
3929         pattrib->last_txcmdsz = pattrib->pktlen;
3930
3931         dump_mgntframe(padapter, pmgntframe);
3932
3933         return;
3934
3935 }
3936
3937 void issue_p2p_GO_confirm(_adapter *padapter, u8* raddr, u8 result)
3938 {
3939
3940         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3941         u8                      action = P2P_PUB_ACTION_ACTION;
3942         u32                     p2poui = cpu_to_be32(P2POUI);
3943         u8                      oui_subtype = P2P_GO_NEGO_CONF;
3944         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
3945         u8                      wpsielen = 0, p2pielen = 0;
3946
3947         struct xmit_frame                       *pmgntframe;
3948         struct pkt_attrib                       *pattrib;
3949         unsigned char                                   *pframe;
3950         struct rtw_ieee80211_hdr        *pwlanhdr;
3951         unsigned short                          *fctrl;
3952         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3953         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3954         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3955         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
3956 #ifdef CONFIG_WFD
3957         u32                                     wfdielen = 0;
3958 #endif //CONFIG_WFD
3959
3960         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
3961         {
3962                 return;
3963         }
3964
3965         DBG_871X( "[%s] In\n", __FUNCTION__ );
3966         //update attribute
3967         pattrib = &pmgntframe->attrib;
3968         update_mgntframe_attrib(padapter, pattrib);
3969
3970         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3971
3972         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3973         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3974
3975         fctrl = &(pwlanhdr->frame_ctl);
3976         *(fctrl) = 0;
3977
3978         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
3979         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
3980         _rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
3981
3982         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3983         pmlmeext->mgnt_seq++;
3984         SetFrameSubType(pframe, WIFI_ACTION);
3985
3986         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
3987         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3988
3989         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
3990         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
3991         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
3992         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3993         pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
3994
3995
3996
3997         //      P2P IE Section.
3998
3999         //      P2P OUI
4000         p2pielen = 0;
4001         p2pie[ p2pielen++ ] = 0x50;
4002         p2pie[ p2pielen++ ] = 0x6F;
4003         p2pie[ p2pielen++ ] = 0x9A;
4004         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
4005
4006         //      Commented by Albert 20110306
4007         //      According to the P2P Specification, the group negoitation request frame should contain 5 P2P attributes
4008         //      1. Status
4009         //      2. P2P Capability
4010         //      3. Operating Channel
4011         //      4. Channel List
4012         //      5. Group ID     ( if this WiFi is GO )
4013
4014         //      P2P Status
4015         //      Type:
4016         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
4017
4018         //      Length:
4019         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
4020         p2pielen += 2;
4021
4022         //      Value:
4023         p2pie[ p2pielen++ ] = result;
4024
4025         //      P2P Capability
4026         //      Type:
4027         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
4028
4029         //      Length:
4030         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
4031         p2pielen += 2;
4032
4033         //      Value:
4034         //      Device Capability Bitmap, 1 byte
4035         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
4036
4037         //      Group Capability Bitmap, 1 byte
4038         if ( pwdinfo->persistent_supported )
4039         {
4040                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
4041         }
4042         else
4043         {
4044                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
4045         }
4046
4047
4048         //      Operating Channel
4049         //      Type:
4050         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4051
4052         //      Length:
4053         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4054         p2pielen += 2;
4055
4056         //      Value:
4057         //      Country String
4058         p2pie[ p2pielen++ ] = 'X';
4059         p2pie[ p2pielen++ ] = 'X';
4060
4061         //      The third byte should be set to 0x04.
4062         //      Described in the "Operating Channel Attribute" section.
4063         p2pie[ p2pielen++ ] = 0x04;
4064
4065
4066         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
4067         {
4068                 if ( pwdinfo->peer_operating_ch <= 14 )
4069                 {
4070                         //      Operating Class
4071                         p2pie[ p2pielen++ ] = 0x51;
4072                 }
4073                 else if ( ( pwdinfo->peer_operating_ch >= 36 ) && ( pwdinfo->peer_operating_ch <= 48 ) )
4074                 {
4075                         //      Operating Class
4076                         p2pie[ p2pielen++ ] = 0x73;
4077                 }
4078                 else
4079                 {
4080                         //      Operating Class
4081                         p2pie[ p2pielen++ ] = 0x7c;
4082                 }
4083
4084                 p2pie[ p2pielen++ ] = pwdinfo->peer_operating_ch;
4085         }
4086         else
4087         {
4088                 if ( pwdinfo->operating_channel <= 14 )
4089                 {
4090                         //      Operating Class
4091                         p2pie[ p2pielen++ ] = 0x51;
4092                 }
4093                 else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
4094                 {
4095                         //      Operating Class
4096                         p2pie[ p2pielen++ ] = 0x73;
4097                 }
4098                 else
4099                 {
4100                         //      Operating Class
4101                         p2pie[ p2pielen++ ] = 0x7c;
4102                 }
4103
4104                 //      Channel Number
4105                 p2pie[ p2pielen++ ] = pwdinfo->operating_channel;               //      Use the listen channel as the operating channel
4106         }
4107
4108
4109         //      Channel List
4110         //      Type:
4111         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
4112
4113         *(u16*) ( p2pie + p2pielen ) = 6;
4114         p2pielen += 2;
4115
4116         //      Country String
4117         p2pie[ p2pielen++ ] = 'X';
4118         p2pie[ p2pielen++ ] = 'X';
4119
4120         //      The third byte should be set to 0x04.
4121         //      Described in the "Operating Channel Attribute" section.
4122         p2pie[ p2pielen++ ] = 0x04;
4123
4124         //      Value:
4125         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
4126         {
4127                 if ( pwdinfo->peer_operating_ch <= 14 )
4128                 {
4129                         //      Operating Class
4130                         p2pie[ p2pielen++ ] = 0x51;
4131                 }
4132                 else if ( ( pwdinfo->peer_operating_ch >= 36 ) && ( pwdinfo->peer_operating_ch <= 48 ) )
4133                 {
4134                         //      Operating Class
4135                         p2pie[ p2pielen++ ] = 0x73;
4136                 }
4137                 else
4138                 {
4139                         //      Operating Class
4140                         p2pie[ p2pielen++ ] = 0x7c;
4141                 }
4142                 p2pie[ p2pielen++ ] = 1;
4143                 p2pie[ p2pielen++ ] = pwdinfo->peer_operating_ch;
4144         }
4145         else
4146         {
4147                 if ( pwdinfo->operating_channel <= 14 )
4148                 {
4149                         //      Operating Class
4150                         p2pie[ p2pielen++ ] = 0x51;
4151                 }
4152                 else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
4153                 {
4154                         //      Operating Class
4155                         p2pie[ p2pielen++ ] = 0x73;
4156                 }
4157                 else
4158                 {
4159                         //      Operating Class
4160                         p2pie[ p2pielen++ ] = 0x7c;
4161                 }
4162
4163                 //      Channel Number
4164                 p2pie[ p2pielen++ ] = 1;
4165                 p2pie[ p2pielen++ ] = pwdinfo->operating_channel;               //      Use the listen channel as the operating channel
4166         }
4167
4168         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
4169         {
4170                 //      Group ID Attribute
4171                 //      Type:
4172                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
4173
4174                 //      Length:
4175                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + pwdinfo->nego_ssidlen );
4176                 p2pielen += 2;
4177
4178                 //      Value:
4179                 //      p2P Device Address
4180                 _rtw_memcpy( p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN );
4181                 p2pielen += ETH_ALEN;
4182
4183                 //      SSID
4184                 _rtw_memcpy( p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
4185                 p2pielen += pwdinfo->nego_ssidlen;
4186         }
4187
4188         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
4189
4190 #ifdef CONFIG_WFD
4191         wfdielen = build_nego_confirm_wfd_ie(pwdinfo, pframe);
4192         pframe += wfdielen;
4193         pattrib->pktlen += wfdielen;
4194 #endif //CONFIG_WFD
4195
4196         pattrib->last_txcmdsz = pattrib->pktlen;
4197
4198         dump_mgntframe(padapter, pmgntframe);
4199
4200         return;
4201
4202 }
4203
4204 void issue_p2p_invitation_request(_adapter *padapter, u8* raddr )
4205 {
4206
4207         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
4208         u8                      action = P2P_PUB_ACTION_ACTION;
4209         u32                     p2poui = cpu_to_be32(P2POUI);
4210         u8                      oui_subtype = P2P_INVIT_REQ;
4211         u8                      p2pie[ 255 ] = { 0x00 };
4212         u8                      p2pielen = 0, i;
4213         u8                      dialogToken = 3;
4214         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
4215         u16                     len_channellist_attr = 0;
4216 #ifdef CONFIG_WFD
4217         u32                                     wfdielen = 0;
4218 #endif //CONFIG_WFD
4219 #ifdef CONFIG_CONCURRENT_MODE
4220         _adapter                                *pbuddy_adapter = padapter->pbuddy_adapter;
4221         struct wifidirect_info  *pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
4222         struct mlme_priv                *pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
4223         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4224 #endif
4225
4226         struct xmit_frame                       *pmgntframe;
4227         struct pkt_attrib                       *pattrib;
4228         unsigned char                                   *pframe;
4229         struct rtw_ieee80211_hdr        *pwlanhdr;
4230         unsigned short                          *fctrl;
4231         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
4232         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4233         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4234         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
4235
4236
4237         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4238         {
4239                 return;
4240         }
4241
4242         //update attribute
4243         pattrib = &pmgntframe->attrib;
4244         update_mgntframe_attrib(padapter, pattrib);
4245
4246         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4247
4248         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4249         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4250
4251         fctrl = &(pwlanhdr->frame_ctl);
4252         *(fctrl) = 0;
4253
4254         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
4255         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
4256         _rtw_memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
4257
4258         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4259         pmlmeext->mgnt_seq++;
4260         SetFrameSubType(pframe, WIFI_ACTION);
4261
4262         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4263         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4264
4265         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4266         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4267         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4268         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4269         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
4270
4271         //      P2P IE Section.
4272
4273         //      P2P OUI
4274         p2pielen = 0;
4275         p2pie[ p2pielen++ ] = 0x50;
4276         p2pie[ p2pielen++ ] = 0x6F;
4277         p2pie[ p2pielen++ ] = 0x9A;
4278         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
4279
4280         //      Commented by Albert 20101011
4281         //      According to the P2P Specification, the P2P Invitation request frame should contain 7 P2P attributes
4282         //      1. Configuration Timeout
4283         //      2. Invitation Flags
4284         //      3. Operating Channel    ( Only GO )
4285         //      4. P2P Group BSSID      ( Should be included if I am the GO )
4286         //      5. Channel List
4287         //      6. P2P Group ID
4288         //      7. P2P Device Info
4289
4290         //      Configuration Timeout
4291         //      Type:
4292         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
4293
4294         //      Length:
4295         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
4296         p2pielen += 2;
4297
4298         //      Value:
4299         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
4300         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
4301
4302         //      Invitation Flags
4303         //      Type:
4304         p2pie[ p2pielen++ ] = P2P_ATTR_INVITATION_FLAGS;
4305
4306         //      Length:
4307         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
4308         p2pielen += 2;
4309
4310         //      Value:
4311         p2pie[ p2pielen++ ] = P2P_INVITATION_FLAGS_PERSISTENT;
4312
4313
4314         //      Operating Channel
4315         //      Type:
4316         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4317
4318         //      Length:
4319         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4320         p2pielen += 2;
4321
4322         //      Value:
4323         //      Country String
4324         p2pie[ p2pielen++ ] = 'X';
4325         p2pie[ p2pielen++ ] = 'X';
4326
4327         //      The third byte should be set to 0x04.
4328         //      Described in the "Operating Channel Attribute" section.
4329         p2pie[ p2pielen++ ] = 0x04;
4330
4331         //      Operating Class
4332         if ( pwdinfo->invitereq_info.operating_ch <= 14 )
4333                 p2pie[ p2pielen++ ] = 0x51;
4334         else if ( ( pwdinfo->invitereq_info.operating_ch >= 36 ) && ( pwdinfo->invitereq_info.operating_ch <= 48 ) )
4335                 p2pie[ p2pielen++ ] = 0x73;
4336         else
4337                 p2pie[ p2pielen++ ] = 0x7c;
4338
4339         //      Channel Number
4340         p2pie[ p2pielen++ ] = pwdinfo->invitereq_info.operating_ch;     //      operating channel number
4341
4342         if ( _rtw_memcmp( myid( &padapter->eeprompriv ), pwdinfo->invitereq_info.go_bssid, ETH_ALEN ) )
4343         {
4344                 //      P2P Group BSSID
4345                 //      Type:
4346                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_BSSID;
4347
4348                 //      Length:
4349                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
4350                 p2pielen += 2;
4351
4352                 //      Value:
4353                 //      P2P Device Address for GO
4354                 _rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN );
4355                 p2pielen += ETH_ALEN;
4356         }
4357
4358         //      Channel List
4359         //      Type:
4360         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
4361
4362
4363         //      Length:
4364         // Country String(3)
4365         // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
4366         // + number of channels in all classes
4367         len_channellist_attr = 3
4368            + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
4369            + get_reg_classes_full_count(pmlmeext->channel_list);
4370
4371 #ifdef CONFIG_CONCURRENT_MODE
4372         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4373         {
4374                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
4375         }
4376         else
4377         {
4378                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4379         }
4380 #else
4381
4382         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4383
4384  #endif
4385         p2pielen += 2;
4386
4387         //      Value:
4388         //      Country String
4389         p2pie[ p2pielen++ ] = 'X';
4390         p2pie[ p2pielen++ ] = 'X';
4391
4392         //      The third byte should be set to 0x04.
4393         //      Described in the "Operating Channel Attribute" section.
4394         p2pie[ p2pielen++ ] = 0x04;
4395
4396         //      Channel Entry List
4397 #ifdef CONFIG_CONCURRENT_MODE
4398         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4399         {
4400                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
4401                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4402
4403                 //      Operating Class
4404                 if ( pbuddy_mlmeext->cur_channel > 14 )
4405                 {
4406                         if ( pbuddy_mlmeext->cur_channel >= 149 )
4407                         {
4408                                 p2pie[ p2pielen++ ] = 0x7c;
4409                         }
4410                         else
4411                         {
4412                                 p2pie[ p2pielen++ ] = 0x73;
4413                         }
4414                 }
4415                 else
4416                 {
4417                         p2pie[ p2pielen++ ] = 0x51;
4418                 }
4419
4420                 //      Number of Channels
4421                 //      Just support 1 channel and this channel is AP's channel
4422                 p2pie[ p2pielen++ ] = 1;
4423
4424                 //      Channel List
4425                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
4426         }
4427         else
4428         {
4429                 int i, j;
4430                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4431                         //      Operating Class
4432                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4433
4434                         //      Number of Channels
4435                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4436
4437                         //      Channel List
4438                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4439                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4440                         }
4441                 }
4442         }
4443 #else // CONFIG_CONCURRENT_MODE
4444         {
4445                 int i, j;
4446                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4447                         //      Operating Class
4448                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4449
4450                         //      Number of Channels
4451                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4452
4453                         //      Channel List
4454                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4455                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4456                         }
4457                 }
4458         }
4459 #endif // CONFIG_CONCURRENT_MODE
4460
4461
4462         //      P2P Group ID
4463         //      Type:
4464         p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
4465
4466         //      Length:
4467         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 6 + pwdinfo->invitereq_info.ssidlen );
4468         p2pielen += 2;
4469
4470         //      Value:
4471         //      P2P Device Address for GO
4472         _rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN );
4473         p2pielen += ETH_ALEN;
4474
4475         //      SSID
4476         _rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid, pwdinfo->invitereq_info.ssidlen );
4477         p2pielen += pwdinfo->invitereq_info.ssidlen;
4478
4479
4480         //      Device Info
4481         //      Type:
4482         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
4483
4484         //      Length:
4485         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
4486         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
4487         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
4488         p2pielen += 2;
4489
4490         //      Value:
4491         //      P2P Device Address
4492         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
4493         p2pielen += ETH_ALEN;
4494
4495         //      Config Method
4496         //      This field should be big endian. Noted by P2P specification.
4497         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
4498         p2pielen += 2;
4499
4500         //      Primary Device Type
4501         //      Category ID
4502         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
4503         p2pielen += 2;
4504
4505         //      OUI
4506         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
4507         p2pielen += 4;
4508
4509         //      Sub Category ID
4510         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
4511         p2pielen += 2;
4512
4513         //      Number of Secondary Device Types
4514         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
4515
4516         //      Device Name
4517         //      Type:
4518         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
4519         p2pielen += 2;
4520
4521         //      Length:
4522         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
4523         p2pielen += 2;
4524
4525         //      Value:
4526         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
4527         p2pielen += pwdinfo->device_name_len;
4528
4529         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
4530
4531 #ifdef CONFIG_WFD
4532         wfdielen = build_invitation_req_wfd_ie(pwdinfo, pframe);
4533         pframe += wfdielen;
4534         pattrib->pktlen += wfdielen;
4535 #endif //CONFIG_WFD
4536
4537         pattrib->last_txcmdsz = pattrib->pktlen;
4538
4539         dump_mgntframe(padapter, pmgntframe);
4540
4541         return;
4542
4543 }
4544
4545 void issue_p2p_invitation_response(_adapter *padapter, u8* raddr, u8 dialogToken, u8 status_code)
4546 {
4547
4548         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
4549         u8                      action = P2P_PUB_ACTION_ACTION;
4550         u32                     p2poui = cpu_to_be32(P2POUI);
4551         u8                      oui_subtype = P2P_INVIT_RESP;
4552         u8                      p2pie[ 255 ] = { 0x00 };
4553         u8                      p2pielen = 0, i;
4554         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
4555         u16                     len_channellist_attr = 0;
4556 #ifdef CONFIG_CONCURRENT_MODE
4557         _adapter                                *pbuddy_adapter = padapter->pbuddy_adapter;
4558         struct wifidirect_info  *pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
4559         struct mlme_priv                *pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
4560         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4561 #endif
4562 #ifdef CONFIG_WFD
4563         u32                                     wfdielen = 0;
4564 #endif //CONFIG_WFD
4565
4566         struct xmit_frame                       *pmgntframe;
4567         struct pkt_attrib                       *pattrib;
4568         unsigned char                                   *pframe;
4569         struct rtw_ieee80211_hdr        *pwlanhdr;
4570         unsigned short                          *fctrl;
4571         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
4572         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4573         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4574         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
4575
4576
4577         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4578         {
4579                 return;
4580         }
4581
4582         //update attribute
4583         pattrib = &pmgntframe->attrib;
4584         update_mgntframe_attrib(padapter, pattrib);
4585
4586         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4587
4588         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4589         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4590
4591         fctrl = &(pwlanhdr->frame_ctl);
4592         *(fctrl) = 0;
4593
4594         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
4595         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
4596         _rtw_memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
4597
4598         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4599         pmlmeext->mgnt_seq++;
4600         SetFrameSubType(pframe, WIFI_ACTION);
4601
4602         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4603         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4604
4605         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4606         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4607         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4608         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4609         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
4610
4611         //      P2P IE Section.
4612
4613         //      P2P OUI
4614         p2pielen = 0;
4615         p2pie[ p2pielen++ ] = 0x50;
4616         p2pie[ p2pielen++ ] = 0x6F;
4617         p2pie[ p2pielen++ ] = 0x9A;
4618         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
4619
4620         //      Commented by Albert 20101005
4621         //      According to the P2P Specification, the P2P Invitation response frame should contain 5 P2P attributes
4622         //      1. Status
4623         //      2. Configuration Timeout
4624         //      3. Operating Channel    ( Only GO )
4625         //      4. P2P Group BSSID      ( Only GO )
4626         //      5. Channel List
4627
4628         //      P2P Status
4629         //      Type:
4630         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
4631
4632         //      Length:
4633         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
4634         p2pielen += 2;
4635
4636         //      Value:
4637         //      When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE.
4638         //      Sent the event receiving the P2P Invitation Req frame to DMP UI.
4639         //      DMP had to compare the MAC address to find out the profile.
4640         //      So, the WiFi driver will send the P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB.
4641         //      If the UI found the corresponding profile, the WiFi driver sends the P2P Invitation Req
4642         //      to NB to rebuild the persistent group.
4643         p2pie[ p2pielen++ ] = status_code;
4644
4645         //      Configuration Timeout
4646         //      Type:
4647         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
4648
4649         //      Length:
4650         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
4651         p2pielen += 2;
4652
4653         //      Value:
4654         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
4655         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
4656
4657         if( status_code == P2P_STATUS_SUCCESS )
4658         {
4659                 if( rtw_p2p_chk_role( pwdinfo, P2P_ROLE_GO ) )
4660                 {
4661                         //      The P2P Invitation request frame asks this Wi-Fi device to be the P2P GO
4662                         //      In this case, the P2P Invitation response frame should carry the two more P2P attributes.
4663                         //      First one is operating channel attribute.
4664                         //      Second one is P2P Group BSSID attribute.
4665
4666                         //      Operating Channel
4667                         //      Type:
4668                         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4669
4670                         //      Length:
4671                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4672                         p2pielen += 2;
4673
4674                         //      Value:
4675                         //      Country String
4676                         p2pie[ p2pielen++ ] = 'X';
4677                         p2pie[ p2pielen++ ] = 'X';
4678
4679                         //      The third byte should be set to 0x04.
4680                         //      Described in the "Operating Channel Attribute" section.
4681                         p2pie[ p2pielen++ ] = 0x04;
4682
4683                         //      Operating Class
4684                         p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
4685
4686                         //      Channel Number
4687                         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
4688
4689
4690                         //      P2P Group BSSID
4691                         //      Type:
4692                         p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_BSSID;
4693
4694                         //      Length:
4695                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
4696                         p2pielen += 2;
4697
4698                         //      Value:
4699                         //      P2P Device Address for GO
4700                         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
4701                         p2pielen += ETH_ALEN;
4702
4703                 }
4704
4705                 //      Channel List
4706                 //      Type:
4707                 p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
4708
4709                 //      Length:
4710                 // Country String(3)
4711                 // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
4712                 // + number of channels in all classes
4713                 len_channellist_attr = 3
4714                         + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
4715                         + get_reg_classes_full_count(pmlmeext->channel_list);
4716
4717 #ifdef CONFIG_CONCURRENT_MODE
4718                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4719                 {
4720                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
4721                 }
4722                 else
4723                 {
4724                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4725                 }
4726 #else
4727
4728                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4729
4730 #endif
4731                 p2pielen += 2;
4732
4733                 //      Value:
4734                 //      Country String
4735                 p2pie[ p2pielen++ ] = 'X';
4736                 p2pie[ p2pielen++ ] = 'X';
4737
4738                 //      The third byte should be set to 0x04.
4739                 //      Described in the "Operating Channel Attribute" section.
4740                 p2pie[ p2pielen++ ] = 0x04;
4741
4742                 //      Channel Entry List
4743 #ifdef CONFIG_CONCURRENT_MODE
4744                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4745                 {
4746                         _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
4747                         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4748
4749                         //      Operating Class
4750                         if ( pbuddy_mlmeext->cur_channel > 14 )
4751                         {
4752                                 if ( pbuddy_mlmeext->cur_channel >= 149 )
4753                                 {
4754                                         p2pie[ p2pielen++ ] = 0x7c;
4755                                 }
4756                                 else
4757                                 {
4758                                         p2pie[ p2pielen++ ] = 0x73;
4759                                 }
4760                         }
4761                         else
4762                         {
4763                                 p2pie[ p2pielen++ ] = 0x51;
4764                         }
4765
4766                         //      Number of Channels
4767                         //      Just support 1 channel and this channel is AP's channel
4768                         p2pie[ p2pielen++ ] = 1;
4769
4770                         //      Channel List
4771                         p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
4772                 }
4773                 else
4774                 {
4775                         int i, j;
4776                         for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4777                                 //      Operating Class
4778                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4779
4780                                 //      Number of Channels
4781                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4782
4783                                 //      Channel List
4784                                 for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4785                                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4786                                 }
4787                         }
4788                 }
4789 #else // CONFIG_CONCURRENT_MODE
4790                 {
4791                         int i, j;
4792                         for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4793                                 //      Operating Class
4794                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4795
4796                                 //      Number of Channels
4797                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4798
4799                                 //      Channel List
4800                                 for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4801                                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4802                                 }
4803                         }
4804                 }
4805 #endif // CONFIG_CONCURRENT_MODE
4806         }
4807
4808         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
4809
4810 #ifdef CONFIG_WFD
4811         wfdielen = build_invitation_resp_wfd_ie(pwdinfo, pframe);
4812         pframe += wfdielen;
4813         pattrib->pktlen += wfdielen;
4814 #endif //CONFIG_WFD
4815
4816         pattrib->last_txcmdsz = pattrib->pktlen;
4817
4818         dump_mgntframe(padapter, pmgntframe);
4819
4820         return;
4821
4822 }
4823
4824 void issue_p2p_provision_request(_adapter *padapter, u8* pssid, u8 ussidlen, u8* pdev_raddr )
4825 {
4826         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
4827         u8                      action = P2P_PUB_ACTION_ACTION;
4828         u8                      dialogToken = 1;
4829         u32                     p2poui = cpu_to_be32(P2POUI);
4830         u8                      oui_subtype = P2P_PROVISION_DISC_REQ;
4831         u8                      wpsie[ 100 ] = { 0x00 };
4832         u8                      wpsielen = 0;
4833         u32                     p2pielen = 0;
4834 #ifdef CONFIG_WFD
4835         u32                                     wfdielen = 0;
4836 #endif //CONFIG_WFD
4837
4838         struct xmit_frame                       *pmgntframe;
4839         struct pkt_attrib                       *pattrib;
4840         unsigned char                                   *pframe;
4841         struct rtw_ieee80211_hdr        *pwlanhdr;
4842         unsigned short                          *fctrl;
4843         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
4844         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4845         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4846         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
4847
4848
4849         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4850         {
4851                 return;
4852         }
4853
4854         DBG_871X( "[%s] In\n", __FUNCTION__ );
4855         //update attribute
4856         pattrib = &pmgntframe->attrib;
4857         update_mgntframe_attrib(padapter, pattrib);
4858
4859         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4860
4861         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4862         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4863
4864         fctrl = &(pwlanhdr->frame_ctl);
4865         *(fctrl) = 0;
4866
4867         _rtw_memcpy(pwlanhdr->addr1, pdev_raddr, ETH_ALEN);
4868         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
4869         _rtw_memcpy(pwlanhdr->addr3, pdev_raddr, ETH_ALEN);
4870
4871         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4872         pmlmeext->mgnt_seq++;
4873         SetFrameSubType(pframe, WIFI_ACTION);
4874
4875         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4876         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4877
4878         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4879         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4880         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4881         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4882         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
4883
4884         p2pielen = build_prov_disc_request_p2p_ie( pwdinfo, pframe, pssid, ussidlen, pdev_raddr );
4885
4886         pframe += p2pielen;
4887         pattrib->pktlen += p2pielen;
4888
4889         wpsielen = 0;
4890         //      WPS OUI
4891         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
4892         wpsielen += 4;
4893
4894         //      WPS version
4895         //      Type:
4896         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
4897         wpsielen += 2;
4898
4899         //      Length:
4900         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
4901         wpsielen += 2;
4902
4903         //      Value:
4904         wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
4905
4906         //      Config Method
4907         //      Type:
4908         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
4909         wpsielen += 2;
4910
4911         //      Length:
4912         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
4913         wpsielen += 2;
4914
4915         //      Value:
4916         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->tx_prov_disc_info.wps_config_method_request );
4917         wpsielen += 2;
4918
4919         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
4920
4921
4922 #ifdef CONFIG_WFD
4923         wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe);
4924         pframe += wfdielen;
4925         pattrib->pktlen += wfdielen;
4926 #endif //CONFIG_WFD
4927
4928         pattrib->last_txcmdsz = pattrib->pktlen;
4929
4930         dump_mgntframe(padapter, pmgntframe);
4931
4932         return;
4933
4934 }
4935
4936
4937 u8 is_matched_in_profilelist( u8* peermacaddr, struct profile_info* profileinfo )
4938 {
4939         u8 i, match_result = 0;
4940
4941         DBG_871X( "[%s] peermac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
4942                     peermacaddr[0], peermacaddr[1],peermacaddr[2],peermacaddr[3],peermacaddr[4],peermacaddr[5]);
4943
4944         for( i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++ )
4945         {
4946                DBG_871X( "[%s] profileinfo_mac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
4947                             profileinfo->peermac[0], profileinfo->peermac[1],profileinfo->peermac[2],profileinfo->peermac[3],profileinfo->peermac[4],profileinfo->peermac[5]);
4948                 if ( _rtw_memcmp( peermacaddr, profileinfo->peermac, ETH_ALEN ) )
4949                 {
4950                         match_result = 1;
4951                         DBG_871X( "[%s] Match!\n", __FUNCTION__ );
4952                         break;
4953                 }
4954         }
4955
4956         return (match_result );
4957 }
4958
4959 void issue_probersp_p2p(_adapter *padapter, unsigned char *da)
4960 {
4961         struct xmit_frame                       *pmgntframe;
4962         struct pkt_attrib                       *pattrib;
4963         unsigned char                                   *pframe;
4964         struct rtw_ieee80211_hdr        *pwlanhdr;
4965         unsigned short                          *fctrl;
4966         unsigned char                                   *mac;
4967         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
4968         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4969         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4970         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
4971         //WLAN_BSSID_EX                 *cur_network = &(pmlmeinfo->network);
4972         u16                                     beacon_interval = 100;
4973         u16                                     capInfo = 0;
4974         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
4975         u8                                      wpsie[255] = { 0x00 };
4976         u32                                     wpsielen = 0, p2pielen = 0;
4977 #ifdef CONFIG_WFD
4978         u32                                     wfdielen = 0;
4979 #endif //CONFIG_WFD
4980 #ifdef CONFIG_INTEL_WIDI
4981         u8 zero_array_check[L2SDTA_SERVICE_VE_LEN] = { 0x00 };
4982 #endif //CONFIG_INTEL_WIDI
4983
4984         //DBG_871X("%s\n", __FUNCTION__);
4985
4986         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4987         {
4988                 return;
4989         }
4990
4991         //update attribute
4992         pattrib = &pmgntframe->attrib;
4993         update_mgntframe_attrib(padapter, pattrib);
4994
4995         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4996
4997         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4998         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4999
5000         mac = myid(&(padapter->eeprompriv));
5001
5002         fctrl = &(pwlanhdr->frame_ctl);
5003         *(fctrl) = 0;
5004         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
5005         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
5006
5007         //      Use the device address for BSSID field.
5008         _rtw_memcpy(pwlanhdr->addr3, mac, ETH_ALEN);
5009
5010         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5011         pmlmeext->mgnt_seq++;
5012         SetFrameSubType(fctrl, WIFI_PROBERSP);
5013
5014         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
5015         pattrib->pktlen = pattrib->hdrlen;
5016         pframe += pattrib->hdrlen;
5017
5018         //timestamp will be inserted by hardware
5019         pframe += 8;
5020         pattrib->pktlen += 8;
5021
5022         // beacon interval: 2 bytes
5023         _rtw_memcpy(pframe, (unsigned char *) &beacon_interval, 2);
5024         pframe += 2;
5025         pattrib->pktlen += 2;
5026
5027         //      capability info: 2 bytes
5028         //      ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of WiFi Direct Spec)
5029         capInfo |= cap_ShortPremble;
5030         capInfo |= cap_ShortSlot;
5031
5032         _rtw_memcpy(pframe, (unsigned char *) &capInfo, 2);
5033         pframe += 2;
5034         pattrib->pktlen += 2;
5035
5036
5037         // SSID
5038         pframe = rtw_set_ie(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid, &pattrib->pktlen);
5039
5040         // supported rates...
5041         //      Use the OFDM rate in the P2P probe response frame. ( 6(B), 9(B), 12, 18, 24, 36, 48, 54 )
5042         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
5043
5044         // DS parameter set
5045         pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&pwdinfo->listen_channel, &pattrib->pktlen);
5046
5047 #ifdef CONFIG_IOCTL_CFG80211
5048         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
5049         {
5050                 if( pmlmepriv->wps_probe_resp_ie != NULL && pmlmepriv->p2p_probe_resp_ie != NULL )
5051                 {
5052                         //WPS IE
5053                         _rtw_memcpy(pframe, pmlmepriv->wps_probe_resp_ie, pmlmepriv->wps_probe_resp_ie_len);
5054                         pattrib->pktlen += pmlmepriv->wps_probe_resp_ie_len;
5055                         pframe += pmlmepriv->wps_probe_resp_ie_len;
5056
5057                         //P2P IE
5058                         _rtw_memcpy(pframe, pmlmepriv->p2p_probe_resp_ie, pmlmepriv->p2p_probe_resp_ie_len);
5059                         pattrib->pktlen += pmlmepriv->p2p_probe_resp_ie_len;
5060                         pframe += pmlmepriv->p2p_probe_resp_ie_len;
5061                 }
5062         }
5063         else
5064 #endif //CONFIG_IOCTL_CFG80211
5065         {
5066
5067                 //      Todo: WPS IE
5068                 //      Noted by Albert 20100907
5069                 //      According to the WPS specification, all the WPS attribute is presented by Big Endian.
5070
5071                 wpsielen = 0;
5072                 //      WPS OUI
5073                 *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
5074                 wpsielen += 4;
5075
5076                 //      WPS version
5077                 //      Type:
5078                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
5079                 wpsielen += 2;
5080
5081                 //      Length:
5082                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5083                 wpsielen += 2;
5084
5085                 //      Value:
5086                 wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
5087
5088 #ifdef CONFIG_INTEL_WIDI
5089                 //      Commented by Kurt
5090                 //      Appended WiDi info. only if we did issued_probereq_widi(), and then we saved ven. ext. in pmlmepriv->sa_ext.
5091                 if(  _rtw_memcmp(pmlmepriv->sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN) == _FALSE
5092                         || pmlmepriv->num_p2p_sdt != 0 )
5093                 {
5094                         //Sec dev type
5095                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SEC_DEV_TYPE_LIST );
5096                         wpsielen += 2;
5097
5098                         //      Length:
5099                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
5100                         wpsielen += 2;
5101
5102                         //      Value:
5103                         //      Category ID
5104                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_DISPLAYS );
5105                         wpsielen += 2;
5106
5107                         //      OUI
5108                         *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( INTEL_DEV_TYPE_OUI );
5109                         wpsielen += 4;
5110
5111                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_WIDI_CONSUMER_SINK );
5112                         wpsielen += 2;
5113
5114                         if(  _rtw_memcmp(pmlmepriv->sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN) == _FALSE )
5115                         {
5116                                 //      Vendor Extension
5117                                 _rtw_memcpy( wpsie + wpsielen, pmlmepriv->sa_ext, L2SDTA_SERVICE_VE_LEN );
5118                                 wpsielen += L2SDTA_SERVICE_VE_LEN;
5119                         }
5120                 }
5121 #endif //CONFIG_INTEL_WIDI
5122
5123                 //      WiFi Simple Config State
5124                 //      Type:
5125                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SIMPLE_CONF_STATE );
5126                 wpsielen += 2;
5127
5128                 //      Length:
5129                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5130                 wpsielen += 2;
5131
5132                 //      Value:
5133                 wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG;   //      Not Configured.
5134
5135                 //      Response Type
5136                 //      Type:
5137                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_RESP_TYPE );
5138                 wpsielen += 2;
5139
5140                 //      Length:
5141                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5142                 wpsielen += 2;
5143
5144                 //      Value:
5145                 wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
5146
5147                 //      UUID-E
5148                 //      Type:
5149                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_UUID_E );
5150                 wpsielen += 2;
5151
5152                 //      Length:
5153                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0010 );
5154                 wpsielen += 2;
5155
5156                 //      Value:
5157                 if (pwdinfo->external_uuid == 0) {
5158                         _rtw_memset( wpsie + wpsielen, 0x0, 16 );
5159                         _rtw_memcpy( wpsie + wpsielen, myid( &padapter->eeprompriv ), ETH_ALEN );
5160                 } else {
5161                         _rtw_memcpy( wpsie + wpsielen, pwdinfo->uuid, 0x10 );
5162                 }
5163                 wpsielen += 0x10;
5164
5165                 //      Manufacturer
5166                 //      Type:
5167                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MANUFACTURER );
5168                 wpsielen += 2;
5169
5170                 //      Length:
5171                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0007 );
5172                 wpsielen += 2;
5173
5174                 //      Value:
5175                 _rtw_memcpy( wpsie + wpsielen, "Realtek", 7 );
5176                 wpsielen += 7;
5177
5178                 //      Model Name
5179                 //      Type:
5180                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MODEL_NAME );
5181                 wpsielen += 2;
5182
5183                 //      Length:
5184                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0006 );
5185                 wpsielen += 2;
5186
5187                 //      Value:
5188                 _rtw_memcpy( wpsie + wpsielen, "8192CU", 6 );
5189                 wpsielen += 6;
5190
5191                 //      Model Number
5192                 //      Type:
5193                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MODEL_NUMBER );
5194                 wpsielen += 2;
5195
5196                 //      Length:
5197                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5198                 wpsielen += 2;
5199
5200                 //      Value:
5201                 wpsie[ wpsielen++ ] = 0x31;             //      character 1
5202
5203                 //      Serial Number
5204                 //      Type:
5205                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SERIAL_NUMBER );
5206                 wpsielen += 2;
5207
5208                 //      Length:
5209                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( ETH_ALEN );
5210                 wpsielen += 2;
5211
5212                 //      Value:
5213                 _rtw_memcpy( wpsie + wpsielen, "123456" , ETH_ALEN );
5214                 wpsielen += ETH_ALEN;
5215
5216                 //      Primary Device Type
5217                 //      Type:
5218                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
5219                 wpsielen += 2;
5220
5221                 //      Length:
5222                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
5223                 wpsielen += 2;
5224
5225                 //      Value:
5226                 //      Category ID
5227                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
5228                 wpsielen += 2;
5229
5230                 //      OUI
5231                 *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( WPSOUI );
5232                 wpsielen += 4;
5233
5234                 //      Sub Category ID
5235                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
5236                 wpsielen += 2;
5237
5238                 //      Device Name
5239                 //      Type:
5240                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
5241                 wpsielen += 2;
5242
5243                 //      Length:
5244                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->device_name_len );
5245                 wpsielen += 2;
5246
5247                 //      Value:
5248                 _rtw_memcpy( wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len );
5249                 wpsielen += pwdinfo->device_name_len;
5250
5251                 //      Config Method
5252                 //      Type:
5253                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
5254                 wpsielen += 2;
5255
5256                 //      Length:
5257                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
5258                 wpsielen += 2;
5259
5260                 //      Value:
5261                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
5262                 wpsielen += 2;
5263
5264
5265                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
5266
5267
5268                 p2pielen = build_probe_resp_p2p_ie(pwdinfo, pframe);
5269                 pframe += p2pielen;
5270                 pattrib->pktlen += p2pielen;
5271         }
5272
5273 #ifdef CONFIG_WFD
5274 #ifdef CONFIG_IOCTL_CFG80211
5275         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
5276 #endif //CONFIG_IOCTL_CFG80211
5277         {
5278                 wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
5279                 pframe += wfdielen;
5280                 pattrib->pktlen += wfdielen;
5281         }
5282 #ifdef CONFIG_IOCTL_CFG80211
5283         else if (pmlmepriv->wfd_probe_resp_ie != NULL && pmlmepriv->wfd_probe_resp_ie_len>0)
5284         {
5285                 //WFD IE
5286                 _rtw_memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, pmlmepriv->wfd_probe_resp_ie_len);
5287                 pattrib->pktlen += pmlmepriv->wfd_probe_resp_ie_len;
5288                 pframe += pmlmepriv->wfd_probe_resp_ie_len;
5289         }
5290 #endif //CONFIG_IOCTL_CFG80211
5291 #endif //CONFIG_WFD
5292
5293         pattrib->last_txcmdsz = pattrib->pktlen;
5294
5295
5296         dump_mgntframe(padapter, pmgntframe);
5297
5298         return;
5299
5300 }
5301
5302 int _issue_probereq_p2p(_adapter *padapter, u8 *da, int wait_ack)
5303 {
5304         int ret = _FAIL;
5305         struct xmit_frame               *pmgntframe;
5306         struct pkt_attrib               *pattrib;
5307         unsigned char                   *pframe;
5308         struct rtw_ieee80211_hdr        *pwlanhdr;
5309         unsigned short          *fctrl;
5310         unsigned char                   *mac;
5311         unsigned char                   bssrate[NumRates];
5312         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
5313         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5314         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5315         int     bssrate_len = 0;
5316         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
5317         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
5318         u8                                      wpsie[255] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
5319         u16                                     wpsielen = 0, p2pielen = 0;
5320 #ifdef CONFIG_WFD
5321         u32                                     wfdielen = 0;
5322 #endif //CONFIG_WFD
5323
5324         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
5325
5326
5327         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
5328         {
5329                 goto exit;
5330         }
5331
5332         //update attribute
5333         pattrib = &pmgntframe->attrib;
5334         update_mgntframe_attrib(padapter, pattrib);
5335
5336
5337         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5338
5339         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5340         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5341
5342         mac = myid(&(padapter->eeprompriv));
5343
5344         fctrl = &(pwlanhdr->frame_ctl);
5345         *(fctrl) = 0;
5346
5347         if (da) {
5348                 _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
5349                 _rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN);
5350         } else {
5351                 if ( ( pwdinfo->p2p_info.scan_op_ch_only ) || ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) )
5352                 {
5353                         //      This two flags will be set when this is only the P2P client mode.
5354                         _rtw_memcpy(pwlanhdr->addr1, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
5355                         _rtw_memcpy(pwlanhdr->addr3, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
5356                 }
5357                 else
5358                 {
5359                         //      broadcast probe request frame
5360                         _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
5361                         _rtw_memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
5362                 }
5363         }
5364         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
5365
5366         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5367         pmlmeext->mgnt_seq++;
5368         SetFrameSubType(pframe, WIFI_PROBEREQ);
5369
5370         pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
5371         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
5372
5373         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
5374         {
5375                 pframe = rtw_set_ie(pframe, _SSID_IE_, pwdinfo->tx_prov_disc_info.ssid.SsidLength, pwdinfo->tx_prov_disc_info.ssid.Ssid, &(pattrib->pktlen));
5376         }
5377         else
5378         {
5379                 pframe = rtw_set_ie(pframe, _SSID_IE_, P2P_WILDCARD_SSID_LEN, pwdinfo->p2p_wildcard_ssid, &(pattrib->pktlen));
5380         }
5381         //      Use the OFDM rate in the P2P probe request frame. ( 6(B), 9(B), 12(B), 24(B), 36, 48, 54 )
5382         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
5383
5384 #ifdef CONFIG_IOCTL_CFG80211
5385         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
5386         {
5387                 if( pmlmepriv->wps_probe_req_ie != NULL && pmlmepriv->p2p_probe_req_ie != NULL )
5388                 {
5389                         //WPS IE
5390                         _rtw_memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
5391                         pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
5392                         pframe += pmlmepriv->wps_probe_req_ie_len;
5393
5394                         //P2P IE
5395                         _rtw_memcpy(pframe, pmlmepriv->p2p_probe_req_ie, pmlmepriv->p2p_probe_req_ie_len);
5396                         pattrib->pktlen += pmlmepriv->p2p_probe_req_ie_len;
5397                         pframe += pmlmepriv->p2p_probe_req_ie_len;
5398                 }
5399         }
5400         else
5401 #endif //CONFIG_IOCTL_CFG80211
5402         {
5403
5404                 //      WPS IE
5405                 //      Noted by Albert 20110221
5406                 //      According to the WPS specification, all the WPS attribute is presented by Big Endian.
5407
5408                 wpsielen = 0;
5409                 //      WPS OUI
5410                 *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
5411                 wpsielen += 4;
5412
5413                 //      WPS version
5414                 //      Type:
5415                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
5416                 wpsielen += 2;
5417
5418                 //      Length:
5419                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5420                 wpsielen += 2;
5421
5422                 //      Value:
5423                 wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
5424
5425                 if( pmlmepriv->wps_probe_req_ie == NULL )
5426                 {
5427                         //      UUID-E
5428                         //      Type:
5429                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_UUID_E );
5430                         wpsielen += 2;
5431
5432                         //      Length:
5433                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0010 );
5434                         wpsielen += 2;
5435
5436                         //      Value:
5437                         if (pwdinfo->external_uuid == 0) {
5438                                 _rtw_memset( wpsie + wpsielen, 0x0, 16 );
5439                                 _rtw_memcpy( wpsie + wpsielen, myid( &padapter->eeprompriv ), ETH_ALEN );
5440                         } else {
5441                                 _rtw_memcpy( wpsie + wpsielen, pwdinfo->uuid, 0x10 );
5442                         }
5443                         wpsielen += 0x10;
5444
5445                         //      Config Method
5446                         //      Type:
5447                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
5448                         wpsielen += 2;
5449
5450                         //      Length:
5451                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
5452                         wpsielen += 2;
5453
5454                         //      Value:
5455                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
5456                         wpsielen += 2;
5457                 }
5458
5459                 //      Device Name
5460                 //      Type:
5461                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
5462                 wpsielen += 2;
5463
5464                 //      Length:
5465                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->device_name_len );
5466                 wpsielen += 2;
5467
5468                 //      Value:
5469                 _rtw_memcpy( wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len );
5470                 wpsielen += pwdinfo->device_name_len;
5471
5472                 //      Primary Device Type
5473                 //      Type:
5474                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
5475                 wpsielen += 2;
5476
5477                 //      Length:
5478                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
5479                 wpsielen += 2;
5480
5481                 //      Value:
5482                 //      Category ID
5483                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_RTK_WIDI );
5484                 wpsielen += 2;
5485
5486                 //      OUI
5487                 *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( WPSOUI );
5488                 wpsielen += 4;
5489
5490                 //      Sub Category ID
5491                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_RTK_DMP );
5492                 wpsielen += 2;
5493
5494                 //      Device Password ID
5495                 //      Type:
5496                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
5497                 wpsielen += 2;
5498
5499                 //      Length:
5500                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
5501                 wpsielen += 2;
5502
5503                 //      Value:
5504                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );  //      Registrar-specified
5505                 wpsielen += 2;
5506
5507                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
5508
5509                 //      P2P OUI
5510                 p2pielen = 0;
5511                 p2pie[ p2pielen++ ] = 0x50;
5512                 p2pie[ p2pielen++ ] = 0x6F;
5513                 p2pie[ p2pielen++ ] = 0x9A;
5514                 p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
5515
5516                 //      Commented by Albert 20110221
5517                 //      According to the P2P Specification, the probe request frame should contain 5 P2P attributes
5518                 //      1. P2P Capability
5519                 //      2. P2P Device ID if this probe request wants to find the specific P2P device
5520                 //      3. Listen Channel
5521                 //      4. Extended Listen Timing
5522                 //      5. Operating Channel if this WiFi is working as the group owner now
5523
5524                 //      P2P Capability
5525                 //      Type:
5526                 p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
5527
5528                 //      Length:
5529                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
5530                 p2pielen += 2;
5531
5532                 //      Value:
5533                 //      Device Capability Bitmap, 1 byte
5534                 p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
5535
5536                 //      Group Capability Bitmap, 1 byte
5537                 if ( pwdinfo->persistent_supported )
5538                         p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
5539                 else
5540                         p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
5541
5542                 //      Listen Channel
5543                 //      Type:
5544                 p2pie[ p2pielen++ ] = P2P_ATTR_LISTEN_CH;
5545
5546                 //      Length:
5547                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
5548                 p2pielen += 2;
5549
5550                 //      Value:
5551                 //      Country String
5552                 p2pie[ p2pielen++ ] = 'X';
5553                 p2pie[ p2pielen++ ] = 'X';
5554
5555                 //      The third byte should be set to 0x04.
5556                 //      Described in the "Operating Channel Attribute" section.
5557                 p2pie[ p2pielen++ ] = 0x04;
5558
5559                 //      Operating Class
5560                 p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
5561
5562                 //      Channel Number
5563                 p2pie[ p2pielen++ ] = pwdinfo->listen_channel;  //      listen channel
5564
5565
5566                 //      Extended Listen Timing
5567                 //      Type:
5568                 p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
5569
5570                 //      Length:
5571                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
5572                 p2pielen += 2;
5573
5574                 //      Value:
5575                 //      Availability Period
5576                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
5577                 p2pielen += 2;
5578
5579                 //      Availability Interval
5580                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
5581                 p2pielen += 2;
5582
5583                 if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
5584                 {
5585                         //      Operating Channel (if this WiFi is working as the group owner now)
5586                         //      Type:
5587                         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
5588
5589                         //      Length:
5590                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
5591                         p2pielen += 2;
5592
5593                         //      Value:
5594                         //      Country String
5595                         p2pie[ p2pielen++ ] = 'X';
5596                         p2pie[ p2pielen++ ] = 'X';
5597
5598                         //      The third byte should be set to 0x04.
5599                         //      Described in the "Operating Channel Attribute" section.
5600                         p2pie[ p2pielen++ ] = 0x04;
5601
5602                         //      Operating Class
5603                         p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
5604
5605                         //      Channel Number
5606                         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
5607
5608                 }
5609
5610                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
5611
5612         }
5613
5614 #ifdef CONFIG_WFD
5615 #ifdef CONFIG_IOCTL_CFG80211
5616         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
5617 #endif
5618         {
5619                 wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe);
5620                 pframe += wfdielen;
5621                 pattrib->pktlen += wfdielen;
5622         }
5623 #ifdef CONFIG_IOCTL_CFG80211
5624         else if (pmlmepriv->wfd_probe_req_ie != NULL && pmlmepriv->wfd_probe_req_ie_len>0)
5625         {
5626                 //WFD IE
5627                 _rtw_memcpy(pframe, pmlmepriv->wfd_probe_req_ie, pmlmepriv->wfd_probe_req_ie_len);
5628                 pattrib->pktlen += pmlmepriv->wfd_probe_req_ie_len;
5629                 pframe += pmlmepriv->wfd_probe_req_ie_len;
5630         }
5631 #endif //CONFIG_IOCTL_CFG80211
5632 #endif //CONFIG_WFD
5633
5634         pattrib->last_txcmdsz = pattrib->pktlen;
5635
5636         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
5637
5638         if (wait_ack) {
5639                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
5640         } else {
5641                 dump_mgntframe(padapter, pmgntframe);
5642                 ret = _SUCCESS;
5643         }
5644
5645 exit:
5646         return ret;
5647 }
5648
5649 inline void issue_probereq_p2p(_adapter *adapter, u8 *da)
5650 {
5651         _issue_probereq_p2p(adapter, da, _FALSE);
5652 }
5653
5654 int issue_probereq_p2p_ex(_adapter *adapter, u8 *da, int try_cnt, int wait_ms)
5655 {
5656         int ret;
5657         int i = 0;
5658         u32 start = rtw_get_current_time();
5659
5660         do
5661         {
5662                 ret = _issue_probereq_p2p(adapter, da, wait_ms>0?_TRUE:_FALSE);
5663
5664                 i++;
5665
5666                 if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
5667                         break;
5668
5669                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
5670                         rtw_msleep_os(wait_ms);
5671
5672         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
5673
5674         if (ret != _FAIL) {
5675                 ret = _SUCCESS;
5676                 #ifndef DBG_XMIT_ACK
5677                 goto exit;
5678                 #endif
5679         }
5680
5681         if (try_cnt && wait_ms) {
5682                 if (da)
5683                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
5684                                 FUNC_ADPT_ARG(adapter), MAC_ARG(da), rtw_get_oper_ch(adapter),
5685                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
5686                 else
5687                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
5688                                 FUNC_ADPT_ARG(adapter), rtw_get_oper_ch(adapter),
5689                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
5690         }
5691 exit:
5692         return ret;
5693 }
5694
5695 #endif //CONFIG_P2P
5696
5697 s32 rtw_action_public_decache(union recv_frame *recv_frame, s32 token)
5698 {
5699         _adapter *adapter = recv_frame->u.hdr.adapter;
5700         struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
5701         u8 *frame = recv_frame->u.hdr.rx_data;
5702         u16 seq_ctrl = ( (recv_frame->u.hdr.attrib.seq_num&0xffff) << 4) |
5703                 (recv_frame->u.hdr.attrib.frag_num & 0xf);
5704
5705         if (GetRetry(frame)) {
5706                 if (token >= 0) {
5707                         if ((seq_ctrl == mlmeext->action_public_rxseq)
5708                                 && (token == mlmeext->action_public_dialog_token))
5709                         {
5710                                 DBG_871X(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x, token:%d\n",
5711                                         FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
5712                                 return _FAIL;
5713                         }
5714                 } else {
5715                         if (seq_ctrl == mlmeext->action_public_rxseq) {
5716                                 DBG_871X(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x\n",
5717                                         FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq);
5718                                 return _FAIL;
5719                         }
5720                 }
5721         }
5722
5723         mlmeext->action_public_rxseq = seq_ctrl;
5724
5725         if (token >= 0)
5726                 mlmeext->action_public_dialog_token = token;
5727
5728         return _SUCCESS;
5729 }
5730
5731 unsigned int on_action_public_p2p(union recv_frame *precv_frame)
5732 {
5733         _adapter *padapter = precv_frame->u.hdr.adapter;
5734         u8 *pframe = precv_frame->u.hdr.rx_data;
5735         uint len = precv_frame->u.hdr.len;
5736         u8 *frame_body;
5737         u8 dialogToken=0;
5738 #ifdef CONFIG_P2P
5739         u8 *p2p_ie;
5740         u32     p2p_ielen, wps_ielen;
5741         struct  wifidirect_info *pwdinfo = &( padapter->wdinfo );
5742         u8      result = P2P_STATUS_SUCCESS;
5743         u8      empty_addr[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
5744         u8 *merged_p2pie = NULL;
5745         u32 merged_p2p_ielen= 0;
5746 #endif //CONFIG_P2P
5747
5748         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
5749
5750         dialogToken = frame_body[7];
5751
5752         if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
5753                 return _FAIL;
5754
5755 #ifdef CONFIG_P2P
5756         _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
5757 #ifdef CONFIG_IOCTL_CFG80211
5758         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211)
5759         {
5760                 rtw_cfg80211_rx_p2p_action_public(padapter, pframe, len);
5761         }
5762         else
5763 #endif //CONFIG_IOCTL_CFG80211
5764         {
5765                 //      Do nothing if the driver doesn't enable the P2P function.
5766                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
5767                         return _SUCCESS;
5768
5769                 len -= sizeof(struct rtw_ieee80211_hdr_3addr);
5770
5771                 switch( frame_body[ 6 ] )//OUI Subtype
5772                 {
5773                         case P2P_GO_NEGO_REQ:
5774                         {
5775                                 DBG_871X( "[%s] Got GO Nego Req Frame\n", __FUNCTION__);
5776                                 _rtw_memset( &pwdinfo->groupid_info, 0x00, sizeof( struct group_id_info ) );
5777
5778                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
5779                                 {
5780                                         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
5781                                 }
5782
5783                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
5784                                 {
5785                                         //      Commented by Albert 20110526
5786                                         //      In this case, this means the previous nego fail doesn't be reset yet.
5787                                         _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
5788                                         //      Restore the previous p2p state
5789                                         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
5790                                         DBG_871X( "[%s] Restore the previous p2p state to %d\n", __FUNCTION__, rtw_p2p_state(pwdinfo) );
5791                                 }
5792 #ifdef CONFIG_CONCURRENT_MODE
5793                                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
5794                                 {
5795                                         _cancel_timer_ex( &pwdinfo->ap_p2p_switch_timer );
5796                                 }
5797 #endif // CONFIG_CONCURRENT_MODE
5798
5799                                 //      Commented by Kurt 20110902
5800                                 //Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered.
5801                                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
5802                                         rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
5803
5804                                 //      Commented by Kurt 20120113
5805                                 //      Get peer_dev_addr here if peer doesn't issue prov_disc frame.
5806                                 if( _rtw_memcmp(pwdinfo->rx_prov_disc_info.peerDevAddr, empty_addr, ETH_ALEN) );
5807                                         _rtw_memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
5808
5809                                 result = process_p2p_group_negotation_req( pwdinfo, frame_body, len );
5810                                 issue_p2p_GO_response( padapter, GetAddr2Ptr(pframe), frame_body, len, result );
5811 #ifdef CONFIG_INTEL_WIDI
5812                                 if( (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) && (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_WFD_CONNECTION) )
5813                                 {
5814                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
5815                                         _cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
5816                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
5817                                 }
5818 #endif //CONFIG_INTEL_WIDI
5819
5820                                 //      Commented by Albert 20110718
5821                                 //      No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer.
5822 #ifdef CONFIG_CONCURRENT_MODE
5823                                 //      Commented by Albert 20120107
5824                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
5825 #else // CONFIG_CONCURRENT_MODE
5826                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
5827 #endif // CONFIG_CONCURRENT_MODE
5828                                 break;
5829                         }
5830                         case P2P_GO_NEGO_RESP:
5831                         {
5832                                 DBG_871X( "[%s] Got GO Nego Resp Frame\n", __FUNCTION__);
5833
5834                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
5835                                 {
5836                                         //      Commented by Albert 20110425
5837                                         //      The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function.
5838                                         _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
5839                                         pwdinfo->nego_req_info.benable = _FALSE;
5840                                         result = process_p2p_group_negotation_resp( pwdinfo, frame_body, len);
5841                                         issue_p2p_GO_confirm( pwdinfo->padapter, GetAddr2Ptr(pframe), result);
5842                                         if ( P2P_STATUS_SUCCESS == result )
5843                                         {
5844                                                 if ( rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT )
5845                                                 {
5846                                                         pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
5847                                                         #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
5848                                                         pwdinfo->p2p_info.operation_ch[ 1 ] = 1;        //Check whether GO is operating in channel 1;
5849                                                         pwdinfo->p2p_info.operation_ch[ 2 ] = 6;        //Check whether GO is operating in channel 6;
5850                                                         pwdinfo->p2p_info.operation_ch[ 3 ] = 11;       //Check whether GO is operating in channel 11;
5851                                                         #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
5852                                                         pwdinfo->p2p_info.scan_op_ch_only = 1;
5853                                                         _set_timer( &pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH );
5854                                                 }
5855                                         }
5856
5857                                         //      Reset the dialog token for group negotiation frames.
5858                                         pwdinfo->negotiation_dialog_token = 1;
5859
5860                                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
5861                                         {
5862                                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
5863                                         }
5864                                 }
5865                                 else
5866                                 {
5867                                         DBG_871X( "[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __FUNCTION__);
5868                                 }
5869
5870                                 break;
5871                         }
5872                         case P2P_GO_NEGO_CONF:
5873                         {
5874                                 DBG_871X( "[%s] Got GO Nego Confirm Frame\n", __FUNCTION__);
5875                                 result = process_p2p_group_negotation_confirm( pwdinfo, frame_body, len);
5876                                 if ( P2P_STATUS_SUCCESS == result )
5877                                 {
5878                                         if ( rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT )
5879                                         {
5880                                                 pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
5881                                                 #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
5882                                                 pwdinfo->p2p_info.operation_ch[ 1 ] = 1;        //Check whether GO is operating in channel 1;
5883                                                 pwdinfo->p2p_info.operation_ch[ 2 ] = 6;        //Check whether GO is operating in channel 6;
5884                                                 pwdinfo->p2p_info.operation_ch[ 3 ] = 11;       //Check whether GO is operating in channel 11;
5885                                                 #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
5886                                                 pwdinfo->p2p_info.scan_op_ch_only = 1;
5887                                                 _set_timer( &pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH );
5888                                         }
5889                                 }
5890                                 break;
5891                         }
5892                         case P2P_INVIT_REQ:
5893                         {
5894                                 //      Added by Albert 2010/10/05
5895                                 //      Received the P2P Invite Request frame.
5896
5897                                 DBG_871X( "[%s] Got invite request frame!\n", __FUNCTION__ );
5898                                 if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
5899                                 {
5900                                         //      Parse the necessary information from the P2P Invitation Request frame.
5901                                         //      For example: The MAC address of sending this P2P Invitation Request frame.
5902                                         u32     attr_contentlen = 0;
5903                                         u8      status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
5904                                         struct group_id_info group_id;
5905                                         u8      invitation_flag = 0;
5906                                         int j=0;
5907
5908                                         merged_p2p_ielen = rtw_get_p2p_merged_ies_len(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_);
5909
5910                                         merged_p2pie = rtw_zmalloc(merged_p2p_ielen + 2);       // 2 is for EID and Length
5911                                         if (merged_p2pie == NULL)
5912                                         {
5913                                                 DBG_871X( "[%s] Malloc p2p ie fail\n", __FUNCTION__);
5914                                                 goto exit;
5915                                         }
5916                                         _rtw_memset(merged_p2pie, 0x00, merged_p2p_ielen);
5917
5918                                         merged_p2p_ielen = rtw_p2p_merge_ies(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, merged_p2pie);
5919
5920                                         rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen);
5921                                         if ( attr_contentlen )
5922                                         {
5923
5924                                                 rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen);
5925                                                 //      Commented by Albert 20120510
5926                                                 //      Copy to the pwdinfo->p2p_peer_interface_addr.
5927                                                 //      So that the WFD UI ( or Sigma ) can get the peer interface address by using the following command.
5928                                                 //      #> iwpriv wlan0 p2p_get peer_ifa
5929                                                 //      After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant.
5930
5931                                                 if ( attr_contentlen )
5932                                                 {
5933                                                         DBG_871X( "[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
5934                                                                         pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1],
5935                                                                         pwdinfo->p2p_peer_interface_addr[2], pwdinfo->p2p_peer_interface_addr[3],
5936                                                                         pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5] );
5937                                                 }
5938
5939                                                 if ( invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT )
5940                                                 {
5941                                                         //      Re-invoke the persistent group.
5942
5943                                                         _rtw_memset( &group_id, 0x00, sizeof( struct group_id_info ) );
5944                                                         rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_ID, ( u8* ) &group_id, &attr_contentlen);
5945                                                         if ( attr_contentlen )
5946                                                         {
5947                                                                 if ( _rtw_memcmp( group_id.go_device_addr, myid( &padapter->eeprompriv ), ETH_ALEN ) )
5948                                                                 {
5949                                                                         //      The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO.
5950                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO );
5951                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_GO );
5952                                                                         status_code = P2P_STATUS_SUCCESS;
5953                                                                 }
5954                                                                 else
5955                                                                 {
5956                                                                         //      The p2p device sending this p2p invitation request wants to be the persistent GO.
5957                                                                         if ( is_matched_in_profilelist( pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[ 0 ] ) )
5958                                                                         {
5959                                                                                 u8 operatingch_info[5] = { 0x00 };
5960                                                                                 if ( rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen) )
5961                                                                                 {
5962                                                                                         if( rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, (u32)operatingch_info[4] ) >= 0 )
5963                                                                                         {
5964                                                                                                 //      The operating channel is acceptable for this device.
5965                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[0]= operatingch_info[4];
5966                                                                                                 #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
5967                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[1]= 1;          //Check whether GO is operating in channel 1;
5968                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[2]= 6;          //Check whether GO is operating in channel 6;
5969                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[3]= 11;         //Check whether GO is operating in channel 11;
5970                                                                                                 #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
5971                                                                                                 pwdinfo->rx_invitereq_info.scan_op_ch_only = 1;
5972                                                                                                 _set_timer( &pwdinfo->reset_ch_sitesurvey, P2P_RESET_SCAN_CH );
5973                                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH );
5974                                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
5975                                                                                                 status_code = P2P_STATUS_SUCCESS;
5976                                                                                         }
5977                                                                                         else
5978                                                                                         {
5979                                                                                                 //      The operating channel isn't supported by this device.
5980                                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
5981                                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_DEVICE );
5982                                                                                                 status_code = P2P_STATUS_FAIL_NO_COMMON_CH;
5983                                                                                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
5984                                                                                         }
5985                                                                                 }
5986                                                                                 else
5987                                                                                 {
5988                                                                                         //      Commented by Albert 20121130
5989                                                                                         //      Intel will use the different P2P IE to store the operating channel information
5990                                                                                         //      Workaround for Intel WiDi 3.5
5991                                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH );
5992                                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
5993                                                                                         status_code = P2P_STATUS_SUCCESS;
5994                                                                                 }
5995                                                                         }
5996                                                                         else
5997                                                                         {
5998                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
5999                                                                                 #ifdef CONFIG_INTEL_WIDI
6000                                                                                 _rtw_memcpy( pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN );
6001                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
6002                                                                                 #endif //CONFIG_INTEL_WIDI
6003
6004                                                                                 status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
6005                                                                         }
6006                                                                 }
6007                                                         }
6008                                                         else
6009                                                         {
6010                                                                 DBG_871X( "[%s] P2P Group ID Attribute NOT FOUND!\n", __FUNCTION__ );
6011                                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6012                                                         }
6013                                                 }
6014                                                 else
6015                                                 {
6016                                                         //      Received the invitation to join a P2P group.
6017
6018                                                         _rtw_memset( &group_id, 0x00, sizeof( struct group_id_info ) );
6019                                                         rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_ID, ( u8* ) &group_id, &attr_contentlen);
6020                                                         if ( attr_contentlen )
6021                                                         {
6022                                                                 if ( _rtw_memcmp( group_id.go_device_addr, myid( &padapter->eeprompriv ), ETH_ALEN ) )
6023                                                                 {
6024                                                                         //      In this case, the GO can't be myself.
6025                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
6026                                                                         status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6027                                                                 }
6028                                                                 else
6029                                                                 {
6030                                                                         //      The p2p device sending this p2p invitation request wants to join an existing P2P group
6031                                                                         //      Commented by Albert 2012/06/28
6032                                                                         //      In this case, this Wi-Fi device should use the iwpriv command to get the peer device address.
6033                                                                         //      The peer device address should be the destination address for the provisioning discovery request.
6034                                                                         //      Then, this Wi-Fi device should use the iwpriv command to get the peer interface address.
6035                                                                         //      The peer interface address should be the address for WPS mac address
6036                                                                         _rtw_memcpy( pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN );
6037                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
6038                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN );
6039                                                                         status_code = P2P_STATUS_SUCCESS;
6040                                                                 }
6041                                                         }
6042                                                         else
6043                                                         {
6044                                                                 DBG_871X( "[%s] P2P Group ID Attribute NOT FOUND!\n", __FUNCTION__ );
6045                                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6046                                                         }
6047                                                 }
6048                                         }
6049                                         else
6050                                         {
6051                                                 DBG_871X( "[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __FUNCTION__ );
6052                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6053                                         }
6054
6055                                         DBG_871X( "[%s] status_code = %d\n", __FUNCTION__, status_code );
6056
6057                                         pwdinfo->inviteresp_info.token = frame_body[ 7 ];
6058                                         issue_p2p_invitation_response( padapter, GetAddr2Ptr(pframe), pwdinfo->inviteresp_info.token, status_code );
6059                                         _set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
6060                                 }
6061 #ifdef CONFIG_INTEL_WIDI
6062                                 if( (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) && (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_WFD_CONNECTION) )
6063                                 {
6064                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
6065                                         _cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
6066                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
6067                                 }
6068 #endif //CONFIG_INTEL_WIDI
6069                                 break;
6070                         }
6071                         case P2P_INVIT_RESP:
6072                         {
6073                                 u8      attr_content = 0x00;
6074                                 u32     attr_contentlen = 0;
6075
6076                                 DBG_871X( "[%s] Got invite response frame!\n", __FUNCTION__ );
6077                                 _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
6078                                 if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
6079                                 {
6080                                         rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
6081
6082                                         if ( attr_contentlen == 1 )
6083                                         {
6084                                                 DBG_871X( "[%s] Status = %d\n", __FUNCTION__, attr_content );
6085                                                 pwdinfo->invitereq_info.benable = _FALSE;
6086
6087                                                 if ( attr_content == P2P_STATUS_SUCCESS )
6088                                                 {
6089                                                         if ( _rtw_memcmp( pwdinfo->invitereq_info.go_bssid, myid( &padapter->eeprompriv ), ETH_ALEN ))
6090                                                         {
6091                                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO );
6092                                                         }
6093                                                         else
6094                                                         {
6095                                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
6096                                                         }
6097                                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_OK );
6098                                                 }
6099                                                 else
6100                                                 {
6101                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
6102                                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
6103                                                 }
6104                                         }
6105                                         else
6106                                         {
6107                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
6108                                                 rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
6109                                         }
6110                                 }
6111                                 else
6112                                 {
6113                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
6114                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
6115                                 }
6116
6117                                 if ( rtw_p2p_chk_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL ) )
6118                                 {
6119                                         _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
6120                                 }
6121                                 break;
6122                         }
6123                         case P2P_DEVDISC_REQ:
6124
6125                                 process_p2p_devdisc_req(pwdinfo, pframe, len);
6126
6127                                 break;
6128
6129                         case P2P_DEVDISC_RESP:
6130
6131                                 process_p2p_devdisc_resp(pwdinfo, pframe, len);
6132
6133                                 break;
6134
6135                         case P2P_PROVISION_DISC_REQ:
6136                                 DBG_871X( "[%s] Got Provisioning Discovery Request Frame\n", __FUNCTION__ );
6137                                 process_p2p_provdisc_req(pwdinfo, pframe, len);
6138                                 _rtw_memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
6139
6140                                 //20110902 Kurt
6141                                 //Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered.
6142                                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
6143                                         rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
6144
6145                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ);
6146                                 _set_timer( &pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT );
6147 #ifdef CONFIG_INTEL_WIDI
6148                                 if( (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) && (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_WFD_CONNECTION) )
6149                                 {
6150                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
6151                                         _cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
6152                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
6153                                 }
6154 #endif //CONFIG_INTEL_WIDI
6155                                 break;
6156
6157                         case P2P_PROVISION_DISC_RESP:
6158                                 //      Commented by Albert 20110707
6159                                 //      Should we check the pwdinfo->tx_prov_disc_info.bsent flag here??
6160                                 DBG_871X( "[%s] Got Provisioning Discovery Response Frame\n", __FUNCTION__ );
6161                                 //      Commented by Albert 20110426
6162                                 //      The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function.
6163                                 _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
6164                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP);
6165                                 process_p2p_provdisc_resp(pwdinfo, pframe);
6166                                 _set_timer( &pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT );
6167                                 break;
6168
6169                 }
6170         }
6171
6172
6173 exit:
6174
6175         if(merged_p2pie)
6176         {
6177                 rtw_mfree(merged_p2pie, merged_p2p_ielen + 2);
6178         }
6179 #endif //CONFIG_P2P
6180         return _SUCCESS;
6181 }
6182
6183 unsigned int on_action_public_vendor(union recv_frame *precv_frame)
6184 {
6185         unsigned int ret = _FAIL;
6186         u8 *pframe = precv_frame->u.hdr.rx_data;
6187         uint frame_len = precv_frame->u.hdr.len;
6188         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6189
6190         if (_rtw_memcmp(frame_body + 2, P2P_OUI, 4) == _TRUE) {
6191                 ret = on_action_public_p2p(precv_frame);
6192         }
6193
6194         return ret;
6195 }
6196
6197 unsigned int on_action_public_default(union recv_frame *precv_frame, u8 action)
6198 {
6199         unsigned int ret = _FAIL;
6200         u8 *pframe = precv_frame->u.hdr.rx_data;
6201         uint frame_len = precv_frame->u.hdr.len;
6202         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6203         u8 token;
6204         _adapter *adapter = precv_frame->u.hdr.adapter;
6205         int cnt = 0;
6206         char msg[64];
6207
6208         token = frame_body[2];
6209
6210         if (rtw_action_public_decache(precv_frame, token) == _FAIL)
6211                 goto exit;
6212
6213         #ifdef CONFIG_IOCTL_CFG80211
6214         cnt += sprintf((msg+cnt), "%s(token:%u)", action_public_str(action), token);
6215         rtw_cfg80211_rx_action(adapter, pframe, frame_len, msg);
6216         #endif
6217
6218         ret = _SUCCESS;
6219
6220 exit:
6221         return ret;
6222 }
6223
6224 unsigned int on_action_public(_adapter *padapter, union recv_frame *precv_frame)
6225 {
6226         unsigned int ret = _FAIL;
6227         u8 *pframe = precv_frame->u.hdr.rx_data;
6228         uint frame_len = precv_frame->u.hdr.len;
6229         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6230         u8 category, action;
6231
6232         /* check RA matches or not */
6233         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
6234                 goto exit;
6235
6236         category = frame_body[0];
6237         if(category != RTW_WLAN_CATEGORY_PUBLIC)
6238                 goto exit;
6239
6240         action = frame_body[1];
6241         switch (action) {
6242         case ACT_PUBLIC_VENDOR:
6243                 ret = on_action_public_vendor(precv_frame);
6244                 break;
6245         default:
6246                 ret = on_action_public_default(precv_frame, action);
6247                 break;
6248         }
6249
6250 exit:
6251         return ret;
6252 }
6253
6254 unsigned int OnAction_ht(_adapter *padapter, union recv_frame *precv_frame)
6255 {
6256         return _SUCCESS;
6257 }
6258
6259 #ifdef CONFIG_IEEE80211W
6260 unsigned int OnAction_sa_query(_adapter *padapter, union recv_frame *precv_frame)
6261 {
6262         u8 *pframe = precv_frame->u.hdr.rx_data;
6263         struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
6264         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6265         unsigned short tid;
6266         //Baron
6267
6268         DBG_871X("OnAction_sa_query\n");
6269
6270         switch (pframe[WLAN_HDR_A3_LEN+1])
6271         {
6272                 case 0: //SA Query req
6273                         _rtw_memcpy(&tid, &pframe[WLAN_HDR_A3_LEN+2], sizeof(unsigned short));
6274                         DBG_871X("OnAction_sa_query request,action=%d, tid=%04x\n", pframe[WLAN_HDR_A3_LEN+1], tid);
6275                         issue_action_SA_Query(padapter, GetAddr2Ptr(pframe), 1, tid);
6276                         break;
6277
6278                 case 1: //SA Query rsp
6279                         _cancel_timer_ex(&pmlmeext->sa_query_timer);
6280                         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]);
6281                         break;
6282                 default:
6283                         break;
6284         }
6285         if(0)
6286         {
6287                 int pp;
6288                 printk("pattrib->pktlen = %d =>", pattrib->pkt_len);
6289                 for(pp=0;pp< pattrib->pkt_len; pp++)
6290                         printk(" %02x ", pframe[pp]);
6291                 printk("\n");
6292         }
6293
6294         return _SUCCESS;
6295 }
6296 #endif //CONFIG_IEEE80211W
6297
6298 unsigned int OnAction_wmm(_adapter *padapter, union recv_frame *precv_frame)
6299 {
6300         return _SUCCESS;
6301 }
6302
6303 unsigned int OnAction_p2p(_adapter *padapter, union recv_frame *precv_frame)
6304 {
6305 #ifdef CONFIG_P2P
6306         u8 *frame_body;
6307         u8 category, OUI_Subtype, dialogToken=0;
6308         u8 *pframe = precv_frame->u.hdr.rx_data;
6309         uint len = precv_frame->u.hdr.len;
6310         struct  wifidirect_info *pwdinfo = &( padapter->wdinfo );
6311
6312
6313         DBG_871X("%s\n", __FUNCTION__);
6314
6315         //check RA matches or not
6316         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))//for if1, sta/ap mode
6317                 return _SUCCESS;
6318
6319         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
6320
6321         category = frame_body[0];
6322         if(category != RTW_WLAN_CATEGORY_P2P)
6323                 return _SUCCESS;
6324
6325         if ( cpu_to_be32( *( ( u32* ) ( frame_body + 1 ) ) ) != P2POUI )
6326                 return _SUCCESS;
6327
6328 #ifdef CONFIG_IOCTL_CFG80211
6329         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
6330         {
6331                 rtw_cfg80211_rx_action_p2p(padapter, pframe, len);
6332                 return _SUCCESS;
6333         }
6334         else
6335 #endif //CONFIG_IOCTL_CFG80211
6336         {
6337                 len -= sizeof(struct rtw_ieee80211_hdr_3addr);
6338                 OUI_Subtype = frame_body[5];
6339                 dialogToken = frame_body[6];
6340
6341                 switch(OUI_Subtype)
6342                 {
6343                         case P2P_NOTICE_OF_ABSENCE:
6344
6345                                 break;
6346
6347                         case P2P_PRESENCE_REQUEST:
6348
6349                                 process_p2p_presence_req(pwdinfo, pframe, len);
6350
6351                                 break;
6352
6353                         case P2P_PRESENCE_RESPONSE:
6354
6355                                 break;
6356
6357                         case P2P_GO_DISC_REQUEST:
6358
6359                                 break;
6360
6361                         default:
6362                                 break;
6363
6364                 }
6365         }
6366 #endif //CONFIG_P2P
6367
6368         return _SUCCESS;
6369
6370 }
6371
6372 unsigned int OnAction(_adapter *padapter, union recv_frame *precv_frame)
6373 {
6374         int i;
6375         unsigned char   category;
6376         struct action_handler *ptable;
6377         unsigned char   *frame_body;
6378         u8 *pframe = precv_frame->u.hdr.rx_data;
6379
6380         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
6381
6382         category = frame_body[0];
6383
6384         for(i = 0; i < sizeof(OnAction_tbl)/sizeof(struct action_handler); i++)
6385         {
6386                 ptable = &OnAction_tbl[i];
6387
6388                 if(category == ptable->num)
6389                         ptable->func(padapter, precv_frame);
6390
6391         }
6392
6393         return _SUCCESS;
6394
6395 }
6396
6397 unsigned int DoReserved(_adapter *padapter, union recv_frame *precv_frame)
6398 {
6399
6400         //DBG_871X("rcvd mgt frame(%x, %x)\n", (GetFrameSubType(pframe) >> 4), *(unsigned int *)GetAddr1Ptr(pframe));
6401         return _SUCCESS;
6402 }
6403
6404 struct xmit_frame *_alloc_mgtxmitframe(struct xmit_priv *pxmitpriv, bool once)
6405 {
6406         struct xmit_frame *pmgntframe;
6407         struct xmit_buf *pxmitbuf;
6408
6409         if (once)
6410                 pmgntframe = rtw_alloc_xmitframe_once(pxmitpriv);
6411         else
6412                 pmgntframe = rtw_alloc_xmitframe_ext(pxmitpriv);
6413
6414         if (pmgntframe == NULL) {
6415                 DBG_871X(FUNC_ADPT_FMT" alloc xmitframe fail, once:%d\n", FUNC_ADPT_ARG(pxmitpriv->adapter), once);
6416                 goto exit;
6417         }
6418
6419         if ((pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv)) == NULL) {
6420                 DBG_871X(FUNC_ADPT_FMT" alloc xmitbuf fail\n", FUNC_ADPT_ARG(pxmitpriv->adapter));
6421                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
6422                 pmgntframe = NULL;
6423                 goto exit;
6424         }
6425
6426         pmgntframe->frame_tag = MGNT_FRAMETAG;
6427         pmgntframe->pxmitbuf = pxmitbuf;
6428         pmgntframe->buf_addr = pxmitbuf->pbuf;
6429         pxmitbuf->priv_data = pmgntframe;
6430
6431 exit:
6432         return pmgntframe;
6433
6434 }
6435
6436 inline struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
6437 {
6438         return _alloc_mgtxmitframe(pxmitpriv, _FALSE);
6439 }
6440
6441 inline struct xmit_frame *alloc_mgtxmitframe_once(struct xmit_priv *pxmitpriv)
6442 {
6443         return _alloc_mgtxmitframe(pxmitpriv, _TRUE);
6444 }
6445
6446
6447 /****************************************************************************
6448
6449 Following are some TX fuctions for WiFi MLME
6450
6451 *****************************************************************************/
6452
6453 void update_mgnt_tx_rate(_adapter *padapter, u8 rate)
6454 {
6455         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6456
6457         pmlmeext->tx_rate = rate;
6458         //DBG_871X("%s(): rate = %x\n",__FUNCTION__, rate);
6459 }
6460
6461 void update_mgntframe_attrib(_adapter *padapter, struct pkt_attrib *pattrib)
6462 {
6463         u8      wireless_mode;
6464         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6465
6466         //_rtw_memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib));
6467
6468         pattrib->hdrlen = 24;
6469         pattrib->nr_frags = 1;
6470         pattrib->priority = 7;
6471         pattrib->mac_id = 0;
6472         pattrib->qsel = 0x12;
6473
6474         pattrib->pktlen = 0;
6475
6476         if (pmlmeext->tx_rate == IEEE80211_CCK_RATE_1MB)
6477                 wireless_mode = WIRELESS_11B;
6478         else
6479                 wireless_mode = WIRELESS_11G;
6480         pattrib->raid =  rtw_get_mgntframe_raid(padapter, wireless_mode);
6481
6482         pattrib->encrypt = _NO_PRIVACY_;
6483         pattrib->bswenc = _FALSE;
6484
6485         pattrib->qos_en = _FALSE;
6486         pattrib->ht_en = _FALSE;
6487         pattrib->bwmode = CHANNEL_WIDTH_20;
6488         pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
6489         pattrib->sgi = _FALSE;
6490
6491         pattrib->seqnum = pmlmeext->mgnt_seq;
6492
6493         pattrib->retry_ctrl = _TRUE;
6494
6495         pattrib->mbssid = 0;
6496
6497 }
6498
6499 void update_mgntframe_attrib_addr(_adapter *padapter, struct xmit_frame *pmgntframe)
6500 {
6501         u8      *pframe;
6502         struct pkt_attrib       *pattrib = &pmgntframe->attrib;
6503
6504         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6505
6506         _rtw_memcpy(pattrib->ra, GetAddr1Ptr(pframe), ETH_ALEN);
6507         _rtw_memcpy(pattrib->ta, GetAddr2Ptr(pframe), ETH_ALEN);
6508 }
6509
6510 void dump_mgntframe(_adapter *padapter, struct xmit_frame *pmgntframe)
6511 {
6512         if(padapter->bSurpriseRemoved == _TRUE ||
6513                 padapter->bDriverStopped == _TRUE)
6514         {
6515                 rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
6516                 rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
6517                 return;
6518         }
6519
6520         rtw_hal_mgnt_xmit(padapter, pmgntframe);
6521 }
6522
6523 s32 dump_mgntframe_and_wait(_adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
6524 {
6525         s32 ret = _FAIL;
6526         _irqL irqL;
6527         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
6528         struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
6529         struct submit_ctx sctx;
6530
6531         if(padapter->bSurpriseRemoved == _TRUE ||
6532                 padapter->bDriverStopped == _TRUE)
6533         {
6534                 rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
6535                 rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
6536                 return ret;
6537         }
6538
6539         rtw_sctx_init(&sctx, timeout_ms);
6540         pxmitbuf->sctx = &sctx;
6541
6542         ret = rtw_hal_mgnt_xmit(padapter, pmgntframe);
6543
6544         if (ret == _SUCCESS)
6545                 ret = rtw_sctx_wait(&sctx, __func__);
6546
6547         _enter_critical(&pxmitpriv->lock_sctx, &irqL);
6548         pxmitbuf->sctx = NULL;
6549         _exit_critical(&pxmitpriv->lock_sctx, &irqL);
6550
6551          return ret;
6552 }
6553
6554 s32 dump_mgntframe_and_wait_ack(_adapter *padapter, struct xmit_frame *pmgntframe)
6555 {
6556 #ifdef CONFIG_XMIT_ACK
6557         static u8 seq_no = 0;
6558         s32 ret = _FAIL;
6559         u32 timeout_ms = 500;//  500ms
6560         struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
6561         #ifdef CONFIG_CONCURRENT_MODE
6562         if (padapter->pbuddy_adapter && !padapter->isprimary)
6563                 pxmitpriv = &(padapter->pbuddy_adapter->xmitpriv);
6564         #endif
6565
6566         if(padapter->bSurpriseRemoved == _TRUE ||
6567                 padapter->bDriverStopped == _TRUE)
6568         {
6569                 rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
6570                 rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
6571                 return -1;
6572         }
6573
6574         _enter_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
6575         pxmitpriv->ack_tx = _TRUE;
6576         pxmitpriv->seq_no = seq_no++;
6577         pmgntframe->ack_report = 1;
6578         if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS) {
6579                 ret = rtw_ack_tx_wait(pxmitpriv, timeout_ms);
6580         }
6581
6582         pxmitpriv->ack_tx = _FALSE;
6583         _exit_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
6584
6585          return ret;
6586 #else //!CONFIG_XMIT_ACK
6587         dump_mgntframe(padapter, pmgntframe);
6588         rtw_msleep_os(50);
6589         return _SUCCESS;
6590 #endif //!CONFIG_XMIT_ACK
6591 }
6592
6593 int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
6594 {
6595         u8 *ssid_ie;
6596         sint ssid_len_ori;
6597         int len_diff = 0;
6598
6599         ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
6600
6601         //DBG_871X("%s hidden_ssid_mode:%u, ssid_ie:%p, ssid_len_ori:%d\n", __FUNCTION__, hidden_ssid_mode, ssid_ie, ssid_len_ori);
6602
6603         if(ssid_ie && ssid_len_ori>0)
6604         {
6605                 switch(hidden_ssid_mode)
6606                 {
6607                         case 1:
6608                         {
6609                                 u8 *next_ie = ssid_ie + 2 + ssid_len_ori;
6610                                 u32 remain_len = 0;
6611
6612                                 remain_len = ies_len -(next_ie-ies);
6613
6614                                 ssid_ie[1] = 0;
6615                                 _rtw_memcpy(ssid_ie+2, next_ie, remain_len);
6616                                 len_diff -= ssid_len_ori;
6617
6618                                 break;
6619                         }
6620                         case 2:
6621                                 _rtw_memset(&ssid_ie[2], 0, ssid_len_ori);
6622                                 break;
6623                         default:
6624                                 break;
6625                 }
6626         }
6627
6628         return len_diff;
6629 }
6630
6631 void issue_beacon(_adapter *padapter, int timeout_ms)
6632 {
6633         struct xmit_frame       *pmgntframe;
6634         struct pkt_attrib       *pattrib;
6635         unsigned char   *pframe;
6636         struct rtw_ieee80211_hdr *pwlanhdr;
6637         unsigned short *fctrl;
6638         unsigned int    rate_len;
6639         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
6640 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6641         _irqL irqL;
6642         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
6643 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6644         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6645         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6646         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
6647         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
6648 #ifdef CONFIG_P2P
6649         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
6650 #endif //CONFIG_P2P
6651
6652
6653         //DBG_871X("%s\n", __FUNCTION__);
6654
6655         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
6656         {
6657                 DBG_871X("%s, alloc mgnt frame fail\n", __FUNCTION__);
6658                 return;
6659         }
6660 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6661         _enter_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
6662 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6663
6664         //update attribute
6665         pattrib = &pmgntframe->attrib;
6666         update_mgntframe_attrib(padapter, pattrib);
6667         pattrib->qsel = 0x10;
6668         #ifdef CONFIG_CONCURRENT_MODE
6669         if(padapter->iface_type == IFACE_PORT1)
6670                 pattrib->mbssid = 1;
6671         #endif
6672
6673         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
6674
6675         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6676         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
6677
6678
6679         fctrl = &(pwlanhdr->frame_ctl);
6680         *(fctrl) = 0;
6681
6682         _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
6683         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
6684         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
6685
6686         SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
6687         //pmlmeext->mgnt_seq++;
6688         SetFrameSubType(pframe, WIFI_BEACON);
6689
6690         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
6691         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
6692
6693         if( (pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
6694         {
6695                 //DBG_871X("ie len=%d\n", cur_network->IELength);
6696 #ifdef CONFIG_P2P
6697                 // for P2P : Primary Device Type & Device Name
6698                 u32 wpsielen=0, insert_len=0;
6699                 u8 *wpsie=NULL;
6700                 wpsie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wpsielen);
6701
6702                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wpsie && wpsielen>0)
6703                 {
6704                         uint wps_offset, remainder_ielen;
6705                         u8 *premainder_ie, *pframe_wscie;
6706
6707                         wps_offset = (uint)(wpsie - cur_network->IEs);
6708
6709                         premainder_ie = wpsie + wpsielen;
6710
6711                         remainder_ielen = cur_network->IELength - wps_offset - wpsielen;
6712
6713 #ifdef CONFIG_IOCTL_CFG80211
6714                         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
6715                         {
6716                                 if(pmlmepriv->wps_beacon_ie && pmlmepriv->wps_beacon_ie_len>0)
6717                                 {
6718                                         _rtw_memcpy(pframe, cur_network->IEs, wps_offset);
6719                                         pframe += wps_offset;
6720                                         pattrib->pktlen += wps_offset;
6721
6722                                         _rtw_memcpy(pframe, pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len);
6723                                         pframe += pmlmepriv->wps_beacon_ie_len;
6724                                         pattrib->pktlen += pmlmepriv->wps_beacon_ie_len;
6725
6726                                         //copy remainder_ie to pframe
6727                                         _rtw_memcpy(pframe, premainder_ie, remainder_ielen);
6728                                         pframe += remainder_ielen;
6729                                         pattrib->pktlen += remainder_ielen;
6730                                 }
6731                                 else
6732                                 {
6733                                         _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
6734                                         pframe += cur_network->IELength;
6735                                         pattrib->pktlen += cur_network->IELength;
6736                                 }
6737                         }
6738                         else
6739 #endif //CONFIG_IOCTL_CFG80211
6740                         {
6741                                 pframe_wscie = pframe + wps_offset;
6742                                 _rtw_memcpy(pframe, cur_network->IEs, wps_offset+wpsielen);
6743                                 pframe += (wps_offset + wpsielen);
6744                                 pattrib->pktlen += (wps_offset + wpsielen);
6745
6746                                 //now pframe is end of wsc ie, insert Primary Device Type & Device Name
6747                                 //      Primary Device Type
6748                                 //      Type:
6749                                 *(u16*) ( pframe + insert_len) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
6750                                 insert_len += 2;
6751
6752                                 //      Length:
6753                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( 0x0008 );
6754                                 insert_len += 2;
6755
6756                                 //      Value:
6757                                 //      Category ID
6758                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
6759                                 insert_len += 2;
6760
6761                                 //      OUI
6762                                 *(u32*) ( pframe + insert_len ) = cpu_to_be32( WPSOUI );
6763                                 insert_len += 4;
6764
6765                                 //      Sub Category ID
6766                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
6767                                 insert_len += 2;
6768
6769
6770                                 //      Device Name
6771                                 //      Type:
6772                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
6773                                 insert_len += 2;
6774
6775                                 //      Length:
6776                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( pwdinfo->device_name_len );
6777                                 insert_len += 2;
6778
6779                                 //      Value:
6780                                 _rtw_memcpy( pframe + insert_len, pwdinfo->device_name, pwdinfo->device_name_len );
6781                                 insert_len += pwdinfo->device_name_len;
6782
6783
6784                                 //update wsc ie length
6785                                 *(pframe_wscie+1) = (wpsielen -2) + insert_len;
6786
6787                                 //pframe move to end
6788                                 pframe+=insert_len;
6789                                 pattrib->pktlen += insert_len;
6790
6791                                 //copy remainder_ie to pframe
6792                                 _rtw_memcpy(pframe, premainder_ie, remainder_ielen);
6793                                 pframe += remainder_ielen;
6794                                 pattrib->pktlen += remainder_ielen;
6795                         }
6796                 }
6797                 else
6798 #endif //CONFIG_P2P
6799                 {
6800                         int len_diff;
6801                         _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
6802                         len_diff = update_hidden_ssid(
6803                                 pframe+_BEACON_IE_OFFSET_
6804                                 , cur_network->IELength-_BEACON_IE_OFFSET_
6805                                 , pmlmeinfo->hidden_ssid_mode
6806                         );
6807                         pframe += (cur_network->IELength+len_diff);
6808                         pattrib->pktlen += (cur_network->IELength+len_diff);
6809                 }
6810
6811                 {
6812                         u8 *wps_ie;
6813                         uint wps_ielen;
6814                         u8 sr = 0;
6815                         wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr+TXDESC_OFFSET+sizeof (struct rtw_ieee80211_hdr_3addr)+_BEACON_IE_OFFSET_,
6816                                 pattrib->pktlen-sizeof (struct rtw_ieee80211_hdr_3addr)-_BEACON_IE_OFFSET_, NULL, &wps_ielen);
6817                         if (wps_ie && wps_ielen>0) {
6818                                 rtw_get_wps_attr_content(wps_ie,  wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8*)(&sr), NULL);
6819                         }
6820                         if (sr != 0)
6821                                 set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
6822                         else
6823                                 _clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
6824                 }
6825
6826 #ifdef CONFIG_P2P
6827                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
6828                 {
6829                         u32 len;
6830 #ifdef CONFIG_IOCTL_CFG80211
6831                         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
6832                         {
6833                                 len = pmlmepriv->p2p_beacon_ie_len;
6834                                 if(pmlmepriv->p2p_beacon_ie && len>0)
6835                                         _rtw_memcpy(pframe, pmlmepriv->p2p_beacon_ie, len);
6836                         }
6837                         else
6838 #endif //CONFIG_IOCTL_CFG80211
6839                         {
6840                                 len = build_beacon_p2p_ie(pwdinfo, pframe);
6841                         }
6842
6843                         pframe += len;
6844                         pattrib->pktlen += len;
6845 #ifdef CONFIG_WFD
6846 #ifdef CONFIG_IOCTL_CFG80211
6847                         if(_TRUE == pwdinfo->wfd_info->wfd_enable)
6848 #endif //CONFIG_IOCTL_CFG80211
6849                         {
6850                         len = build_beacon_wfd_ie( pwdinfo, pframe );
6851                         }
6852 #ifdef CONFIG_IOCTL_CFG80211
6853                         else
6854                         {
6855                                 len = 0;
6856                                 if(pmlmepriv->wfd_beacon_ie && pmlmepriv->wfd_beacon_ie_len>0)
6857                                 {
6858                                         len = pmlmepriv->wfd_beacon_ie_len;
6859                                         _rtw_memcpy(pframe, pmlmepriv->wfd_beacon_ie, len);
6860                                 }
6861                         }
6862 #endif //CONFIG_IOCTL_CFG80211
6863                         pframe += len;
6864                         pattrib->pktlen += len;
6865 #endif //CONFIG_WFD
6866                 }
6867 #endif //CONFIG_P2P
6868
6869                 goto _issue_bcn;
6870
6871         }
6872
6873         //below for ad-hoc mode
6874
6875         //timestamp will be inserted by hardware
6876         pframe += 8;
6877         pattrib->pktlen += 8;
6878
6879         // beacon interval: 2 bytes
6880
6881         _rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
6882
6883         pframe += 2;
6884         pattrib->pktlen += 2;
6885
6886         // capability info: 2 bytes
6887
6888         _rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
6889
6890         pframe += 2;
6891         pattrib->pktlen += 2;
6892
6893         // SSID
6894         pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
6895
6896         // supported rates...
6897         rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
6898         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8)? 8: rate_len), cur_network->SupportedRates, &pattrib->pktlen);
6899
6900         // DS parameter set
6901         pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
6902
6903         //if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
6904         {
6905                 u8 erpinfo=0;
6906                 u32 ATIMWindow;
6907                 // IBSS Parameter Set...
6908                 //ATIMWindow = cur->Configuration.ATIMWindow;
6909                 ATIMWindow = 0;
6910                 pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
6911
6912                 //ERP IE
6913                 pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
6914         }
6915
6916
6917         // EXTERNDED SUPPORTED RATE
6918         if (rate_len > 8)
6919         {
6920                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
6921         }
6922
6923
6924         //todo:HT for adhoc
6925
6926 _issue_bcn:
6927
6928 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6929         pmlmepriv->update_bcn = _FALSE;
6930
6931         _exit_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
6932 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6933
6934         if ((pattrib->pktlen + TXDESC_SIZE) > 512)
6935         {
6936                 DBG_871X("beacon frame too large\n");
6937                 return;
6938         }
6939
6940         pattrib->last_txcmdsz = pattrib->pktlen;
6941
6942         //DBG_871X("issue bcn_sz=%d\n", pattrib->last_txcmdsz);
6943         if(timeout_ms > 0)
6944                 dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
6945         else
6946                 dump_mgntframe(padapter, pmgntframe);
6947
6948 }
6949
6950 void issue_probersp(_adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
6951 {
6952         struct xmit_frame                       *pmgntframe;
6953         struct pkt_attrib                       *pattrib;
6954         unsigned char                                   *pframe;
6955         struct rtw_ieee80211_hdr        *pwlanhdr;
6956         unsigned short                          *fctrl;
6957         unsigned char                                   *mac, *bssid;
6958         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
6959 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6960         u8 *pwps_ie;
6961         uint wps_ielen;
6962         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
6963 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6964         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6965         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6966         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
6967         unsigned int    rate_len;
6968 #ifdef CONFIG_P2P
6969         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
6970 #ifdef CONFIG_WFD
6971         u32                                     wfdielen = 0;
6972 #endif //CONFIG_WFD
6973 #endif //CONFIG_P2P
6974
6975         //DBG_871X("%s\n", __FUNCTION__);
6976
6977         if(da == NULL)
6978                 return;
6979
6980         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
6981         {
6982                 DBG_871X("%s, alloc mgnt frame fail\n", __FUNCTION__);
6983                 return;
6984         }
6985
6986
6987         //update attribute
6988         pattrib = &pmgntframe->attrib;
6989         update_mgntframe_attrib(padapter, pattrib);
6990
6991         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
6992
6993         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6994         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
6995
6996         mac = myid(&(padapter->eeprompriv));
6997         bssid = cur_network->MacAddress;
6998
6999         fctrl = &(pwlanhdr->frame_ctl);
7000         *(fctrl) = 0;
7001         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
7002         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
7003         _rtw_memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
7004
7005         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7006         pmlmeext->mgnt_seq++;
7007         SetFrameSubType(fctrl, WIFI_PROBERSP);
7008
7009         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7010         pattrib->pktlen = pattrib->hdrlen;
7011         pframe += pattrib->hdrlen;
7012
7013
7014         if(cur_network->IELength>MAX_IE_SZ)
7015                 return;
7016
7017 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7018         if( (pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
7019         {
7020                 pwps_ie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
7021
7022                 //inerset & update wps_probe_resp_ie
7023                 if((pmlmepriv->wps_probe_resp_ie!=NULL) && pwps_ie && (wps_ielen>0))
7024                 {
7025                         uint wps_offset, remainder_ielen;
7026                         u8 *premainder_ie;
7027
7028                         wps_offset = (uint)(pwps_ie - cur_network->IEs);
7029
7030                         premainder_ie = pwps_ie + wps_ielen;
7031
7032                         remainder_ielen = cur_network->IELength - wps_offset - wps_ielen;
7033
7034                         _rtw_memcpy(pframe, cur_network->IEs, wps_offset);
7035                         pframe += wps_offset;
7036                         pattrib->pktlen += wps_offset;
7037
7038                         wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];//to get ie data len
7039                         if((wps_offset+wps_ielen+2)<=MAX_IE_SZ)
7040                         {
7041                                 _rtw_memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen+2);
7042                                 pframe += wps_ielen+2;
7043                                 pattrib->pktlen += wps_ielen+2;
7044                         }
7045
7046                         if((wps_offset+wps_ielen+2+remainder_ielen)<=MAX_IE_SZ)
7047                         {
7048                                 _rtw_memcpy(pframe, premainder_ie, remainder_ielen);
7049                                 pframe += remainder_ielen;
7050                                 pattrib->pktlen += remainder_ielen;
7051                         }
7052                 }
7053                 else
7054                 {
7055                         _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
7056                         pframe += cur_network->IELength;
7057                         pattrib->pktlen += cur_network->IELength;
7058                 }
7059
7060                 /* retrieve SSID IE from cur_network->Ssid */
7061                 {
7062                         u8 *ssid_ie;
7063                         sint ssid_ielen;
7064                         sint ssid_ielen_diff;
7065                         u8 buf[MAX_IE_SZ];
7066                         u8 *ies = pmgntframe->buf_addr+TXDESC_OFFSET+sizeof(struct rtw_ieee80211_hdr_3addr);
7067
7068                         ssid_ie = rtw_get_ie(ies+_FIXED_IE_LENGTH_, _SSID_IE_, &ssid_ielen,
7069                                 (pframe-ies)-_FIXED_IE_LENGTH_);
7070
7071                         ssid_ielen_diff = cur_network->Ssid.SsidLength - ssid_ielen;
7072
7073                         if (ssid_ie &&  cur_network->Ssid.SsidLength) {
7074                                 uint remainder_ielen;
7075                                 u8 *remainder_ie;
7076                                 remainder_ie = ssid_ie+2;
7077                                 remainder_ielen = (pframe-remainder_ie);
7078
7079                                 if (remainder_ielen > MAX_IE_SZ) {
7080                                         DBG_871X_LEVEL(_drv_warning_, FUNC_ADPT_FMT" remainder_ielen > MAX_IE_SZ\n", FUNC_ADPT_ARG(padapter));
7081                                         remainder_ielen = MAX_IE_SZ;
7082                                 }
7083
7084                                 _rtw_memcpy(buf, remainder_ie, remainder_ielen);
7085                                 _rtw_memcpy(remainder_ie+ssid_ielen_diff, buf, remainder_ielen);
7086                                 *(ssid_ie+1) = cur_network->Ssid.SsidLength;
7087                                 _rtw_memcpy(ssid_ie+2, cur_network->Ssid.Ssid, cur_network->Ssid.SsidLength);
7088
7089                                 pframe += ssid_ielen_diff;
7090                                 pattrib->pktlen += ssid_ielen_diff;
7091                         }
7092                 }
7093         }
7094         else
7095 #endif
7096         {
7097
7098                 //timestamp will be inserted by hardware
7099                 pframe += 8;
7100                 pattrib->pktlen += 8;
7101
7102                 // beacon interval: 2 bytes
7103
7104                 _rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
7105
7106                 pframe += 2;
7107                 pattrib->pktlen += 2;
7108
7109                 // capability info: 2 bytes
7110
7111                 _rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
7112
7113                 pframe += 2;
7114                 pattrib->pktlen += 2;
7115
7116                 //below for ad-hoc mode
7117
7118                 // SSID
7119                 pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
7120
7121                 // supported rates...
7122                 rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
7123                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8)? 8: rate_len), cur_network->SupportedRates, &pattrib->pktlen);
7124
7125                 // DS parameter set
7126                 pframe =rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
7127
7128                 if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
7129                 {
7130                         u8 erpinfo=0;
7131                         u32 ATIMWindow;
7132                         // IBSS Parameter Set...
7133                         //ATIMWindow = cur->Configuration.ATIMWindow;
7134                         ATIMWindow = 0;
7135                         pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
7136
7137                         //ERP IE
7138                         pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
7139                 }
7140
7141
7142                 // EXTERNDED SUPPORTED RATE
7143                 if (rate_len > 8)
7144                 {
7145                         pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
7146                 }
7147
7148
7149                 //todo:HT for adhoc
7150
7151         }
7152
7153 #ifdef CONFIG_P2P
7154         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) /*&& is_valid_p2p_probereq*/)
7155         {
7156                 u32 len;
7157 #ifdef CONFIG_IOCTL_CFG80211
7158                 if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
7159                 {
7160                         //if pwdinfo->role == P2P_ROLE_DEVICE will call issue_probersp_p2p()
7161                         len = pmlmepriv->p2p_go_probe_resp_ie_len;
7162                         if(pmlmepriv->p2p_go_probe_resp_ie && len>0)
7163                                 _rtw_memcpy(pframe, pmlmepriv->p2p_go_probe_resp_ie, len);
7164                 }
7165                 else
7166 #endif //CONFIG_IOCTL_CFG80211
7167                 {
7168                         len = build_probe_resp_p2p_ie(pwdinfo, pframe);
7169                 }
7170
7171                 pframe += len;
7172                 pattrib->pktlen += len;
7173
7174 #ifdef CONFIG_WFD
7175 #ifdef CONFIG_IOCTL_CFG80211
7176                 if(_TRUE == pwdinfo->wfd_info->wfd_enable)
7177 #endif //CONFIG_IOCTL_CFG80211
7178                 {
7179                         len = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
7180                 }
7181 #ifdef CONFIG_IOCTL_CFG80211
7182                 else
7183                 {
7184                         len = 0;
7185                         if(pmlmepriv->wfd_probe_resp_ie && pmlmepriv->wfd_probe_resp_ie_len>0)
7186                         {
7187                                 len = pmlmepriv->wfd_probe_resp_ie_len;
7188                                 _rtw_memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, len);
7189                         }
7190                 }
7191 #endif //CONFIG_IOCTL_CFG80211
7192                 pframe += len;
7193                 pattrib->pktlen += len;
7194 #endif //CONFIG_WFD
7195
7196         }
7197 #endif //CONFIG_P2P
7198
7199
7200 #ifdef CONFIG_AUTO_AP_MODE
7201 {
7202         struct sta_info *psta;
7203         struct sta_priv *pstapriv = &padapter->stapriv;
7204
7205         DBG_871X("(%s)\n", __FUNCTION__);
7206
7207         //check rc station
7208         psta = rtw_get_stainfo(pstapriv, da);
7209         if (psta && psta->isrc && psta->pid>0)
7210         {
7211                 u8 RC_OUI[4]={0x00,0xE0,0x4C,0x0A};
7212                 u8 RC_INFO[14] = {0};
7213                 //EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2]
7214                 u16 cu_ch = (u16)cur_network->Configuration.DSConfig;
7215
7216                 DBG_871X("%s, reply rc(pid=0x%x) device "MAC_FMT" in ch=%d\n", __FUNCTION__,
7217                         psta->pid, MAC_ARG(psta->hwaddr), cu_ch);
7218
7219                 //append vendor specific ie
7220                 _rtw_memcpy(RC_INFO, RC_OUI, sizeof(RC_OUI));
7221                 _rtw_memcpy(&RC_INFO[4], mac, ETH_ALEN);
7222                 _rtw_memcpy(&RC_INFO[10], (u8*)&psta->pid, 2);
7223                 _rtw_memcpy(&RC_INFO[12], (u8*)&cu_ch, 2);
7224
7225                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, sizeof(RC_INFO), RC_INFO, &pattrib->pktlen);
7226         }
7227 }
7228 #endif //CONFIG_AUTO_AP_MODE
7229
7230
7231         pattrib->last_txcmdsz = pattrib->pktlen;
7232
7233
7234         dump_mgntframe(padapter, pmgntframe);
7235
7236         return;
7237
7238 }
7239
7240 int _issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da, int wait_ack)
7241 {
7242         int ret = _FAIL;
7243         struct xmit_frame               *pmgntframe;
7244         struct pkt_attrib               *pattrib;
7245         unsigned char                   *pframe;
7246         struct rtw_ieee80211_hdr        *pwlanhdr;
7247         unsigned short          *fctrl;
7248         unsigned char                   *mac;
7249         unsigned char                   bssrate[NumRates];
7250         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
7251         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
7252         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7253         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7254         int     bssrate_len = 0;
7255         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
7256
7257         RT_TRACE(_module_rtl871x_mlme_c_,_drv_notice_,("+issue_probereq\n"));
7258
7259         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7260         {
7261                 goto exit;
7262         }
7263
7264         //update attribute
7265         pattrib = &pmgntframe->attrib;
7266         update_mgntframe_attrib(padapter, pattrib);
7267
7268
7269         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7270
7271         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7272         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7273
7274         mac = myid(&(padapter->eeprompriv));
7275
7276         fctrl = &(pwlanhdr->frame_ctl);
7277         *(fctrl) = 0;
7278
7279         if (da)
7280         {
7281                 //      unicast probe request frame
7282                 _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
7283                 _rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN);
7284         }
7285         else
7286         {
7287                 //      broadcast probe request frame
7288                 _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
7289                 _rtw_memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
7290         }
7291
7292         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
7293
7294         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7295         pmlmeext->mgnt_seq++;
7296         SetFrameSubType(pframe, WIFI_PROBEREQ);
7297
7298         pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
7299         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
7300
7301         if(pssid)
7302                 pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &(pattrib->pktlen));
7303         else
7304                 pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &(pattrib->pktlen));
7305
7306         get_rate_set(padapter, bssrate, &bssrate_len);
7307
7308         if (bssrate_len > 8)
7309         {
7310                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
7311                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
7312         }
7313         else
7314         {
7315                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
7316         }
7317
7318 #if 0
7319         //add wps_ie for wps2.0
7320         if(pmlmepriv->probereq_wpsie_len>0 && pmlmepriv->probereq_wpsie_len<MAX_WPS_IE_LEN)
7321         {
7322                 _rtw_memcpy(pframe, pmlmepriv->probereq_wpsie, pmlmepriv->probereq_wpsie_len);
7323                 pframe += pmlmepriv->probereq_wpsie_len;
7324                 pattrib->pktlen += pmlmepriv->probereq_wpsie_len;
7325                 //pmlmepriv->probereq_wpsie_len = 0 ;//reset to zero
7326         }
7327 #else
7328         //add wps_ie for wps2.0
7329         if(pmlmepriv->wps_probe_req_ie_len>0 && pmlmepriv->wps_probe_req_ie)
7330         {
7331                 _rtw_memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
7332                 pframe += pmlmepriv->wps_probe_req_ie_len;
7333                 pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
7334                 //pmlmepriv->wps_probe_req_ie_len = 0 ;//reset to zero
7335         }
7336 #endif
7337
7338         pattrib->last_txcmdsz = pattrib->pktlen;
7339
7340         RT_TRACE(_module_rtl871x_mlme_c_,_drv_notice_,("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
7341
7342         if (wait_ack) {
7343                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
7344         } else {
7345                 dump_mgntframe(padapter, pmgntframe);
7346                 ret = _SUCCESS;
7347         }
7348
7349 exit:
7350         return ret;
7351 }
7352
7353 inline void issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da)
7354 {
7355         _issue_probereq(padapter, pssid, da, _FALSE);
7356 }
7357
7358 int issue_probereq_ex(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da,
7359         int try_cnt, int wait_ms)
7360 {
7361         int ret;
7362         int i = 0;
7363         u32 start = rtw_get_current_time();
7364
7365         do
7366         {
7367                 ret = _issue_probereq(padapter, pssid, da, wait_ms>0?_TRUE:_FALSE);
7368
7369                 i++;
7370
7371                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
7372                         break;
7373
7374                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
7375                         rtw_msleep_os(wait_ms);
7376
7377         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
7378
7379         if (ret != _FAIL) {
7380                 ret = _SUCCESS;
7381                 #ifndef DBG_XMIT_ACK
7382                 goto exit;
7383                 #endif
7384         }
7385
7386         if (try_cnt && wait_ms) {
7387                 if (da)
7388                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
7389                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
7390                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7391                 else
7392                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
7393                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
7394                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7395         }
7396 exit:
7397         return ret;
7398 }
7399
7400 // if psta == NULL, indiate we are station(client) now...
7401 void issue_auth(_adapter *padapter, struct sta_info *psta, unsigned short status)
7402 {
7403         struct xmit_frame                       *pmgntframe;
7404         struct pkt_attrib                       *pattrib;
7405         unsigned char                                   *pframe;
7406         struct rtw_ieee80211_hdr        *pwlanhdr;
7407         unsigned short                          *fctrl;
7408         unsigned int                                    val32;
7409         unsigned short                          val16;
7410         int use_shared_key = 0;
7411         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
7412         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7413         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7414
7415         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7416         {
7417                 return;
7418         }
7419
7420         //update attribute
7421         pattrib = &pmgntframe->attrib;
7422         update_mgntframe_attrib(padapter, pattrib);
7423
7424         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7425
7426         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7427         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7428
7429         fctrl = &(pwlanhdr->frame_ctl);
7430         *(fctrl) = 0;
7431
7432         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7433         pmlmeext->mgnt_seq++;
7434         SetFrameSubType(pframe, WIFI_AUTH);
7435
7436         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
7437         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7438
7439
7440         if(psta)// for AP mode
7441         {
7442 #ifdef CONFIG_NATIVEAP_MLME
7443
7444                 _rtw_memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);
7445                 _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
7446                 _rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
7447
7448
7449                 // setting auth algo number
7450                 val16 = (u16)psta->authalg;
7451
7452                 if(status != _STATS_SUCCESSFUL_)
7453                         val16 = 0;
7454
7455                 if (val16)      {
7456                         val16 = cpu_to_le16(val16);
7457                         use_shared_key = 1;
7458                 }
7459
7460                 pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7461
7462                 // setting auth seq number
7463                 val16 =(u16)psta->auth_seq;
7464                 val16 = cpu_to_le16(val16);
7465                 pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7466
7467                 // setting status code...
7468                 val16 = status;
7469                 val16 = cpu_to_le16(val16);
7470                 pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen));
7471
7472                 // added challenging text...
7473                 if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
7474                 {
7475                         pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, psta->chg_txt, &(pattrib->pktlen));
7476                 }
7477 #endif
7478         }
7479         else
7480         {
7481                 _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
7482                 _rtw_memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
7483                 _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
7484
7485                 // setting auth algo number
7486                 val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)? 1: 0;// 0:OPEN System, 1:Shared key
7487                 if (val16)      {
7488                         val16 = cpu_to_le16(val16);
7489                         use_shared_key = 1;
7490                 }
7491                 //DBG_871X("%s auth_algo= %s auth_seq=%d\n",__FUNCTION__,(pmlmeinfo->auth_algo==0)?"OPEN":"SHARED",pmlmeinfo->auth_seq);
7492
7493                 //setting IV for auth seq #3
7494                 if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
7495                 {
7496                         //DBG_871X("==> iv(%d),key_index(%d)\n",pmlmeinfo->iv,pmlmeinfo->key_index);
7497                         val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
7498                         val32 = cpu_to_le32(val32);
7499                         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&val32, &(pattrib->pktlen));
7500
7501                         pattrib->iv_len = 4;
7502                 }
7503
7504                 pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7505
7506                 // setting auth seq number
7507                 val16 = pmlmeinfo->auth_seq;
7508                 val16 = cpu_to_le16(val16);
7509                 pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7510
7511
7512                 // setting status code...
7513                 val16 = status;
7514                 val16 = cpu_to_le16(val16);
7515                 pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen));
7516
7517                 // then checking to see if sending challenging text...
7518                 if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
7519                 {
7520                         pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen));
7521
7522                         SetPrivacy(fctrl);
7523
7524                         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7525
7526                         pattrib->encrypt = _WEP40_;
7527
7528                         pattrib->icv_len = 4;
7529
7530                         pattrib->pktlen += pattrib->icv_len;
7531
7532                 }
7533
7534         }
7535
7536         pattrib->last_txcmdsz = pattrib->pktlen;
7537
7538         rtw_wep_encrypt(padapter, (u8 *)pmgntframe);
7539         DBG_871X("%s\n", __FUNCTION__);
7540         dump_mgntframe(padapter, pmgntframe);
7541
7542         return;
7543 }
7544
7545
7546 void issue_asocrsp(_adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
7547 {
7548 #ifdef CONFIG_AP_MODE
7549         struct xmit_frame       *pmgntframe;
7550         struct rtw_ieee80211_hdr        *pwlanhdr;
7551         struct pkt_attrib *pattrib;
7552         unsigned char   *pbuf, *pframe;
7553         unsigned short val;
7554         unsigned short *fctrl;
7555         struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
7556         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
7557         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
7558         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7559         WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);
7560         u8 *ie = pnetwork->IEs;
7561 #ifdef CONFIG_P2P
7562         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
7563 #ifdef CONFIG_WFD
7564         u32                                     wfdielen = 0;
7565 #endif //CONFIG_WFD
7566
7567 #endif //CONFIG_P2P
7568
7569         DBG_871X("%s\n", __FUNCTION__);
7570
7571         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7572         {
7573                 return;
7574         }
7575
7576         //update attribute
7577         pattrib = &pmgntframe->attrib;
7578         update_mgntframe_attrib(padapter, pattrib);
7579
7580
7581         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7582
7583         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7584         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7585
7586         fctrl = &(pwlanhdr->frame_ctl);
7587         *(fctrl) = 0;
7588
7589         _rtw_memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
7590         _rtw_memcpy((void *)GetAddr2Ptr(pwlanhdr), myid(&(padapter->eeprompriv)), ETH_ALEN);
7591         _rtw_memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7592
7593
7594         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7595         pmlmeext->mgnt_seq++;
7596         if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
7597                 SetFrameSubType(pwlanhdr, pkt_type);
7598         else
7599                 return;
7600
7601         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7602         pattrib->pktlen += pattrib->hdrlen;
7603         pframe += pattrib->hdrlen;
7604
7605         //capability
7606         val = *(unsigned short *)rtw_get_capability_from_ie(ie);
7607
7608         pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_ , (unsigned char *)&val, &(pattrib->pktlen));
7609
7610         status = cpu_to_le16(status);
7611         pframe = rtw_set_fixed_ie(pframe , _STATUS_CODE_ , (unsigned char *)&status, &(pattrib->pktlen));
7612
7613         val = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
7614         pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_ , (unsigned char *)&val, &(pattrib->pktlen));
7615
7616         if (pstat->bssratelen <= 8)
7617         {
7618                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen));
7619         }
7620         else
7621         {
7622                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &(pattrib->pktlen));
7623                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (pstat->bssratelen-8), pstat->bssrateset+8, &(pattrib->pktlen));
7624         }
7625
7626 #ifdef CONFIG_80211N_HT
7627         if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option))
7628         {
7629                 uint ie_len=0;
7630
7631                 //FILL HT CAP INFO IE
7632                 //p = hostapd_eid_ht_capabilities_info(hapd, p);
7633                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
7634                 if(pbuf && ie_len>0)
7635                 {
7636                         _rtw_memcpy(pframe, pbuf, ie_len+2);
7637                         pframe += (ie_len+2);
7638                         pattrib->pktlen +=(ie_len+2);
7639                 }
7640
7641                 //FILL HT ADD INFO IE
7642                 //p = hostapd_eid_ht_operation(hapd, p);
7643                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
7644                 if(pbuf && ie_len>0)
7645                 {
7646                         _rtw_memcpy(pframe, pbuf, ie_len+2);
7647                         pframe += (ie_len+2);
7648                         pattrib->pktlen +=(ie_len+2);
7649                 }
7650
7651         }
7652 #endif
7653
7654 #ifdef CONFIG_80211AC_VHT
7655         if ((pstat->flags & WLAN_STA_VHT) && (pmlmepriv->vhtpriv.vht_option))
7656         {
7657                 u32 ie_len=0;
7658
7659                 //FILL VHT CAP IE
7660                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, EID_VHTCapability, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
7661                 if(pbuf && ie_len>0)
7662                 {
7663                         _rtw_memcpy(pframe, pbuf, ie_len+2);
7664                         pframe += (ie_len+2);
7665                         pattrib->pktlen +=(ie_len+2);
7666                 }
7667
7668                 //FILL VHT OPERATION IE
7669                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, EID_VHTOperation, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
7670                 if(pbuf && ie_len>0)
7671                 {
7672                         _rtw_memcpy(pframe, pbuf, ie_len+2);
7673                         pframe += (ie_len+2);
7674                         pattrib->pktlen +=(ie_len+2);
7675                 }
7676         }
7677 #endif //CONFIG_80211AC_VHT
7678
7679         //FILL WMM IE
7680         if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option))
7681         {
7682                 uint ie_len=0;
7683                 unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
7684
7685                 for (pbuf = ie + _BEACON_IE_OFFSET_; ;pbuf+= (ie_len + 2))
7686                 {
7687                         pbuf = rtw_get_ie(pbuf, _VENDOR_SPECIFIC_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
7688                         if(pbuf && _rtw_memcmp(pbuf+2, WMM_PARA_IE, 6))
7689                         {
7690                                 _rtw_memcpy(pframe, pbuf, ie_len+2);
7691                                 pframe += (ie_len+2);
7692                                 pattrib->pktlen +=(ie_len+2);
7693
7694                                 break;
7695                         }
7696
7697                         if ((pbuf == NULL) || (ie_len == 0))
7698                         {
7699                                 break;
7700                         }
7701                 }
7702
7703         }
7704
7705
7706         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
7707         {
7708                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
7709         }
7710
7711         //add WPS IE ie for wps 2.0
7712         if(pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len>0)
7713         {
7714                 _rtw_memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len);
7715
7716                 pframe += pmlmepriv->wps_assoc_resp_ie_len;
7717                 pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
7718         }
7719
7720 #ifdef CONFIG_P2P
7721         if( padapter->wdinfo.driver_interface == DRIVER_WEXT )
7722         {
7723                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && (pstat->is_p2p_device == _TRUE))
7724                 {
7725                         u32 len;
7726
7727                         len = build_assoc_resp_p2p_ie(pwdinfo, pframe, pstat->p2p_status_code);
7728
7729                         pframe += len;
7730                         pattrib->pktlen += len;
7731                 }
7732         }
7733 #ifdef CONFIG_WFD
7734         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)
7735 #ifdef CONFIG_IOCTL_CFG80211
7736                 && (_TRUE == pwdinfo->wfd_info->wfd_enable)
7737 #endif //CONFIG_IOCTL_CFG80211
7738         )
7739         {
7740                 wfdielen = build_assoc_resp_wfd_ie(pwdinfo, pframe);
7741                 pframe += wfdielen;
7742                 pattrib->pktlen += wfdielen;
7743         }
7744 #endif //CONFIG_WFD
7745 #endif //CONFIG_P2P
7746
7747         pattrib->last_txcmdsz = pattrib->pktlen;
7748
7749         dump_mgntframe(padapter, pmgntframe);
7750
7751 #endif
7752 }
7753
7754 void issue_assocreq(_adapter *padapter)
7755 {
7756         int ret = _FAIL;
7757         struct xmit_frame                               *pmgntframe;
7758         struct pkt_attrib                               *pattrib;
7759         unsigned char                                   *pframe, *p;
7760         struct rtw_ieee80211_hdr                        *pwlanhdr;
7761         unsigned short                          *fctrl;
7762         unsigned short                          val16;
7763         unsigned int                                    i, j, ie_len, index=0;
7764         unsigned char                                   rf_type, bssrate[NumRates], sta_bssrate[NumRates];
7765         PNDIS_802_11_VARIABLE_IEs       pIE;
7766         struct registry_priv    *pregpriv = &padapter->registrypriv;
7767         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
7768         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
7769         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7770         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7771         int     bssrate_len = 0, sta_bssrate_len = 0;
7772 #ifdef CONFIG_P2P
7773         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
7774         u8                                      p2pie[ 255 ] = { 0x00 };
7775         u16                                     p2pielen = 0;
7776 #ifdef CONFIG_WFD
7777         u32                                     wfdielen = 0;
7778 #endif //CONFIG_WFD
7779 #endif //CONFIG_P2P
7780
7781 #ifdef CONFIG_DFS
7782         u16     cap;
7783 #endif //CONFIG_DFS
7784
7785         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7786                 goto exit;
7787
7788         //update attribute
7789         pattrib = &pmgntframe->attrib;
7790         update_mgntframe_attrib(padapter, pattrib);
7791
7792
7793         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7794
7795         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7796         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7797
7798         fctrl = &(pwlanhdr->frame_ctl);
7799         *(fctrl) = 0;
7800         _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7801         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
7802         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7803
7804         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7805         pmlmeext->mgnt_seq++;
7806         SetFrameSubType(pframe, WIFI_ASSOCREQ);
7807
7808         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
7809         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7810
7811         //caps
7812
7813 #ifdef CONFIG_DFS
7814         _rtw_memcpy(&cap, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
7815         cap |= BIT(8);
7816         _rtw_memcpy(pframe, &cap, 2);
7817 #else
7818         _rtw_memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
7819 #endif //CONFIG_DFS
7820
7821         pframe += 2;
7822         pattrib->pktlen += 2;
7823
7824         //listen interval
7825         //todo: listen interval for power saving
7826         val16 = cpu_to_le16(3);
7827         _rtw_memcpy(pframe ,(unsigned char *)&val16, 2);
7828         pframe += 2;
7829         pattrib->pktlen += 2;
7830
7831         //SSID
7832         pframe = rtw_set_ie(pframe, _SSID_IE_,  pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &(pattrib->pktlen));
7833
7834         //supported rate & extended supported rate
7835
7836 #if 1   // Check if the AP's supported rates are also supported by STA.
7837         get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);
7838         //DBG_871X("sta_bssrate_len=%d\n", sta_bssrate_len);
7839
7840         if(pmlmeext->cur_channel == 14)// for JAPAN, channel 14 can only uses B Mode(CCK)
7841         {
7842                 sta_bssrate_len = 4;
7843         }
7844
7845
7846         //for (i = 0; i < sta_bssrate_len; i++) {
7847         //      DBG_871X("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]);
7848         //}
7849
7850         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
7851                 if (pmlmeinfo->network.SupportedRates[i] == 0) break;
7852                 DBG_871X("network.SupportedRates[%d]=%02X\n", i, pmlmeinfo->network.SupportedRates[i]);
7853         }
7854
7855
7856         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
7857                 if (pmlmeinfo->network.SupportedRates[i] == 0) break;
7858
7859
7860                 // Check if the AP's supported rates are also supported by STA.
7861                 for (j=0; j < sta_bssrate_len; j++) {
7862                          // Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP
7863                         if ( (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK)
7864                                         == (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)) {
7865                                 //DBG_871X("match i = %d, j=%d\n", i, j);
7866                                 break;
7867                         } else {
7868                                 //DBG_871X("not match: %02X != %02X\n", (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK), (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK));
7869                         }
7870                 }
7871
7872                 if (j == sta_bssrate_len) {
7873                         // the rate is not supported by STA
7874                         DBG_871X("%s(): the rate[%d]=%02X is not supported by STA!\n",__FUNCTION__, i, pmlmeinfo->network.SupportedRates[i]);
7875                 } else {
7876                         // the rate is supported by STA
7877                         bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
7878                 }
7879         }
7880
7881         bssrate_len = index;
7882         DBG_871X("bssrate_len = %d\n", bssrate_len);
7883
7884 #else   // Check if the AP's supported rates are also supported by STA.
7885 #if 0
7886         get_rate_set(padapter, bssrate, &bssrate_len);
7887 #else
7888         for (bssrate_len = 0; bssrate_len < NumRates; bssrate_len++) {
7889                 if (pmlmeinfo->network.SupportedRates[bssrate_len] == 0) break;
7890
7891                 if (pmlmeinfo->network.SupportedRates[bssrate_len] == 0x2C) // Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP
7892                         break;
7893
7894                 bssrate[bssrate_len] = pmlmeinfo->network.SupportedRates[bssrate_len];
7895         }
7896 #endif
7897 #endif  // Check if the AP's supported rates are also supported by STA.
7898
7899         if (bssrate_len == 0) {
7900                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
7901                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
7902                 goto exit; //don't connect to AP if no joint supported rate
7903         }
7904
7905
7906         if (bssrate_len > 8)
7907         {
7908                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
7909                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
7910         }
7911         else
7912         {
7913                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
7914         }
7915
7916         //vendor specific IE, such as WPA, WMM, WPS
7917         for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;)
7918         {
7919                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i);
7920
7921                 switch (pIE->ElementID)
7922                 {
7923                         case _VENDOR_SPECIFIC_IE_:
7924                                 if ((_rtw_memcmp(pIE->data, RTW_WPA_OUI, 4)) ||
7925                                                 (_rtw_memcmp(pIE->data, WMM_OUI, 4)) ||
7926                                                 (_rtw_memcmp(pIE->data, WPS_OUI, 4)))
7927                                 {
7928                                         if(!padapter->registrypriv.wifi_spec)
7929                                         {
7930                                                 //Commented by Kurt 20110629
7931                                                 //In some older APs, WPS handshake
7932                                                 //would be fail if we append vender extensions informations to AP
7933                                                 if(_rtw_memcmp(pIE->data, WPS_OUI, 4)){
7934                                                         pIE->Length=14;
7935                                                 }
7936                                         }
7937                                         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, pIE->Length, pIE->data, &(pattrib->pktlen));
7938                                 }
7939                                 break;
7940
7941                         case EID_WPA2:
7942                                 pframe = rtw_set_ie(pframe, EID_WPA2, pIE->Length, pIE->data, &(pattrib->pktlen));
7943                                 break;
7944 #ifdef CONFIG_80211N_HT
7945                         case EID_HTCapability:
7946                                 if(padapter->mlmepriv.htpriv.ht_option==_TRUE) {
7947                                         if (!(is_ap_in_tkip(padapter)))
7948                                         {
7949                                                 _rtw_memcpy(&(pmlmeinfo->HT_caps), pIE->data, sizeof(struct HT_caps_element));
7950
7951                                                 pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = cpu_to_le16(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info);
7952
7953                                                 pframe = rtw_set_ie(pframe, EID_HTCapability, pIE->Length , (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
7954                                         }
7955                                 }
7956                                 break;
7957
7958                         case EID_EXTCapability:
7959                                 if(padapter->mlmepriv.htpriv.ht_option==_TRUE) {
7960                                         pframe = rtw_set_ie(pframe, EID_EXTCapability, pIE->Length, pIE->data, &(pattrib->pktlen));
7961                                 }
7962                                 break;
7963 #endif //CONFIG_80211N_HT
7964 #ifdef CONFIG_80211AC_VHT
7965                         case EID_VHTCapability:
7966                                 if (padapter->mlmepriv.vhtpriv.vht_option ==_TRUE) {
7967                                         pframe = rtw_set_ie(pframe, EID_VHTCapability, pIE->Length, pIE->data, &(pattrib->pktlen));
7968                                 }
7969                                 break;
7970
7971                         case EID_OpModeNotification:
7972                                 if (padapter->mlmepriv.vhtpriv.vht_option ==_TRUE) {
7973                                         pframe = rtw_set_ie(pframe, EID_OpModeNotification, pIE->Length, pIE->data, &(pattrib->pktlen));
7974                                 }
7975                                 break;
7976 #endif // CONFIG_80211AC_VHT
7977                         default:
7978                                 break;
7979                 }
7980
7981                 i += (pIE->Length + 2);
7982         }
7983
7984         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
7985         {
7986                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
7987         }
7988
7989
7990 #ifdef CONFIG_WAPI_SUPPORT
7991         rtw_build_assoc_req_wapi_ie(padapter, pframe, pattrib);
7992 #endif
7993
7994
7995 #ifdef CONFIG_P2P
7996
7997 #ifdef CONFIG_IOCTL_CFG80211
7998         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
7999         {
8000                 if(pmlmepriv->p2p_assoc_req_ie && pmlmepriv->p2p_assoc_req_ie_len>0)
8001                 {
8002                         _rtw_memcpy(pframe, pmlmepriv->p2p_assoc_req_ie, pmlmepriv->p2p_assoc_req_ie_len);
8003                         pframe += pmlmepriv->p2p_assoc_req_ie_len;
8004                         pattrib->pktlen += pmlmepriv->p2p_assoc_req_ie_len;
8005                 }
8006         }
8007         else
8008 #endif //CONFIG_IOCTL_CFG80211
8009         {
8010                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
8011                 {
8012                         //      Should add the P2P IE in the association request frame.
8013                         //      P2P OUI
8014
8015                         p2pielen = 0;
8016                         p2pie[ p2pielen++ ] = 0x50;
8017                         p2pie[ p2pielen++ ] = 0x6F;
8018                         p2pie[ p2pielen++ ] = 0x9A;
8019                         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
8020
8021                         //      Commented by Albert 20101109
8022                         //      According to the P2P Specification, the association request frame should contain 3 P2P attributes
8023                         //      1. P2P Capability
8024                         //      2. Extended Listen Timing
8025                         //      3. Device Info
8026                         //      Commented by Albert 20110516
8027                         //      4. P2P Interface
8028
8029                         //      P2P Capability
8030                         //      Type:
8031                         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
8032
8033                         //      Length:
8034                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
8035                         p2pielen += 2;
8036
8037                         //      Value:
8038                         //      Device Capability Bitmap, 1 byte
8039                         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
8040
8041                         //      Group Capability Bitmap, 1 byte
8042                         if ( pwdinfo->persistent_supported )
8043                                 p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
8044                         else
8045                                 p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
8046
8047                         //      Extended Listen Timing
8048                         //      Type:
8049                         p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
8050
8051                         //      Length:
8052                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
8053                         p2pielen += 2;
8054
8055                         //      Value:
8056                         //      Availability Period
8057                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
8058                         p2pielen += 2;
8059
8060                         //      Availability Interval
8061                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
8062                         p2pielen += 2;
8063
8064                         //      Device Info
8065                         //      Type:
8066                         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
8067
8068                         //      Length:
8069                         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
8070                         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
8071                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
8072                         p2pielen += 2;
8073
8074                         //      Value:
8075                         //      P2P Device Address
8076                         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
8077                         p2pielen += ETH_ALEN;
8078
8079                         //      Config Method
8080                         //      This field should be big endian. Noted by P2P specification.
8081                         if ( ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN ) ||
8082                                 ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN ) )
8083                         {
8084                                 *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
8085                         }
8086                         else
8087                         {
8088                                 *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_PBC );
8089                         }
8090
8091                         p2pielen += 2;
8092
8093                         //      Primary Device Type
8094                         //      Category ID
8095                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
8096                         p2pielen += 2;
8097
8098                         //      OUI
8099                         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
8100                         p2pielen += 4;
8101
8102                         //      Sub Category ID
8103                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
8104                         p2pielen += 2;
8105
8106                         //      Number of Secondary Device Types
8107                         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
8108
8109                         //      Device Name
8110                         //      Type:
8111                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
8112                         p2pielen += 2;
8113
8114                         //      Length:
8115                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
8116                         p2pielen += 2;
8117
8118                         //      Value:
8119                         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
8120                         p2pielen += pwdinfo->device_name_len;
8121
8122                         //      P2P Interface
8123                         //      Type:
8124                         p2pie[ p2pielen++ ] = P2P_ATTR_INTERFACE;
8125
8126                         //      Length:
8127                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x000D );
8128                         p2pielen += 2;
8129
8130                         //      Value:
8131                         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );        //      P2P Device Address
8132                         p2pielen += ETH_ALEN;
8133
8134                         p2pie[ p2pielen++ ] = 1;        //      P2P Interface Address Count
8135
8136                         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );        //      P2P Interface Address List
8137                         p2pielen += ETH_ALEN;
8138
8139                         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
8140
8141 #ifdef CONFIG_WFD
8142                         //wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);
8143                         //pframe += wfdielen;
8144                         //pattrib->pktlen += wfdielen;
8145 #endif //CONFIG_WFD
8146                 }
8147         }
8148
8149 #endif //CONFIG_P2P
8150
8151 #ifdef CONFIG_WFD
8152 #ifdef CONFIG_IOCTL_CFG80211
8153         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
8154 #endif //CONFIG_IOCTL_CFG80211
8155         {
8156                 wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);
8157                 pframe += wfdielen;
8158                 pattrib->pktlen += wfdielen;
8159         }
8160 #ifdef CONFIG_IOCTL_CFG80211
8161         else if (pmlmepriv->wfd_assoc_req_ie != NULL && pmlmepriv->wfd_assoc_req_ie_len>0)
8162         {
8163                 //WFD IE
8164                 _rtw_memcpy(pframe, pmlmepriv->wfd_assoc_req_ie, pmlmepriv->wfd_assoc_req_ie_len);
8165                 pattrib->pktlen += pmlmepriv->wfd_assoc_req_ie_len;
8166                 pframe += pmlmepriv->wfd_assoc_req_ie_len;
8167         }
8168 #endif //CONFIG_IOCTL_CFG80211
8169 #endif //CONFIG_WFD
8170
8171         pattrib->last_txcmdsz = pattrib->pktlen;
8172         dump_mgntframe(padapter, pmgntframe);
8173
8174         ret = _SUCCESS;
8175
8176 exit:
8177         if (ret == _SUCCESS)
8178                 rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen);
8179         else
8180                 rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
8181
8182         return;
8183 }
8184
8185 //when wait_ack is ture, this function shoule be called at process context
8186 static int _issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
8187 {
8188         int ret = _FAIL;
8189         struct xmit_frame                       *pmgntframe;
8190         struct pkt_attrib                       *pattrib;
8191         unsigned char                                   *pframe;
8192         struct rtw_ieee80211_hdr        *pwlanhdr;
8193         unsigned short                          *fctrl;
8194         struct xmit_priv        *pxmitpriv;
8195         struct mlme_ext_priv    *pmlmeext;
8196         struct mlme_ext_info    *pmlmeinfo;
8197
8198         //DBG_871X("%s:%d\n", __FUNCTION__, power_mode);
8199
8200         if(!padapter)
8201                 goto exit;
8202
8203         pxmitpriv = &(padapter->xmitpriv);
8204         pmlmeext = &(padapter->mlmeextpriv);
8205         pmlmeinfo = &(pmlmeext->mlmext_info);
8206
8207         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8208         {
8209                 goto exit;
8210         }
8211
8212         //update attribute
8213         pattrib = &pmgntframe->attrib;
8214         update_mgntframe_attrib(padapter, pattrib);
8215         pattrib->retry_ctrl = _FALSE;
8216
8217         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8218
8219         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8220         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8221
8222         fctrl = &(pwlanhdr->frame_ctl);
8223         *(fctrl) = 0;
8224
8225         if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
8226         {
8227                 SetFrDs(fctrl);
8228         }
8229         else if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
8230         {
8231                 SetToDs(fctrl);
8232         }
8233
8234         if (power_mode)
8235         {
8236                 SetPwrMgt(fctrl);
8237         }
8238
8239         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
8240         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8241         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8242
8243         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8244         pmlmeext->mgnt_seq++;
8245         SetFrameSubType(pframe, WIFI_DATA_NULL);
8246
8247         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8248         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8249
8250         pattrib->last_txcmdsz = pattrib->pktlen;
8251
8252         if(wait_ack)
8253         {
8254                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
8255         }
8256         else
8257         {
8258                 dump_mgntframe(padapter, pmgntframe);
8259                 ret = _SUCCESS;
8260         }
8261
8262 exit:
8263         return ret;
8264 }
8265
8266 /*
8267  * [IMPORTANT] Don't call this function in interrupt context
8268  *
8269  * When wait_ms > 0, this function shoule be called at process context
8270  * da == NULL for station mode
8271  */
8272 int issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
8273 {
8274         int ret;
8275         int i = 0;
8276         u32 start = rtw_get_current_time();
8277         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8278         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8279         struct sta_info *psta;
8280
8281
8282         /* da == NULL, assum it's null data for sta to ap*/
8283         if (da == NULL)
8284                 da = get_my_bssid(&(pmlmeinfo->network));
8285
8286         psta = rtw_get_stainfo(&padapter->stapriv, da);
8287         if (psta) {
8288                 if (power_mode)
8289                         rtw_hal_macid_sleep(padapter, psta->mac_id);
8290                 else
8291                         rtw_hal_macid_wakeup(padapter, psta->mac_id);
8292         } else {
8293                 DBG_871X(FUNC_ADPT_FMT ": Can't find sta info for " MAC_FMT ", skip macid %s!!\n",
8294                         FUNC_ADPT_ARG(padapter), MAC_ARG(da), power_mode?"sleep":"wakeup");
8295                 rtw_warn_on(1);
8296         }
8297
8298         do {
8299                 ret = _issue_nulldata(padapter, da, power_mode, wait_ms>0?_TRUE:_FALSE);
8300
8301                 i++;
8302
8303                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
8304                         break;
8305
8306                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
8307                         rtw_msleep_os(wait_ms);
8308
8309         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
8310
8311         if (ret != _FAIL) {
8312                 ret = _SUCCESS;
8313                 #ifndef DBG_XMIT_ACK
8314                 goto exit;
8315                 #endif
8316         }
8317
8318         if (try_cnt && wait_ms) {
8319                 if (da)
8320                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
8321                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
8322                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8323                 else
8324                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
8325                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
8326                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8327         }
8328 exit:
8329         return ret;
8330 }
8331
8332 /*
8333  * [IMPORTANT] This function run in interrupt context
8334  *
8335  * The null data packet would be sent without power bit,
8336  * and not guarantee success.
8337  */
8338 s32 issue_nulldata_in_interrupt(PADAPTER padapter, u8 *da)
8339 {
8340         int ret;
8341         struct mlme_ext_priv *pmlmeext;
8342         struct mlme_ext_info *pmlmeinfo;
8343
8344
8345         pmlmeext = &padapter->mlmeextpriv;
8346         pmlmeinfo = &pmlmeext->mlmext_info;
8347
8348         /* da == NULL, assum it's null data for sta to ap*/
8349         if (da == NULL)
8350                 da = get_my_bssid(&(pmlmeinfo->network));
8351
8352         ret = _issue_nulldata(padapter, da, 0, _FALSE);
8353
8354         return ret;
8355 }
8356
8357 //when wait_ack is ture, this function shoule be called at process context
8358 static int _issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int wait_ack)
8359 {
8360         int ret = _FAIL;
8361         struct xmit_frame                       *pmgntframe;
8362         struct pkt_attrib                       *pattrib;
8363         unsigned char                                   *pframe;
8364         struct rtw_ieee80211_hdr        *pwlanhdr;
8365         unsigned short                          *fctrl, *qc;
8366         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
8367         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8368         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8369
8370         DBG_871X("%s\n", __FUNCTION__);
8371
8372         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8373         {
8374                 goto exit;
8375         }
8376
8377         //update attribute
8378         pattrib = &pmgntframe->attrib;
8379         update_mgntframe_attrib(padapter, pattrib);
8380
8381         pattrib->hdrlen +=2;
8382         pattrib->qos_en = _TRUE;
8383         pattrib->eosp = 1;
8384         pattrib->ack_policy = 0;
8385         pattrib->mdata = 0;
8386
8387         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8388
8389         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8390         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8391
8392         fctrl = &(pwlanhdr->frame_ctl);
8393         *(fctrl) = 0;
8394
8395         if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
8396         {
8397                 SetFrDs(fctrl);
8398         }
8399         else if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
8400         {
8401                 SetToDs(fctrl);
8402         }
8403
8404         if(pattrib->mdata)
8405                 SetMData(fctrl);
8406
8407         qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
8408
8409         SetPriority(qc, tid);
8410
8411         SetEOSP(qc, pattrib->eosp);
8412
8413         SetAckpolicy(qc, pattrib->ack_policy);
8414
8415         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
8416         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8417         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8418
8419         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8420         pmlmeext->mgnt_seq++;
8421         SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
8422
8423         pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos);
8424         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos);
8425
8426         pattrib->last_txcmdsz = pattrib->pktlen;
8427
8428         if(wait_ack)
8429         {
8430                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
8431         }
8432         else
8433         {
8434                 dump_mgntframe(padapter, pmgntframe);
8435                 ret = _SUCCESS;
8436         }
8437
8438 exit:
8439         return ret;
8440 }
8441
8442 //when wait_ms >0 , this function shoule be called at process context
8443 //da == NULL for station mode
8444 int issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms)
8445 {
8446         int ret;
8447         int i = 0;
8448         u32 start = rtw_get_current_time();
8449         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8450         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8451
8452         /* da == NULL, assum it's null data for sta to ap*/
8453         if (da == NULL)
8454                 da = get_my_bssid(&(pmlmeinfo->network));
8455
8456         do
8457         {
8458                 ret = _issue_qos_nulldata(padapter, da, tid, wait_ms>0?_TRUE:_FALSE);
8459
8460                 i++;
8461
8462                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
8463                         break;
8464
8465                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
8466                         rtw_msleep_os(wait_ms);
8467
8468         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
8469
8470         if (ret != _FAIL) {
8471                 ret = _SUCCESS;
8472                 #ifndef DBG_XMIT_ACK
8473                 goto exit;
8474                 #endif
8475         }
8476
8477         if (try_cnt && wait_ms) {
8478                 if (da)
8479                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
8480                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
8481                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8482                 else
8483                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
8484                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
8485                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8486         }
8487 exit:
8488         return ret;
8489 }
8490
8491 static int _issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack)
8492 {
8493         struct xmit_frame                       *pmgntframe;
8494         struct pkt_attrib                       *pattrib;
8495         unsigned char                                   *pframe;
8496         struct rtw_ieee80211_hdr        *pwlanhdr;
8497         unsigned short                          *fctrl;
8498         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
8499         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8500         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8501         int ret = _FAIL;
8502 #ifdef CONFIG_P2P
8503         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
8504 #endif //CONFIG_P2P
8505
8506         //DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
8507
8508 #ifdef CONFIG_P2P
8509         if ( !( rtw_p2p_chk_state( pwdinfo, P2P_STATE_NONE ) ) && ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) )
8510         {
8511                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
8512                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
8513         }
8514 #endif //CONFIG_P2P
8515
8516         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8517         {
8518                 goto exit;
8519         }
8520
8521         //update attribute
8522         pattrib = &pmgntframe->attrib;
8523         update_mgntframe_attrib(padapter, pattrib);
8524         pattrib->retry_ctrl = _FALSE;
8525
8526         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8527
8528         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8529         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8530
8531         fctrl = &(pwlanhdr->frame_ctl);
8532         *(fctrl) = 0;
8533
8534         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
8535         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8536         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8537
8538         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8539         pmlmeext->mgnt_seq++;
8540         SetFrameSubType(pframe, WIFI_DEAUTH);
8541
8542         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8543         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8544
8545         reason = cpu_to_le16(reason);
8546         pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_ , (unsigned char *)&reason, &(pattrib->pktlen));
8547
8548         pattrib->last_txcmdsz = pattrib->pktlen;
8549
8550
8551         if(wait_ack)
8552         {
8553                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
8554         }
8555         else
8556         {
8557                 dump_mgntframe(padapter, pmgntframe);
8558                 ret = _SUCCESS;
8559         }
8560
8561 exit:
8562         return ret;
8563 }
8564
8565 int issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason)
8566 {
8567         DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
8568         return _issue_deauth(padapter, da, reason, _FALSE);
8569 }
8570
8571 int issue_deauth_ex(_adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
8572         int wait_ms)
8573 {
8574         int ret;
8575         int i = 0;
8576         u32 start = rtw_get_current_time();
8577
8578         do
8579         {
8580                 ret = _issue_deauth(padapter, da, reason, wait_ms>0?_TRUE:_FALSE);
8581
8582                 i++;
8583
8584                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
8585                         break;
8586
8587                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
8588                         rtw_msleep_os(wait_ms);
8589
8590         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
8591
8592         if (ret != _FAIL) {
8593                 ret = _SUCCESS;
8594                 #ifndef DBG_XMIT_ACK
8595                 goto exit;
8596                 #endif
8597         }
8598
8599         if (try_cnt && wait_ms) {
8600                 if (da)
8601                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
8602                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
8603                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8604                 else
8605                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
8606                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
8607                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8608         }
8609 exit:
8610         return ret;
8611 }
8612
8613 void issue_action_spct_ch_switch(_adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset)
8614 {
8615         _irqL   irqL;
8616         _list           *plist, *phead;
8617         struct xmit_frame                       *pmgntframe;
8618         struct pkt_attrib                       *pattrib;
8619         unsigned char                           *pframe;
8620         struct rtw_ieee80211_hdr        *pwlanhdr;
8621         unsigned short                  *fctrl;
8622         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
8623         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
8624         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8625         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8626
8627
8628         DBG_871X(FUNC_NDEV_FMT" ra="MAC_FMT", ch:%u, offset:%u\n",
8629                 FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(ra), new_ch, ch_offset);
8630
8631         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8632                 return;
8633
8634         //update attribute
8635         pattrib = &pmgntframe->attrib;
8636         update_mgntframe_attrib(padapter, pattrib);
8637
8638         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8639
8640         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8641         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8642
8643         fctrl = &(pwlanhdr->frame_ctl);
8644         *(fctrl) = 0;
8645
8646         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN); /* RA */
8647         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); /* TA */
8648         _rtw_memcpy(pwlanhdr->addr3, ra, ETH_ALEN); /* DA = RA */
8649
8650         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8651         pmlmeext->mgnt_seq++;
8652         SetFrameSubType(pframe, WIFI_ACTION);
8653
8654         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8655         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8656
8657         /* category, action */
8658         {
8659                 u8 category, action;
8660                 category = RTW_WLAN_CATEGORY_SPECTRUM_MGMT;
8661                 action = RTW_WLAN_ACTION_SPCT_CHL_SWITCH;
8662
8663                 pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
8664                 pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
8665         }
8666
8667         pframe = rtw_set_ie_ch_switch(pframe, &(pattrib->pktlen), 0, new_ch, 0);
8668         pframe = rtw_set_ie_secondary_ch_offset(pframe, &(pattrib->pktlen),
8669                 hal_ch_offset_to_secondary_ch_offset(ch_offset));
8670
8671         pattrib->last_txcmdsz = pattrib->pktlen;
8672
8673         dump_mgntframe(padapter, pmgntframe);
8674
8675 }
8676
8677 #ifdef CONFIG_IEEE80211W
8678 void issue_action_SA_Query(_adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short tid)
8679 {
8680         u8      category = RTW_WLAN_CATEGORY_SA_QUERY;
8681         u16     reason_code;
8682         struct xmit_frame               *pmgntframe;
8683         struct pkt_attrib               *pattrib;
8684         u8                                      *pframe;
8685         struct rtw_ieee80211_hdr        *pwlanhdr;
8686         u16                                     *fctrl;
8687         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
8688         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8689         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8690         struct sta_info         *psta;
8691         struct sta_priv         *pstapriv = &padapter->stapriv;
8692         struct registry_priv            *pregpriv = &padapter->registrypriv;
8693
8694
8695         DBG_871X("%s\n", __FUNCTION__);
8696
8697         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8698         {
8699                 DBG_871X("%s: alloc_mgtxmitframe fail\n", __FUNCTION__);
8700                 return;
8701         }
8702
8703         //update attribute
8704         pattrib = &pmgntframe->attrib;
8705         update_mgntframe_attrib(padapter, pattrib);
8706
8707         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8708
8709         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8710         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8711
8712         fctrl = &(pwlanhdr->frame_ctl);
8713         *(fctrl) = 0;
8714
8715         if(raddr)
8716                 _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
8717         else
8718                 _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8719         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8720         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8721
8722         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8723         pmlmeext->mgnt_seq++;
8724         SetFrameSubType(pframe, WIFI_ACTION);
8725
8726         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8727         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8728
8729         pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
8730         pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
8731
8732         switch (action)
8733         {
8734                 case 0: //SA Query req
8735                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&pmlmeext->sa_query_seq, &pattrib->pktlen);
8736                         pmlmeext->sa_query_seq++;
8737                         //send sa query request to AP, AP should reply sa query response in 1 second
8738                         set_sa_query_timer(pmlmeext, 1000);
8739                         break;
8740
8741                 case 1: //SA Query rsp
8742                         tid = cpu_to_le16(tid);
8743                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&tid, &pattrib->pktlen);
8744                         break;
8745                 default:
8746                         break;
8747         }
8748
8749         pattrib->last_txcmdsz = pattrib->pktlen;
8750
8751         dump_mgntframe(padapter, pmgntframe);
8752 }
8753 #endif //CONFIG_IEEE80211W
8754
8755 void issue_action_BA(_adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short status)
8756 {
8757         u8      category = RTW_WLAN_CATEGORY_BACK;
8758         u16     start_seq;
8759         u16     BA_para_set;
8760         u16     reason_code;
8761         u16     BA_timeout_value;
8762         u16     BA_starting_seqctrl;
8763         HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor;
8764         struct xmit_frame               *pmgntframe;
8765         struct pkt_attrib               *pattrib;
8766         u8                                      *pframe;
8767         struct rtw_ieee80211_hdr        *pwlanhdr;
8768         u16                                     *fctrl;
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         struct sta_info         *psta;
8773         struct sta_priv         *pstapriv = &padapter->stapriv;
8774         struct registry_priv            *pregpriv = &padapter->registrypriv;
8775
8776 #ifdef CONFIG_80211N_HT
8777         DBG_871X("%s, category=%d, action=%d, status=%d\n", __FUNCTION__, category, action, status);
8778
8779         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8780         {
8781                 return;
8782         }
8783
8784         //update attribute
8785         pattrib = &pmgntframe->attrib;
8786         update_mgntframe_attrib(padapter, pattrib);
8787
8788         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8789
8790         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8791         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8792
8793         fctrl = &(pwlanhdr->frame_ctl);
8794         *(fctrl) = 0;
8795
8796         //_rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8797         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
8798         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8799         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8800
8801         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8802         pmlmeext->mgnt_seq++;
8803         SetFrameSubType(pframe, WIFI_ACTION);
8804
8805         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8806         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8807
8808         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
8809         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
8810
8811       status = cpu_to_le16(status);
8812
8813
8814         if (category == 3)
8815         {
8816                 switch (action)
8817                 {
8818                         case 0: //ADDBA req
8819                                 do {
8820                                         pmlmeinfo->dialogToken++;
8821                                 } while (pmlmeinfo->dialogToken == 0);
8822                                 pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen));
8823
8824 #ifdef CONFIG_BT_COEXIST
8825                                 if (rtw_btcoex_IsBTCoexCtrlAMPDUSize(padapter) == _TRUE)
8826                                 {
8827                                         // A-MSDU NOT Supported
8828                                         BA_para_set = 0;
8829                                         // immediate Block Ack
8830                                         BA_para_set |= (1 << 1) & IEEE80211_ADDBA_PARAM_POLICY_MASK;
8831                                         // TID
8832                                         BA_para_set |= (status << 2) & IEEE80211_ADDBA_PARAM_TID_MASK;
8833                                         // max buffer size is 8 MSDU
8834                                         BA_para_set |= (8 << 6) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
8835                                 }
8836                                 else
8837 #endif
8838                                 {
8839                                         #if defined(CONFIG_RTL8188E) && defined(CONFIG_SDIO_HCI)
8840                                         BA_para_set = (0x0802 | ((status & 0xf) << 2)); //immediate ack & 16 buffer size
8841                                         #else
8842                                         BA_para_set = (0x1002 | ((status & 0xf) << 2)); //immediate ack & 64 buffer size
8843                                         #endif
8844                                 }
8845                                 //sys_mib.BA_para_set = 0x0802; //immediate ack & 32 buffer size
8846                                 BA_para_set = cpu_to_le16(BA_para_set);
8847                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
8848
8849                                 //BA_timeout_value = 0xffff;//max: 65535 TUs(~ 65 ms)
8850                                 BA_timeout_value = 5000;//~ 5ms
8851                                 BA_timeout_value = cpu_to_le16(BA_timeout_value);
8852                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_timeout_value)), &(pattrib->pktlen));
8853
8854                                 //if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
8855                                 if ((psta = rtw_get_stainfo(pstapriv, raddr)) != NULL)
8856                                 {
8857                                         start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07]&0xfff) + 1;
8858
8859                                         DBG_871X("BA_starting_seqctrl = %d for TID=%d\n", start_seq, status & 0x07);
8860
8861                                         psta->BA_starting_seqctrl[status & 0x07] = start_seq;
8862
8863                                         BA_starting_seqctrl = start_seq << 4;
8864                                 }
8865
8866                                 BA_starting_seqctrl = cpu_to_le16(BA_starting_seqctrl);
8867                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_starting_seqctrl)), &(pattrib->pktlen));
8868                                 break;
8869
8870                         case 1: //ADDBA rsp
8871                                 pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen));
8872                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&status), &(pattrib->pktlen));
8873                                 /*
8874                                 //BA_para_set = cpu_to_le16((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); //64 buffer size
8875                                 #if defined(CONFIG_RTL8188E )&& defined (CONFIG_SDIO_HCI)
8876                                 BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0800); //32buffer size
8877                                 #else
8878                                 BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); //64 buffer size
8879                                 #endif
8880                                 */
8881                                 rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
8882                                 if(MAX_AMPDU_FACTOR_64K == max_rx_ampdu_factor)
8883                                         BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); //64 buffer size
8884                                 else if(MAX_AMPDU_FACTOR_32K == max_rx_ampdu_factor)
8885                                         BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0800); //32 buffer size
8886                                 else if(MAX_AMPDU_FACTOR_16K == max_rx_ampdu_factor)
8887                                         BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0400); //16 buffer size
8888                                 else if(MAX_AMPDU_FACTOR_8K == max_rx_ampdu_factor)
8889                                         BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0200); //8 buffer size
8890                                 else
8891                                         BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); //64 buffer size
8892
8893 #ifdef CONFIG_BT_COEXIST
8894                                 if (rtw_btcoex_IsBTCoexCtrlAMPDUSize(padapter) == _TRUE)
8895                                 {
8896                                         // max buffer size is 8 MSDU
8897                                         BA_para_set &= ~RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
8898                                         BA_para_set |= (8 << 6) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
8899                                 }
8900 #endif
8901
8902                                 if(pregpriv->ampdu_amsdu==0)//disabled
8903                                         BA_para_set = cpu_to_le16(BA_para_set & ~BIT(0));
8904                                 else if(pregpriv->ampdu_amsdu==1)//enabled
8905                                         BA_para_set = cpu_to_le16(BA_para_set | BIT(0));
8906                                 else //auto
8907                                         BA_para_set = cpu_to_le16(BA_para_set);
8908
8909                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
8910                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen));
8911                                 break;
8912                         case 2://DELBA
8913                                 BA_para_set = (status & 0x1F) << 3;
8914                                 BA_para_set = cpu_to_le16(BA_para_set);
8915                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
8916
8917                                 reason_code = 37;//Requested from peer STA as it does not want to use the mechanism
8918                                 reason_code = cpu_to_le16(reason_code);
8919                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(reason_code)), &(pattrib->pktlen));
8920                                 break;
8921                         default:
8922                                 break;
8923                 }
8924         }
8925
8926         pattrib->last_txcmdsz = pattrib->pktlen;
8927
8928         dump_mgntframe(padapter, pmgntframe);
8929 #endif //CONFIG_80211N_HT
8930 }
8931
8932 static void issue_action_BSSCoexistPacket(_adapter *padapter)
8933 {
8934         _irqL   irqL;
8935         _list           *plist, *phead;
8936         unsigned char category, action;
8937         struct xmit_frame                       *pmgntframe;
8938         struct pkt_attrib                       *pattrib;
8939         unsigned char                           *pframe;
8940         struct rtw_ieee80211_hdr        *pwlanhdr;
8941         unsigned short                  *fctrl;
8942         struct  wlan_network    *pnetwork = NULL;
8943         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
8944         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
8945         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8946         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8947         _queue          *queue  = &(pmlmepriv->scanned_queue);
8948         u8 InfoContent[16] = {0};
8949         u8 ICS[8][15];
8950 #ifdef CONFIG_80211N_HT
8951         if((pmlmepriv->num_FortyMHzIntolerant==0) || (pmlmepriv->num_sta_no_ht==0))
8952                 return;
8953
8954         if(_TRUE == pmlmeinfo->bwmode_updated)
8955                 return;
8956
8957
8958         DBG_871X("%s\n", __FUNCTION__);
8959
8960
8961         category = RTW_WLAN_CATEGORY_PUBLIC;
8962         action = ACT_PUBLIC_BSSCOEXIST;
8963
8964         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8965         {
8966                 return;
8967         }
8968
8969         //update attribute
8970         pattrib = &pmgntframe->attrib;
8971         update_mgntframe_attrib(padapter, pattrib);
8972
8973         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8974
8975         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8976         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8977
8978         fctrl = &(pwlanhdr->frame_ctl);
8979         *(fctrl) = 0;
8980
8981         _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8982         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8983         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8984
8985         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8986         pmlmeext->mgnt_seq++;
8987         SetFrameSubType(pframe, WIFI_ACTION);
8988
8989         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8990         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8991
8992         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
8993         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
8994
8995
8996         //
8997         if(pmlmepriv->num_FortyMHzIntolerant>0)
8998         {
8999                 u8 iedata=0;
9000
9001                 iedata |= BIT(2);//20 MHz BSS Width Request
9002
9003                 pframe = rtw_set_ie(pframe, EID_BSSCoexistence,  1, &iedata, &(pattrib->pktlen));
9004
9005         }
9006
9007
9008         //
9009         _rtw_memset(ICS, 0, sizeof(ICS));
9010         if(pmlmepriv->num_sta_no_ht>0)
9011         {
9012                 int i;
9013
9014                 _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
9015
9016                 phead = get_list_head(queue);
9017                 plist = get_next(phead);
9018
9019                 while(1)
9020                 {
9021                         int len;
9022                         u8 *p;
9023                         WLAN_BSSID_EX *pbss_network;
9024
9025                         if (rtw_end_of_queue_search(phead,plist)== _TRUE)
9026                                 break;
9027
9028                         pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
9029
9030                         plist = get_next(plist);
9031
9032                         pbss_network = (WLAN_BSSID_EX *)&pnetwork->network;
9033
9034                         p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
9035                         if((p==NULL) || (len==0))//non-HT
9036                         {
9037                                 if((pbss_network->Configuration.DSConfig<=0) || (pbss_network->Configuration.DSConfig>14))
9038                                         continue;
9039
9040                                 ICS[0][pbss_network->Configuration.DSConfig]=1;
9041
9042                                 if(ICS[0][0] == 0)
9043                                         ICS[0][0] = 1;
9044                         }
9045
9046                 }
9047
9048                 _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
9049
9050
9051                 for(i= 0;i<8;i++)
9052                 {
9053                         if(ICS[i][0] == 1)
9054                         {
9055                                 int j, k = 0;
9056
9057                                 InfoContent[k] = i;
9058                                 //SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent,i);
9059                                 k++;
9060
9061                                 for(j=1;j<=14;j++)
9062                                 {
9063                                         if(ICS[i][j]==1)
9064                                         {
9065                                                 if(k<16)
9066                                                 {
9067                                                         InfoContent[k] = j; //channel number
9068                                                         //SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j);
9069                                                         k++;
9070                                                 }
9071                                         }
9072                                 }
9073
9074                                 pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &(pattrib->pktlen));
9075
9076                         }
9077
9078                 }
9079
9080
9081         }
9082
9083
9084         pattrib->last_txcmdsz = pattrib->pktlen;
9085
9086         dump_mgntframe(padapter, pmgntframe);
9087 #endif //CONFIG_80211N_HT
9088 }
9089
9090 unsigned int send_delba(_adapter *padapter, u8 initiator, u8 *addr)
9091 {
9092         struct sta_priv *pstapriv = &padapter->stapriv;
9093         struct sta_info *psta = NULL;
9094         //struct recv_reorder_ctrl *preorder_ctrl;
9095         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9096         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9097         u16 tid;
9098
9099         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
9100                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
9101                         return _SUCCESS;
9102
9103         psta = rtw_get_stainfo(pstapriv, addr);
9104         if(psta==NULL)
9105                 return _SUCCESS;
9106
9107         //DBG_871X("%s:%s\n", __FUNCTION__, (initiator==0)?"RX_DIR":"TX_DIR");
9108
9109         if(initiator==0) // recipient
9110         {
9111                 for(tid = 0;tid<MAXTID;tid++)
9112                 {
9113                         if(psta->recvreorder_ctrl[tid].enable == _TRUE)
9114                         {
9115                                 DBG_871X("rx agg disable tid(%d)\n",tid);
9116                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F));
9117                                 psta->recvreorder_ctrl[tid].enable = _FALSE;
9118                                 psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
9119                                 #ifdef DBG_RX_SEQ
9120                                 DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
9121                                         psta->recvreorder_ctrl[tid].indicate_seq);
9122                                 #endif
9123                         }
9124                 }
9125         }
9126         else if(initiator == 1)// originator
9127         {
9128 #ifdef CONFIG_80211N_HT
9129                 //DBG_871X("tx agg_enable_bitmap(0x%08x)\n", psta->htpriv.agg_enable_bitmap);
9130                 for(tid = 0;tid<MAXTID;tid++)
9131                 {
9132                         if(psta->htpriv.agg_enable_bitmap & BIT(tid))
9133                         {
9134                                 DBG_871X("tx agg disable tid(%d)\n",tid);
9135                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F) );
9136                                 psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
9137                                 psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
9138
9139                         }
9140                 }
9141 #endif //CONFIG_80211N_HT
9142         }
9143
9144         return _SUCCESS;
9145
9146 }
9147
9148 unsigned int send_beacon(_adapter *padapter)
9149 {
9150         u8      bxmitok = _FALSE;
9151         int     issue=0;
9152         int poll = 0;
9153 //#ifdef CONFIG_CONCURRENT_MODE
9154         //struct mlme_ext_priv  *pmlmeext = &(padapter->mlmeextpriv);
9155         //struct mlme_ext_info  *pmlmeinfo = &(pmlmeext->mlmext_info);
9156         //_adapter *pbuddy_adapter = padapter->pbuddy_adapter;
9157         //struct mlme_priv *pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
9158 //#endif
9159
9160 #ifdef CONFIG_PCI_HCI
9161
9162         //DBG_871X("%s\n", __FUNCTION__);
9163
9164         issue_beacon(padapter, 0);
9165
9166         return _SUCCESS;
9167
9168 #endif
9169
9170 #if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
9171         u32 start = rtw_get_current_time();
9172
9173         rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
9174         rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
9175         do{
9176                 issue_beacon(padapter, 100);
9177                 issue++;
9178                 do {
9179                         rtw_yield_os();
9180                         rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
9181                         poll++;
9182                 }while((poll%10)!=0 && _FALSE == bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
9183
9184         }while(_FALSE == bxmitok && issue<100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
9185
9186         if(padapter->bSurpriseRemoved || padapter->bDriverStopped)
9187         {
9188                 return _FAIL;
9189         }
9190
9191
9192         if(_FALSE == bxmitok)
9193         {
9194                 DBG_871X("%s fail! %u ms\n", __FUNCTION__, rtw_get_passing_time_ms(start));
9195                 return _FAIL;
9196         }
9197         else
9198         {
9199                 u32 passing_time = rtw_get_passing_time_ms(start);
9200
9201                 if(passing_time > 100 || issue > 3)
9202                         DBG_871X("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start));
9203                 //else
9204                 //      DBG_871X("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start));
9205
9206                 return _SUCCESS;
9207         }
9208
9209 #endif
9210
9211 }
9212
9213 /****************************************************************************
9214
9215 Following are some utitity fuctions for WiFi MLME
9216
9217 *****************************************************************************/
9218
9219 BOOLEAN IsLegal5GChannel(
9220         IN PADAPTER                     Adapter,
9221         IN u8                   channel)
9222 {
9223
9224         int i=0;
9225         u8 Channel_5G[45] = {36,38,40,42,44,46,48,50,52,54,56,58,
9226                 60,62,64,100,102,104,106,108,110,112,114,116,118,120,122,
9227                 124,126,128,130,132,134,136,138,140,149,151,153,155,157,159,
9228                 161,163,165};
9229         for(i=0;i<sizeof(Channel_5G);i++)
9230                 if(channel == Channel_5G[i])
9231                         return _TRUE;
9232         return _FALSE;
9233 }
9234
9235 void site_survey(_adapter *padapter)
9236 {
9237         unsigned char           survey_channel = 0, val8;
9238         RT_SCAN_TYPE    ScanType = SCAN_PASSIVE;
9239         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9240         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9241         u32 initialgain = 0;
9242
9243 #ifdef CONFIG_P2P
9244
9245 #ifdef CONFIG_CONCURRENT_MODE
9246
9247 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
9248         u8 stay_buddy_ch = 0;
9249 #endif
9250         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
9251         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
9252         struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
9253
9254 #endif //CONFIG_CONCURRENT_MODE
9255         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
9256         static unsigned char  prev_survey_channel = 0;
9257         static unsigned int p2p_scan_count = 0;
9258
9259         if ( ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) || ( pwdinfo->p2p_info.scan_op_ch_only ) )
9260         {
9261                 if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
9262                 {
9263                         survey_channel = pwdinfo->rx_invitereq_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
9264                 }
9265                 else
9266                 {
9267                         survey_channel = pwdinfo->p2p_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
9268                 }
9269                 ScanType = SCAN_ACTIVE;
9270         }
9271         else if(rtw_p2p_findphase_ex_is_social(pwdinfo))
9272         {
9273                 //      Commented by Albert 2011/06/03
9274                 //      The driver is in the find phase, it should go through the social channel.
9275                 int ch_set_idx;
9276                 survey_channel = pwdinfo->social_chan[pmlmeext->sitesurvey_res.channel_idx];
9277                 ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, survey_channel);
9278                 if (ch_set_idx >= 0)
9279                         ScanType = pmlmeext->channel_set[ch_set_idx].ScanType;
9280                 else
9281                         ScanType = SCAN_ACTIVE;
9282         }
9283         else
9284 #endif //CONFIG_P2P
9285         {
9286                 struct rtw_ieee80211_channel *ch;
9287                 if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
9288                         ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
9289                         survey_channel = ch->hw_value;
9290                         ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
9291                 }
9292         }
9293
9294         if (0){
9295 #ifdef CONFIG_P2P
9296                 DBG_871X(FUNC_ADPT_FMT" ch:%u (cnt:%u,idx:%d) at %dms, %c%c%c\n"
9297                 , FUNC_ADPT_ARG(padapter)
9298                 , survey_channel
9299                 , pwdinfo->find_phase_state_exchange_cnt, pmlmeext->sitesurvey_res.channel_idx
9300                 , rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time)
9301                 , ScanType?'A':'P', pmlmeext->sitesurvey_res.scan_mode?'A':'P'
9302                 , pmlmeext->sitesurvey_res.ssid[0].SsidLength?'S':' '
9303                 );
9304 #else
9305                 DBG_871X(FUNC_ADPT_FMT" ch:%u (cnt:%u) at %dms, %c%c%c\n"
9306                 , FUNC_ADPT_ARG(padapter)
9307                 , survey_channel
9308                 , pmlmeext->sitesurvey_res.channel_idx
9309                 , rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time)
9310                 , ScanType?'A':'P', pmlmeext->sitesurvey_res.scan_mode?'A':'P'
9311                 , pmlmeext->sitesurvey_res.ssid[0].SsidLength?'S':' '
9312                 );
9313 #endif // CONFIG_P2P
9314                 #ifdef DBG_FIXED_CHAN
9315                 DBG_871X(FUNC_ADPT_FMT" fixed_chan:%u\n", pmlmeext->fixed_chan);
9316                 #endif
9317         }
9318
9319         if(survey_channel != 0)
9320         {
9321                 //PAUSE 4-AC Queue when site_survey
9322                 //rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
9323                 //val8 |= 0x0f;
9324                 //rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
9325 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
9326                 if((padapter->pbuddy_adapter->mlmeextpriv.mlmext_info.state&0x03) == WIFI_FW_AP_STATE)
9327                 {
9328                         if( pmlmeinfo->scan_cnt == RTW_SCAN_NUM_OF_CH )
9329                         {
9330                                 pmlmeinfo->scan_cnt = 0;
9331                                 survey_channel = pbuddy_mlmeext->cur_channel;
9332                                 stay_buddy_ch = 1;
9333                         }
9334                         else
9335                         {
9336                                 if( pmlmeinfo->scan_cnt == 0 )
9337                                         stay_buddy_ch = 2;
9338                                 pmlmeinfo->scan_cnt++;
9339                         }
9340                 }
9341 #endif
9342                 if(pmlmeext->sitesurvey_res.channel_idx == 0)
9343                 {
9344 #ifdef DBG_FIXED_CHAN
9345                         if(pmlmeext->fixed_chan !=0xff)
9346                                 set_channel_bwmode(padapter, pmlmeext->fixed_chan, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
9347                         else
9348 #endif
9349                                 set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
9350                 }
9351                 else
9352                 {
9353 #ifdef DBG_FIXED_CHAN
9354                         if(pmlmeext->fixed_chan!=0xff)
9355                                 SelectChannel(padapter, pmlmeext->fixed_chan);
9356                         else
9357 #endif
9358                                 SelectChannel(padapter, survey_channel);
9359                 }
9360
9361 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
9362                 if( stay_buddy_ch == 1 )
9363                 {
9364                         val8 = 0; //survey done
9365                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
9366
9367                         if(check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) &&
9368                                 check_buddy_fwstate(padapter, _FW_LINKED))
9369                         {
9370                                 update_beacon(padapter->pbuddy_adapter, 0, NULL, _TRUE);
9371                         }
9372                 }
9373                 else if( stay_buddy_ch == 2 )
9374                 {
9375                         val8 = 1; //under site survey
9376                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
9377                 }
9378 #endif
9379
9380                 if(ScanType == SCAN_ACTIVE) //obey the channel plan setting...
9381                 {
9382                         #ifdef CONFIG_P2P
9383                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) ||
9384                                 rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)
9385                         )
9386                         {
9387                                 issue_probereq_p2p(padapter, NULL);
9388                                 issue_probereq_p2p(padapter, NULL);
9389                                 issue_probereq_p2p(padapter, NULL);
9390                         }
9391                         else
9392                         #endif //CONFIG_P2P
9393                         {
9394                                 int i;
9395                                 for(i=0;i<RTW_SSID_SCAN_AMOUNT;i++){
9396                                         if(pmlmeext->sitesurvey_res.ssid[i].SsidLength) {
9397                                                 //todo: to issue two probe req???
9398                                                 issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
9399                                                 //rtw_msleep_os(SURVEY_TO>>1);
9400                                                 issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
9401                                         }
9402                                 }
9403
9404                                 if(pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
9405                                         //todo: to issue two probe req???
9406                                         issue_probereq(padapter, NULL, NULL);
9407                                         //rtw_msleep_os(SURVEY_TO>>1);
9408                                         issue_probereq(padapter, NULL, NULL);
9409                                 }
9410                         }
9411                 }
9412 #if  defined(CONFIG_ATMEL_RC_PATCH)
9413                 // treat wlan0 & p2p0 in same way, may be changed in near feature.
9414                 // assume home channel is 6, channel switch sequence will be
9415                 //      1,2-6-3,4-6-5,6-6-7,8-6-9,10-6-11,12-6-13,14
9416                 //if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)==_TRUE)
9417
9418                 if( stay_buddy_ch == 1 )
9419                         set_survey_timer(pmlmeext, pmlmeext->chan_scan_time * RTW_STAY_AP_CH_MILLISECOND );
9420                 else {
9421                         if( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
9422                                 set_survey_timer(pmlmeext, 20);
9423                         else
9424                                 set_survey_timer(pmlmeext, 40);
9425                 }
9426 #elif defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE)
9427                 if( stay_buddy_ch == 1 )
9428                         set_survey_timer(pmlmeext, pmlmeext->chan_scan_time * RTW_STAY_AP_CH_MILLISECOND );
9429                 else
9430                         set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
9431 #else
9432                         set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
9433 #endif
9434         }
9435         else
9436         {
9437
9438                 //      channel number is 0 or this channel is not valid.
9439
9440 #ifdef CONFIG_CONCURRENT_MODE
9441                 u8 cur_channel;
9442                 u8 cur_bwmode;
9443                 u8 cur_ch_offset;
9444
9445                 if (rtw_get_ch_setting_union(padapter, &cur_channel, &cur_bwmode, &cur_ch_offset) != 0)
9446                 {
9447                         if (0)
9448                         DBG_871X(FUNC_ADPT_FMT" back to linked union - ch:%u, bw:%u, offset:%u\n",
9449                                 FUNC_ADPT_ARG(padapter), cur_channel, cur_bwmode, cur_ch_offset);
9450                 }
9451                 #ifdef CONFIG_IOCTL_CFG80211
9452                 else if(padapter->pbuddy_adapter
9453                         && pbuddy_adapter->wdinfo.driver_interface == DRIVER_CFG80211
9454                         && adapter_wdev_data(pbuddy_adapter)->p2p_enabled
9455                         && rtw_p2p_chk_state(&pbuddy_adapter->wdinfo, P2P_STATE_LISTEN)
9456                         )
9457                 {
9458                         cur_channel = pbuddy_adapter->wdinfo.listen_channel;
9459                         cur_bwmode = pbuddy_mlmeext->cur_bwmode;
9460                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
9461                 }
9462                 #endif
9463                 else
9464                 {
9465                         cur_channel = pmlmeext->cur_channel;
9466                         cur_bwmode = pmlmeext->cur_bwmode;
9467                         cur_ch_offset = pmlmeext->cur_ch_offset;
9468                 }
9469 #endif
9470
9471
9472 #ifdef CONFIG_P2P
9473                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
9474                 {
9475                         if( ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) || ( pwdinfo->p2p_info.scan_op_ch_only ) )
9476                         {
9477                                 //      Set the find_phase_state_exchange_cnt to P2P_FINDPHASE_EX_CNT.
9478                                 //      This will let the following flow to run the scanning end.
9479                                 rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
9480                         }
9481                         #ifdef CONFIG_DBG_P2P
9482                         DBG_871X( "[%s] find phase exchange cnt = %d\n", __FUNCTION__, pwdinfo->find_phase_state_exchange_cnt );
9483                         #endif
9484                 }
9485
9486                 if(rtw_p2p_findphase_ex_is_needed(pwdinfo))
9487                 {
9488                         //      Set the P2P State to the listen state of find phase and set the current channel to the listen channel
9489                         set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
9490                         rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN);
9491                         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
9492
9493                         initialgain = 0xff; //restore RX GAIN
9494                         rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
9495                         //turn on dynamic functions
9496                         Restore_DM_Func_Flag(padapter);
9497                         //Switch_DM_Func(padapter, DYNAMIC_FUNC_DIG|DYNAMIC_FUNC_HP|DYNAMIC_FUNC_SS, _TRUE);
9498
9499                         _set_timer( &pwdinfo->find_phase_timer, ( u32 ) ( ( u32 ) ( pwdinfo->listen_dwell ) * 100 ) );
9500                 }
9501                 else
9502 #endif //CONFIG_P2P
9503                 {
9504
9505 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
9506                         pmlmeinfo->scan_cnt = 0;
9507 #endif
9508
9509 #ifdef CONFIG_ANTENNA_DIVERSITY
9510                         // 20100721:Interrupt scan operation here.
9511                         // For SW antenna diversity before link, it needs to switch to another antenna and scan again.
9512                         // It compares the scan result and select beter one to do connection.
9513                         if(rtw_hal_antdiv_before_linked(padapter))
9514                         {
9515                                 pmlmeext->sitesurvey_res.bss_cnt = 0;
9516                                 pmlmeext->sitesurvey_res.channel_idx = -1;
9517                                 pmlmeext->chan_scan_time = SURVEY_TO /2;
9518                                 set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
9519                                 return;
9520                         }
9521 #endif
9522
9523 #ifdef CONFIG_P2P
9524                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
9525                         {
9526                         #ifdef CONFIG_CONCURRENT_MODE
9527                                 if( pwdinfo->driver_interface == DRIVER_WEXT )
9528                                 {
9529                                         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
9530                                         {
9531                                                 _set_timer( &pwdinfo->ap_p2p_switch_timer, 500 );
9532                                         }
9533                                 }
9534                                 rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
9535                         #else
9536                                 rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
9537                         #endif
9538                         }
9539                         rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
9540 #endif //CONFIG_P2P
9541
9542                         pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
9543
9544                         //switch back to the original channel
9545                         //SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset);
9546
9547                         {
9548 #ifdef CONFIG_CONCURRENT_MODE
9549                                 set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
9550 #else
9551 #ifdef CONFIG_DUALMAC_CONCURRENT
9552                                 dc_set_channel_bwmode_survey_done(padapter);
9553 #else
9554
9555 #ifdef CONFIG_P2P
9556                         if( (pwdinfo->driver_interface == DRIVER_WEXT) && (rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN)) )
9557                                 set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
9558                         else
9559 #endif //CONFIG_P2P
9560                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
9561
9562 #endif //CONFIG_DUALMAC_CONCURRENT
9563 #endif //CONFIG_CONCURRENT_MODE
9564                         }
9565
9566                         //flush 4-AC Queue after site_survey
9567                         //val8 = 0;
9568                         //rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
9569
9570                         //config MSR
9571                         Set_MSR(padapter, (pmlmeinfo->state & 0x3));
9572
9573                         initialgain = 0xff; //restore RX GAIN
9574                         rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
9575                         //turn on dynamic functions
9576                         Restore_DM_Func_Flag(padapter);
9577                         //Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE);
9578
9579                         if (is_client_associated_to_ap(padapter) == _TRUE)
9580                         {
9581                                 issue_nulldata(padapter, NULL, 0, 3, 500);
9582
9583 #ifdef CONFIG_CONCURRENT_MODE
9584                                 if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
9585                                 {
9586                                         DBG_871X("adapter is surveydone(buddy_adapter is linked), issue nulldata(pwrbit=0)\n");
9587
9588                                         issue_nulldata(padapter->pbuddy_adapter, NULL, 0, 3, 500);
9589                                 }
9590 #endif
9591                         }
9592 #ifdef CONFIG_CONCURRENT_MODE
9593                         else if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
9594                         {
9595                                 issue_nulldata(padapter->pbuddy_adapter, NULL, 0, 3, 500);
9596                         }
9597 #endif
9598
9599                         val8 = 0; //survey done
9600                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
9601
9602                         report_surveydone_event(padapter);
9603
9604                         pmlmeext->chan_scan_time = SURVEY_TO;
9605                         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
9606
9607                         issue_action_BSSCoexistPacket(padapter);
9608                         issue_action_BSSCoexistPacket(padapter);
9609                         issue_action_BSSCoexistPacket(padapter);
9610
9611                 }
9612
9613 #ifdef CONFIG_CONCURRENT_MODE
9614                 if(check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) &&
9615                         check_buddy_fwstate(padapter, _FW_LINKED))
9616                 {
9617
9618                         DBG_871X("survey done, current CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
9619
9620                         DBG_871X("restart pbuddy_adapter's beacon\n");
9621
9622                         update_beacon(padapter->pbuddy_adapter, 0, NULL, _TRUE);
9623                 }
9624 #endif
9625
9626         }
9627
9628         return;
9629
9630 }
9631
9632 //collect bss info from Beacon and Probe request/response frames.
9633 u8 collect_bss_info(_adapter *padapter, union recv_frame *precv_frame, WLAN_BSSID_EX *bssid)
9634 {
9635         int     i;
9636         u32     len;
9637         u8      *p;
9638         u16     val16, subtype;
9639         u8      *pframe = precv_frame->u.hdr.rx_data;
9640         u32     packet_len = precv_frame->u.hdr.len;
9641         u8 ie_offset;
9642         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
9643         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9644         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9645
9646         len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
9647
9648         if (len > MAX_IE_SZ)
9649         {
9650                 //DBG_871X("IE too long for survey event\n");
9651                 return _FAIL;
9652         }
9653
9654         _rtw_memset(bssid, 0, sizeof(WLAN_BSSID_EX));
9655
9656         subtype = GetFrameSubType(pframe);
9657
9658         if(subtype==WIFI_BEACON) {
9659                 bssid->Reserved[0] = 1;
9660                 ie_offset = _BEACON_IE_OFFSET_;
9661         } else {
9662                 // FIXME : more type
9663                 if (subtype == WIFI_PROBERSP) {
9664                         ie_offset = _PROBERSP_IE_OFFSET_;
9665                         bssid->Reserved[0] = 3;
9666                 }
9667                 else if (subtype == WIFI_PROBEREQ) {
9668                         ie_offset = _PROBEREQ_IE_OFFSET_;
9669                         bssid->Reserved[0] = 2;
9670                 }
9671                 else {
9672                         bssid->Reserved[0] = 0;
9673                         ie_offset = _FIXED_IE_LENGTH_;
9674                 }
9675         }
9676
9677         bssid->Length = sizeof(WLAN_BSSID_EX) - MAX_IE_SZ + len;
9678
9679         //below is to copy the information element
9680         bssid->IELength = len;
9681         _rtw_memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength);
9682
9683         //get the signal strength
9684         //bssid->Rssi = precv_frame->u.hdr.attrib.SignalStrength; // 0-100 index.
9685         bssid->Rssi = precv_frame->u.hdr.attrib.phy_info.RecvSignalPower; // in dBM.raw data
9686         bssid->PhyInfo.SignalQuality = precv_frame->u.hdr.attrib.phy_info.SignalQuality;//in percentage
9687         bssid->PhyInfo.SignalStrength = precv_frame->u.hdr.attrib.phy_info.SignalStrength;//in percentage
9688 #ifdef CONFIG_ANTENNA_DIVERSITY
9689         //rtw_hal_get_hwreg(padapter, HW_VAR_CURRENT_ANTENNA, (u8 *)(&bssid->PhyInfo.Optimum_antenna));
9690         rtw_hal_get_def_var(padapter, HAL_DEF_CURRENT_ANTENNA,  &bssid->PhyInfo.Optimum_antenna);
9691 #endif
9692
9693         // checking SSID
9694         if ((p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset)) == NULL)
9695         {
9696                 DBG_871X("marc: cannot find SSID for survey event\n");
9697                 return _FAIL;
9698         }
9699
9700         if (*(p + 1))
9701         {
9702                 if (len > NDIS_802_11_LENGTH_SSID)
9703                 {
9704                         DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
9705                         return _FAIL;
9706                 }
9707                 _rtw_memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
9708                 bssid->Ssid.SsidLength = *(p + 1);
9709         }
9710         else
9711         {
9712                 bssid->Ssid.SsidLength = 0;
9713         }
9714
9715         _rtw_memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
9716
9717         //checking rate info...
9718         i = 0;
9719         p = rtw_get_ie(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
9720         if (p != NULL)
9721         {
9722                 if (len > NDIS_802_11_LENGTH_RATES_EX)
9723                 {
9724                         DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
9725                         return _FAIL;
9726                 }
9727                 _rtw_memcpy(bssid->SupportedRates, (p + 2), len);
9728                 i = len;
9729         }
9730
9731         p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
9732         if (p != NULL)
9733         {
9734                 if (len > (NDIS_802_11_LENGTH_RATES_EX-i))
9735                 {
9736                         DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
9737                         return _FAIL;
9738                 }
9739                 _rtw_memcpy(bssid->SupportedRates + i, (p + 2), len);
9740         }
9741
9742         //todo:
9743 #if 0
9744         if (judge_network_type(bssid->SupportedRates, (len + i)) == WIRELESS_11B)
9745         {
9746                 bssid->NetworkTypeInUse = Ndis802_11DS;
9747         }
9748         else
9749 #endif
9750         {
9751                 bssid->NetworkTypeInUse = Ndis802_11OFDM24;
9752         }
9753
9754 #ifdef CONFIG_P2P
9755         if (subtype == WIFI_PROBEREQ)
9756         {
9757                 u8 *p2p_ie;
9758                 u32     p2p_ielen;
9759                 // Set Listion Channel
9760                 if ((p2p_ie = rtw_get_p2p_ie(bssid->IEs, bssid->IELength, NULL, &p2p_ielen)))
9761                 {
9762                         u32     attr_contentlen = 0;
9763                         u8 listen_ch[5] = { 0x00 };
9764
9765                         if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_LISTEN_CH, listen_ch, &attr_contentlen) != NULL)
9766                                 bssid->Configuration.DSConfig = listen_ch[4];
9767                         else
9768                                 return _FALSE; // Intel device maybe no bring Listen Channel
9769                 } else
9770                 { // use current channel
9771                         bssid->Configuration.DSConfig = padapter->mlmeextpriv.cur_channel;
9772                         DBG_871X("%s()-%d: Cannot get p2p_ie. set DSconfig to op_ch(%d)\n", __FUNCTION__, __LINE__, bssid->Configuration.DSConfig);
9773                 }
9774
9775                 // FIXME
9776                 bssid->InfrastructureMode = Ndis802_11Infrastructure;
9777                 _rtw_memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
9778                 bssid->Privacy = 1;
9779                 return _SUCCESS;
9780         }
9781 #endif //CONFIG_P2P
9782
9783         if (bssid->IELength < 12)
9784                 return _FAIL;
9785
9786         // Checking for DSConfig
9787         p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
9788
9789         bssid->Configuration.DSConfig = 0;
9790         bssid->Configuration.Length = 0;
9791
9792         if (p)
9793         {
9794                 bssid->Configuration.DSConfig = *(p + 2);
9795         }
9796         else
9797         {// In 5G, some ap do not have DSSET IE
9798                 // checking HT info for channel
9799                 p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
9800                 if(p)
9801                 {
9802                         struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
9803                         bssid->Configuration.DSConfig = HT_info->primary_channel;
9804                 }
9805                 else
9806                 { // use current channel
9807                         bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter);
9808                 }
9809         }
9810
9811         _rtw_memcpy(&bssid->Configuration.BeaconPeriod, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
9812         bssid->Configuration.BeaconPeriod = le32_to_cpu(bssid->Configuration.BeaconPeriod);
9813
9814         val16 = rtw_get_capability((WLAN_BSSID_EX *)bssid);
9815
9816         if (val16 & BIT(0))
9817         {
9818                 bssid->InfrastructureMode = Ndis802_11Infrastructure;
9819                 _rtw_memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
9820         }
9821         else
9822         {
9823                 bssid->InfrastructureMode = Ndis802_11IBSS;
9824                 _rtw_memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN);
9825         }
9826
9827         if (val16 & BIT(4))
9828                 bssid->Privacy = 1;
9829         else
9830                 bssid->Privacy = 0;
9831
9832         bssid->Configuration.ATIMWindow = 0;
9833
9834         //20/40 BSS Coexistence check
9835         if((pregistrypriv->wifi_spec==1) && (_FALSE == pmlmeinfo->bwmode_updated))
9836         {
9837                 struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
9838 #ifdef CONFIG_80211N_HT
9839                 p = rtw_get_ie(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset);
9840                 if(p && len>0)
9841                 {
9842                         struct HT_caps_element  *pHT_caps;
9843                         pHT_caps = (struct HT_caps_element      *)(p + 2);
9844
9845                         if(pHT_caps->u.HT_cap_element.HT_caps_info&BIT(14))
9846                         {
9847                                 pmlmepriv->num_FortyMHzIntolerant++;
9848                         }
9849                 }
9850                 else
9851                 {
9852                         pmlmepriv->num_sta_no_ht++;
9853                 }
9854 #endif //CONFIG_80211N_HT
9855
9856         }
9857
9858 #ifdef CONFIG_INTEL_WIDI
9859         //process_intel_widi_query_or_tigger(padapter, bssid);
9860         if(process_intel_widi_query_or_tigger(padapter, bssid))
9861         {
9862                 return _FAIL;
9863         }
9864 #endif // CONFIG_INTEL_WIDI
9865
9866         #if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) & 1
9867         if(strcmp(bssid->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
9868                 DBG_871X("Receiving %s("MAC_FMT", DSConfig:%u) from ch%u with ss:%3u, sq:%3u, RawRSSI:%3ld\n"
9869                         , bssid->Ssid.Ssid, MAC_ARG(bssid->MacAddress), bssid->Configuration.DSConfig
9870                         , rtw_get_oper_ch(padapter)
9871                         , bssid->PhyInfo.SignalStrength, bssid->PhyInfo.SignalQuality, bssid->Rssi
9872                 );
9873         }
9874         #endif
9875
9876         // mark bss info receving from nearby channel as SignalQuality 101
9877         if(bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
9878         {
9879                 bssid->PhyInfo.SignalQuality= 101;
9880         }
9881
9882         return _SUCCESS;
9883 }
9884
9885 void start_create_ibss(_adapter* padapter)
9886 {
9887         unsigned short  caps;
9888         u8      val8;
9889         u8      join_type;
9890         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9891         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9892         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
9893         pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
9894         pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
9895
9896         //update wireless mode
9897         update_wireless_mode(padapter);
9898
9899         //udpate capability
9900         caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
9901         update_capinfo(padapter, caps);
9902         if(caps&cap_IBSS)//adhoc master
9903         {
9904                 //set_opmode_cmd(padapter, adhoc);//removed
9905
9906                 val8 = 0xcf;
9907                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
9908
9909                 rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
9910
9911                 //switch channel
9912                 //SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE);
9913                 set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
9914
9915                 beacon_timing_control(padapter);
9916
9917                 //set msr to WIFI_FW_ADHOC_STATE
9918                 pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
9919                 Set_MSR(padapter, (pmlmeinfo->state & 0x3));
9920
9921                 //issue beacon
9922                 if(send_beacon(padapter)==_FAIL)
9923                 {
9924                         RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("issuing beacon frame fail....\n"));
9925
9926                         report_join_res(padapter, -1);
9927                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
9928                 }
9929                 else
9930                 {
9931                         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
9932                         join_type = 0;
9933                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
9934
9935                         report_join_res(padapter, 1);
9936                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
9937                         rtw_indicate_connect(padapter);
9938                 }
9939         }
9940         else
9941         {
9942                 DBG_871X("start_create_ibss, invalid cap:%x\n", caps);
9943                 return;
9944         }
9945         //update bc/mc sta_info
9946         update_bmc_sta(padapter);
9947
9948 }
9949
9950 void start_clnt_join(_adapter* padapter)
9951 {
9952         unsigned short  caps;
9953         u8      val8;
9954         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9955         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9956         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
9957         int beacon_timeout;
9958
9959         //update wireless mode
9960         update_wireless_mode(padapter);
9961
9962         //udpate capability
9963         caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
9964         update_capinfo(padapter, caps);
9965         if (caps&cap_ESS)
9966         {
9967                 Set_MSR(padapter, WIFI_FW_STATION_STATE);
9968
9969                 val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf;
9970
9971 #ifdef CONFIG_WAPI_SUPPORT
9972                 if (padapter->wapiInfo.bWapiEnable && pmlmeinfo->auth_algo == dot11AuthAlgrthm_WAPI)
9973                 {
9974                         //Disable TxUseDefaultKey, RxUseDefaultKey, RxBroadcastUseDefaultKey.
9975                         val8 = 0x4c;
9976                 }
9977 #endif
9978                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
9979
9980                 #ifdef CONFIG_DEAUTH_BEFORE_CONNECT
9981                 // Because of AP's not receiving deauth before
9982                 // AP may: 1)not response auth or 2)deauth us after link is complete
9983                 // issue deauth before issuing auth to deal with the situation
9984
9985                 //      Commented by Albert 2012/07/21
9986                 //      For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it.
9987                 {
9988                         #ifdef CONFIG_P2P
9989                         _queue *queue = &(padapter->mlmepriv.scanned_queue);
9990                         _list   *head = get_list_head(queue);
9991                         _list *pos = get_next(head);
9992                         struct wlan_network *scanned = NULL;
9993                         u8 ie_offset = 0;
9994                         _irqL irqL;
9995                         bool has_p2p_ie = _FALSE;
9996
9997                         _enter_critical_bh(&(padapter->mlmepriv.scanned_queue.lock), &irqL);
9998
9999                         for (pos = get_next(head);!rtw_end_of_queue_search(head, pos); pos = get_next(pos)) {
10000
10001                                 scanned = LIST_CONTAINOR(pos, struct wlan_network, list);
10002                                 if(scanned==NULL)
10003                                         rtw_warn_on(1);
10004
10005                                 if (_rtw_memcmp(&(scanned->network.Ssid), &(pnetwork->Ssid), sizeof(NDIS_802_11_SSID)) == _TRUE
10006                                         && _rtw_memcmp(scanned->network.MacAddress, pnetwork->MacAddress, sizeof(NDIS_802_11_MAC_ADDRESS)) == _TRUE
10007                                 ) {
10008                                         ie_offset = (scanned->network.Reserved[0] == 2? 0:12);
10009                                         if (rtw_get_p2p_ie(scanned->network.IEs+ie_offset, scanned->network.IELength-ie_offset, NULL, NULL))
10010                                                 has_p2p_ie = _TRUE;
10011                                         break;
10012                                 }
10013                         }
10014
10015                         _exit_critical_bh(&(padapter->mlmepriv.scanned_queue.lock), &irqL);
10016
10017                         if (scanned == NULL || rtw_end_of_queue_search(head, pos) || has_p2p_ie == _FALSE)
10018                         #endif /* CONFIG_P2P */
10019                                 //To avoid connecting to AP fail during resume process, change retry count from 5 to 1
10020                                 issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
10021                 }
10022                 #endif /* CONFIG_DEAUTH_BEFORE_CONNECT */
10023
10024                 //here wait for receiving the beacon to start auth
10025                 //and enable a timer
10026                 beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
10027                 set_link_timer(pmlmeext, beacon_timeout);
10028                 _set_timer( &padapter->mlmepriv.assoc_timer,
10029                         (REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) +beacon_timeout);
10030
10031                 pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
10032         }
10033         else if (caps&cap_IBSS) //adhoc client
10034         {
10035                 Set_MSR(padapter, WIFI_FW_ADHOC_STATE);
10036
10037                 val8 = 0xcf;
10038                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
10039
10040                 beacon_timing_control(padapter);
10041
10042                 pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
10043
10044                 report_join_res(padapter, 1);
10045         }
10046         else
10047         {
10048                 //DBG_871X("marc: invalid cap:%x\n", caps);
10049                 return;
10050         }
10051
10052 }
10053
10054 void start_clnt_auth(_adapter* padapter)
10055 {
10056         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10057         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10058
10059         _cancel_timer_ex(&pmlmeext->link_timer);
10060
10061         pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
10062         pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
10063
10064         pmlmeinfo->auth_seq = 1;
10065         pmlmeinfo->reauth_count = 0;
10066         pmlmeinfo->reassoc_count = 0;
10067         pmlmeinfo->link_count = 0;
10068         pmlmeext->retry = 0;
10069
10070
10071         DBG_871X_LEVEL(_drv_always_, "start auth\n");
10072         issue_auth(padapter, NULL, 0);
10073
10074         set_link_timer(pmlmeext, REAUTH_TO);
10075
10076 }
10077
10078
10079 void start_clnt_assoc(_adapter* padapter)
10080 {
10081         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10082         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10083
10084         _cancel_timer_ex(&pmlmeext->link_timer);
10085
10086         pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
10087         pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
10088
10089         issue_assocreq(padapter);
10090
10091         set_link_timer(pmlmeext, REASSOC_TO);
10092 }
10093
10094 unsigned int receive_disconnect(_adapter *padapter, unsigned char *MacAddr, unsigned short reason)
10095 {
10096         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10097         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10098
10099         //check A3
10100         if (!(_rtw_memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
10101                 return _SUCCESS;
10102
10103         DBG_871X("%s\n", __FUNCTION__);
10104
10105         if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
10106         {
10107                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
10108                 {
10109                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
10110                         report_del_sta_event(padapter, MacAddr, reason);
10111
10112                 }
10113                 else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE)
10114                 {
10115                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
10116                         report_join_res(padapter, -2);
10117                 }
10118         }
10119
10120         return _SUCCESS;
10121 }
10122
10123 #ifdef CONFIG_80211D
10124 static void process_80211d(PADAPTER padapter, WLAN_BSSID_EX *bssid)
10125 {
10126         struct registry_priv *pregistrypriv;
10127         struct mlme_ext_priv *pmlmeext;
10128         RT_CHANNEL_INFO *chplan_new;
10129         u8 channel;
10130         u8 i;
10131
10132
10133         pregistrypriv = &padapter->registrypriv;
10134         pmlmeext = &padapter->mlmeextpriv;
10135
10136         // Adjust channel plan by AP Country IE
10137         if (pregistrypriv->enable80211d &&
10138                 (!pmlmeext->update_channel_plan_by_ap_done))
10139         {
10140                 u8 *ie, *p;
10141                 u32 len;
10142                 RT_CHANNEL_PLAN chplan_ap;
10143                 RT_CHANNEL_INFO chplan_sta[MAX_CHANNEL_NUM];
10144                 u8 country[4];
10145                 u8 fcn; // first channel number
10146                 u8 noc; // number of channel
10147                 u8 j, k;
10148
10149                 ie = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
10150                 if (!ie) return;
10151                 if (len < 6) return;
10152
10153                 ie += 2;
10154                 p = ie;
10155                 ie += len;
10156
10157                 _rtw_memset(country, 0, 4);
10158                 _rtw_memcpy(country, p, 3);
10159                 p += 3;
10160                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
10161                                 ("%s: 802.11d country=%s\n", __FUNCTION__, country));
10162
10163                 i = 0;
10164                 while ((ie - p) >= 3)
10165                 {
10166                         fcn = *(p++);
10167                         noc = *(p++);
10168                         p++;
10169
10170                         for (j = 0; j < noc; j++)
10171                         {
10172                                 if (fcn <= 14) channel = fcn + j; // 2.4 GHz
10173                                 else channel = fcn + j*4; // 5 GHz
10174
10175                                 chplan_ap.Channel[i++] = channel;
10176                         }
10177                 }
10178                 chplan_ap.Len = i;
10179
10180 #ifdef CONFIG_DEBUG_RTL871X
10181                 i = 0;
10182                 DBG_871X("%s: AP[%s] channel plan {", __FUNCTION__, bssid->Ssid.Ssid);
10183                 while ((i < chplan_ap.Len) && (chplan_ap.Channel[i] != 0))
10184                 {
10185                         DBG_8192C("%02d,", chplan_ap.Channel[i]);
10186                         i++;
10187                 }
10188                 DBG_871X("}\n");
10189 #endif
10190
10191                 _rtw_memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
10192 #ifdef CONFIG_DEBUG_RTL871X
10193                 i = 0;
10194                 DBG_871X("%s: STA channel plan {", __FUNCTION__);
10195                 while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
10196                 {
10197                         DBG_871X("%02d(%c),", chplan_sta[i].ChannelNum, chplan_sta[i].ScanType==SCAN_PASSIVE?'p':'a');
10198                         i++;
10199                 }
10200                 DBG_871X("}\n");
10201 #endif
10202
10203                 _rtw_memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
10204                 chplan_new = pmlmeext->channel_set;
10205
10206                 i = j = k = 0;
10207                 if (pregistrypriv->wireless_mode & WIRELESS_11G)
10208                 {
10209                         do {
10210                                 if ((i == MAX_CHANNEL_NUM) ||
10211                                         (chplan_sta[i].ChannelNum == 0) ||
10212                                         (chplan_sta[i].ChannelNum > 14))
10213                                         break;
10214
10215                                 if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
10216                                         break;
10217
10218                                 if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j])
10219                                 {
10220                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10221                                         chplan_new[k].ScanType = SCAN_ACTIVE;
10222                                         i++;
10223                                         j++;
10224                                         k++;
10225                                 }
10226                                 else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j])
10227                                 {
10228                                         chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10229 //                                      chplan_new[k].ScanType = chplan_sta[i].ScanType;
10230                                         chplan_new[k].ScanType = SCAN_PASSIVE;
10231                                         i++;
10232                                         k++;
10233                                 }
10234                                 else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j])
10235                                 {
10236                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10237                                         chplan_new[k].ScanType = SCAN_ACTIVE;
10238                                         j++;
10239                                         k++;
10240                                 }
10241                         } while (1);
10242
10243                         // change AP not support channel to Passive scan
10244                         while ((i < MAX_CHANNEL_NUM) &&
10245                                 (chplan_sta[i].ChannelNum != 0) &&
10246                                 (chplan_sta[i].ChannelNum <= 14))
10247                         {
10248                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10249 //                              chplan_new[k].ScanType = chplan_sta[i].ScanType;
10250                                 chplan_new[k].ScanType = SCAN_PASSIVE;
10251                                 i++;
10252                                 k++;
10253                         }
10254
10255                         // add channel AP supported
10256                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
10257                         {
10258                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10259                                 chplan_new[k].ScanType = SCAN_ACTIVE;
10260                                 j++;
10261                                 k++;
10262                         }
10263                 }
10264                 else
10265                 {
10266                         // keep original STA 2.4G channel plan
10267                         while ((i < MAX_CHANNEL_NUM) &&
10268                                 (chplan_sta[i].ChannelNum != 0) &&
10269                                 (chplan_sta[i].ChannelNum <= 14))
10270                         {
10271                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10272                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
10273                                 i++;
10274                                 k++;
10275                         }
10276
10277                         // skip AP 2.4G channel plan
10278                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
10279                         {
10280                                 j++;
10281                         }
10282                 }
10283
10284                 if (pregistrypriv->wireless_mode & WIRELESS_11A)
10285                 {
10286                         do {
10287                                 if ((i == MAX_CHANNEL_NUM) ||
10288                                         (chplan_sta[i].ChannelNum == 0))
10289                                         break;
10290
10291                                 if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0))
10292                                         break;
10293
10294                                 if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j])
10295                                 {
10296                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10297                                         chplan_new[k].ScanType = SCAN_ACTIVE;
10298                                         i++;
10299                                         j++;
10300                                         k++;
10301                                 }
10302                                 else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j])
10303                                 {
10304                                         chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10305 //                                      chplan_new[k].ScanType = chplan_sta[i].ScanType;
10306                                         chplan_new[k].ScanType = SCAN_PASSIVE;
10307                                         i++;
10308                                         k++;
10309                                 }
10310                                 else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j])
10311                                 {
10312                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10313                                         chplan_new[k].ScanType = SCAN_ACTIVE;
10314                                         j++;
10315                                         k++;
10316                                 }
10317                         } while (1);
10318
10319                         // change AP not support channel to Passive scan
10320                         while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
10321                         {
10322                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10323 //                              chplan_new[k].ScanType = chplan_sta[i].ScanType;
10324                                 chplan_new[k].ScanType = SCAN_PASSIVE;
10325                                 i++;
10326                                 k++;
10327                         }
10328
10329                         // add channel AP supported
10330                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] != 0))
10331                         {
10332                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10333                                 chplan_new[k].ScanType = SCAN_ACTIVE;
10334                                 j++;
10335                                 k++;
10336                         }
10337                 }
10338                 else
10339                 {
10340                         // keep original STA 5G channel plan
10341                         while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
10342                         {
10343                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10344                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
10345                                 i++;
10346                                 k++;
10347                         }
10348                 }
10349
10350                 pmlmeext->update_channel_plan_by_ap_done = 1;
10351
10352 #ifdef CONFIG_DEBUG_RTL871X
10353                 k = 0;
10354                 DBG_871X("%s: new STA channel plan {", __FUNCTION__);
10355                 while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0))
10356                 {
10357                         DBG_871X("%02d(%c),", chplan_new[k].ChannelNum, chplan_new[k].ScanType==SCAN_PASSIVE?'p':'c');
10358                         k++;
10359                 }
10360                 DBG_871X("}\n");
10361 #endif
10362
10363 #if 0
10364                 // recover the right channel index
10365                 channel = chplan_sta[pmlmeext->sitesurvey_res.channel_idx].ChannelNum;
10366                 k = 0;
10367                 while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0))
10368                 {
10369                         if (chplan_new[k].ChannelNum == channel) {
10370                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
10371                                                  ("%s: change mlme_ext sitesurvey channel index from %d to %d\n",
10372                                                   __FUNCTION__, pmlmeext->sitesurvey_res.channel_idx, k));
10373                                 pmlmeext->sitesurvey_res.channel_idx = k;
10374                                 break;
10375                         }
10376                         k++;
10377                 }
10378 #endif
10379         }
10380
10381         // If channel is used by AP, set channel scan type to active
10382         channel = bssid->Configuration.DSConfig;
10383         chplan_new = pmlmeext->channel_set;
10384         i = 0;
10385         while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0))
10386         {
10387                 if (chplan_new[i].ChannelNum == channel)
10388                 {
10389                         if (chplan_new[i].ScanType == SCAN_PASSIVE)
10390                         {
10391                                 //5G Bnad 2, 3 (DFS) doesn't change to active scan
10392                                 if(channel >= 52 && channel <= 144)
10393                                         break;
10394
10395                                 chplan_new[i].ScanType = SCAN_ACTIVE;
10396                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
10397                                                  ("%s: change channel %d scan type from passive to active\n",
10398                                                   __FUNCTION__, channel));
10399                         }
10400                         break;
10401                 }
10402                 i++;
10403         }
10404 }
10405 #endif
10406
10407 /****************************************************************************
10408
10409 Following are the functions to report events
10410
10411 *****************************************************************************/
10412
10413 void report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
10414 {
10415         struct cmd_obj *pcmd_obj;
10416         u8      *pevtcmd;
10417         u32 cmdsz;
10418         struct survey_event     *psurvey_evt;
10419         struct C2HEvent_Header *pc2h_evt_hdr;
10420         struct mlme_ext_priv *pmlmeext;
10421         struct cmd_priv *pcmdpriv;
10422         //u8 *pframe = precv_frame->u.hdr.rx_data;
10423         //uint len = precv_frame->u.hdr.len;
10424
10425         if(!padapter)
10426                 return;
10427
10428         pmlmeext = &padapter->mlmeextpriv;
10429         pcmdpriv = &padapter->cmdpriv;
10430
10431
10432         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10433         {
10434                 return;
10435         }
10436
10437         cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
10438         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
10439         {
10440                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10441                 return;
10442         }
10443
10444         _rtw_init_listhead(&pcmd_obj->list);
10445
10446         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
10447         pcmd_obj->cmdsz = cmdsz;
10448         pcmd_obj->parmbuf = pevtcmd;
10449
10450         pcmd_obj->rsp = NULL;
10451         pcmd_obj->rspsz  = 0;
10452
10453         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
10454         pc2h_evt_hdr->len = sizeof(struct survey_event);
10455         pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
10456         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
10457
10458         psurvey_evt = (struct survey_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
10459
10460         if (collect_bss_info(padapter, precv_frame, (WLAN_BSSID_EX *)&psurvey_evt->bss) == _FAIL)
10461         {
10462                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10463                 rtw_mfree((u8 *)pevtcmd, cmdsz);
10464                 return;
10465         }
10466
10467 #ifdef CONFIG_80211D
10468         process_80211d(padapter, &psurvey_evt->bss);
10469 #endif
10470
10471         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
10472
10473         pmlmeext->sitesurvey_res.bss_cnt++;
10474
10475         return;
10476
10477 }
10478
10479 void report_surveydone_event(_adapter *padapter)
10480 {
10481         struct cmd_obj *pcmd_obj;
10482         u8      *pevtcmd;
10483         u32 cmdsz;
10484         struct surveydone_event *psurveydone_evt;
10485         struct C2HEvent_Header  *pc2h_evt_hdr;
10486         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
10487         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
10488
10489         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10490         {
10491                 return;
10492         }
10493
10494         cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
10495         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
10496         {
10497                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10498                 return;
10499         }
10500
10501         _rtw_init_listhead(&pcmd_obj->list);
10502
10503         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
10504         pcmd_obj->cmdsz = cmdsz;
10505         pcmd_obj->parmbuf = pevtcmd;
10506
10507         pcmd_obj->rsp = NULL;
10508         pcmd_obj->rspsz  = 0;
10509
10510         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
10511         pc2h_evt_hdr->len = sizeof(struct surveydone_event);
10512         pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
10513         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
10514
10515         psurveydone_evt = (struct surveydone_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
10516         psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
10517
10518         DBG_871X("survey done event(%x) band:%d for "ADPT_FMT"\n", psurveydone_evt->bss_cnt, padapter->setband, ADPT_ARG(padapter));
10519
10520         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
10521
10522         return;
10523
10524 }
10525
10526 void report_join_res(_adapter *padapter, int res)
10527 {
10528         struct cmd_obj *pcmd_obj;
10529         u8      *pevtcmd;
10530         u32 cmdsz;
10531         struct joinbss_event            *pjoinbss_evt;
10532         struct C2HEvent_Header  *pc2h_evt_hdr;
10533         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
10534         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10535         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
10536
10537         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10538         {
10539                 return;
10540         }
10541
10542         cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
10543         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
10544         {
10545                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10546                 return;
10547         }
10548
10549         _rtw_init_listhead(&pcmd_obj->list);
10550
10551         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
10552         pcmd_obj->cmdsz = cmdsz;
10553         pcmd_obj->parmbuf = pevtcmd;
10554
10555         pcmd_obj->rsp = NULL;
10556         pcmd_obj->rspsz  = 0;
10557
10558         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
10559         pc2h_evt_hdr->len = sizeof(struct joinbss_event);
10560         pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
10561         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
10562
10563         pjoinbss_evt = (struct joinbss_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
10564         _rtw_memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
10565         pjoinbss_evt->network.join_res  = pjoinbss_evt->network.aid = res;
10566
10567         DBG_871X("report_join_res(%d)\n", res);
10568
10569
10570         rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network);
10571
10572
10573         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
10574
10575         return;
10576
10577 }
10578
10579 void report_del_sta_event(_adapter *padapter, unsigned char* MacAddr, unsigned short reason)
10580 {
10581         struct cmd_obj *pcmd_obj;
10582         u8      *pevtcmd;
10583         u32 cmdsz;
10584         struct sta_info *psta;
10585         int     mac_id;
10586         struct stadel_event                     *pdel_sta_evt;
10587         struct C2HEvent_Header  *pc2h_evt_hdr;
10588         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
10589         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
10590
10591         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10592         {
10593                 return;
10594         }
10595
10596         cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
10597         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
10598         {
10599                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10600                 return;
10601         }
10602
10603         _rtw_init_listhead(&pcmd_obj->list);
10604
10605         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
10606         pcmd_obj->cmdsz = cmdsz;
10607         pcmd_obj->parmbuf = pevtcmd;
10608
10609         pcmd_obj->rsp = NULL;
10610         pcmd_obj->rspsz  = 0;
10611
10612         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
10613         pc2h_evt_hdr->len = sizeof(struct stadel_event);
10614         pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
10615         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
10616
10617         pdel_sta_evt = (struct stadel_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
10618         _rtw_memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
10619         _rtw_memcpy((unsigned char *)(pdel_sta_evt->rsvd),(unsigned char *)(&reason),2);
10620
10621
10622         psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
10623         if(psta)
10624                 mac_id = (int)psta->mac_id;
10625         else
10626                 mac_id = (-1);
10627
10628         pdel_sta_evt->mac_id = mac_id;
10629
10630         DBG_871X("report_del_sta_event: delete STA, mac_id=%d\n", mac_id);
10631
10632         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
10633
10634         return;
10635 }
10636
10637 void report_add_sta_event(_adapter *padapter, unsigned char* MacAddr, int cam_idx)
10638 {
10639         struct cmd_obj *pcmd_obj;
10640         u8      *pevtcmd;
10641         u32 cmdsz;
10642         struct stassoc_event            *padd_sta_evt;
10643         struct C2HEvent_Header  *pc2h_evt_hdr;
10644         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
10645         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
10646
10647         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10648         {
10649                 return;
10650         }
10651
10652         cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
10653         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
10654         {
10655                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10656                 return;
10657         }
10658
10659         _rtw_init_listhead(&pcmd_obj->list);
10660
10661         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
10662         pcmd_obj->cmdsz = cmdsz;
10663         pcmd_obj->parmbuf = pevtcmd;
10664
10665         pcmd_obj->rsp = NULL;
10666         pcmd_obj->rspsz  = 0;
10667
10668         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
10669         pc2h_evt_hdr->len = sizeof(struct stassoc_event);
10670         pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
10671         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
10672
10673         padd_sta_evt = (struct stassoc_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
10674         _rtw_memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN);
10675         padd_sta_evt->cam_id = cam_idx;
10676
10677         DBG_871X("report_add_sta_event: add STA\n");
10678
10679         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
10680
10681         return;
10682 }
10683
10684
10685 bool rtw_port_switch_chk(_adapter *adapter)
10686 {
10687         bool switch_needed = _FALSE;
10688 #ifdef CONFIG_CONCURRENT_MODE
10689 #ifdef CONFIG_RUNTIME_PORT_SWITCH
10690         struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
10691         struct pwrctrl_priv *pwrctl = dvobj_to_pwrctl(dvobj);
10692         _adapter *if_port0 = NULL;
10693         _adapter *if_port1 = NULL;
10694         struct mlme_ext_info *if_port0_mlmeinfo = NULL;
10695         struct mlme_ext_info *if_port1_mlmeinfo = NULL;
10696         int i;
10697
10698         for (i = 0; i < dvobj->iface_nums; i++) {
10699                 if (get_iface_type(dvobj->padapters[i]) == IFACE_PORT0) {
10700                         if_port0 = dvobj->padapters[i];
10701                         if_port0_mlmeinfo = &(if_port0->mlmeextpriv.mlmext_info);
10702                 }
10703                 else if (get_iface_type(dvobj->padapters[i]) == IFACE_PORT1) {
10704                         if_port1 = dvobj->padapters[i];
10705                         if_port1_mlmeinfo = &(if_port1->mlmeextpriv.mlmext_info);
10706                 }
10707         }
10708
10709         if (if_port0 == NULL) {
10710                 rtw_warn_on(1);
10711                 goto exit;
10712         }
10713
10714         if (if_port1 == NULL) {
10715                 rtw_warn_on(1);
10716                 goto exit;
10717         }
10718
10719 #ifdef DBG_RUNTIME_PORT_SWITCH
10720         DBG_871X(FUNC_ADPT_FMT" wowlan_mode:%u\n"
10721                 ADPT_FMT", port0, mlmeinfo->state:0x%08x, p2p_state:%d, %d\n"
10722                 ADPT_FMT", port1, mlmeinfo->state:0x%08x, p2p_state:%d, %d\n",
10723                 FUNC_ADPT_ARG(adapter), pwrctl->wowlan_mode,
10724                 ADPT_ARG(if_port0), if_port0_mlmeinfo->state, rtw_p2p_state(&if_port0->wdinfo), rtw_p2p_chk_state(&if_port0->wdinfo, P2P_STATE_NONE),
10725                 ADPT_ARG(if_port1), if_port1_mlmeinfo->state, rtw_p2p_state(&if_port1->wdinfo), rtw_p2p_chk_state(&if_port1->wdinfo, P2P_STATE_NONE));
10726 #endif /* DBG_RUNTIME_PORT_SWITCH */
10727
10728 #ifdef CONFIG_WOWLAN
10729         /* WOWLAN interface(primary, for now) should be port0 */
10730         if (pwrctl->wowlan_mode == _TRUE) {
10731                 if(!is_primary_adapter(if_port0)) {
10732                         DBG_871X("%s "ADPT_FMT" enable WOWLAN\n", __func__, ADPT_ARG(if_port1));
10733                         switch_needed = _TRUE;
10734                 }
10735                 goto exit;
10736         }
10737 #endif /* CONFIG_WOWLAN */
10738
10739         /* AP should use port0 for ctl frame's ack */
10740         if ((if_port1_mlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
10741                 DBG_871X("%s "ADPT_FMT" is AP/GO\n", __func__, ADPT_ARG(if_port1));
10742                 switch_needed = _TRUE;
10743                 goto exit;
10744         }
10745
10746         /* GC should use port0 for p2p ps */
10747         if (((if_port1_mlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE)
10748                 && (if_port1_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
10749                 && !rtw_p2p_chk_state(&if_port1->wdinfo, P2P_STATE_NONE)
10750                 && !check_fwstate(&if_port1->mlmepriv, WIFI_UNDER_WPS)
10751         ) {
10752                 DBG_871X("%s "ADPT_FMT" is GC\n", __func__, ADPT_ARG(if_port1));
10753                 switch_needed = _TRUE;
10754                 goto exit;
10755         }
10756
10757         /* port1 linked, but port0 not linked */
10758         if ((if_port1_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
10759                 && !(if_port0_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
10760                 && ((if_port0_mlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
10761         ) {
10762                 DBG_871X("%s "ADPT_FMT" is SINGLE_LINK\n", __func__, ADPT_ARG(if_port1));
10763                 switch_needed = _TRUE;
10764                 goto exit;
10765         }
10766
10767 exit:
10768 #ifdef DBG_RUNTIME_PORT_SWITCH
10769         DBG_871X(FUNC_ADPT_FMT" ret:%d\n", FUNC_ADPT_ARG(adapter), switch_needed);
10770 #endif /* DBG_RUNTIME_PORT_SWITCH */
10771 #endif /* CONFIG_RUNTIME_PORT_SWITCH */
10772 #endif /* CONFIG_CONCURRENT_MODE */
10773         return switch_needed;
10774 }
10775
10776 /****************************************************************************
10777
10778 Following are the event callback functions
10779
10780 *****************************************************************************/
10781
10782 //for sta/adhoc mode
10783 void update_sta_info(_adapter *padapter, struct sta_info *psta)
10784 {
10785         _irqL   irqL;
10786         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
10787         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10788         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10789
10790         //ERP
10791         VCS_update(padapter, psta);
10792
10793 #ifdef CONFIG_80211N_HT
10794         //HT
10795         if(pmlmepriv->htpriv.ht_option)
10796         {
10797                 psta->htpriv.ht_option = _TRUE;
10798
10799                 psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
10800
10801                 if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_20))
10802                         psta->htpriv.sgi_20m = _TRUE;
10803
10804                 if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_40))
10805                         psta->htpriv.sgi_40m = _TRUE;
10806
10807                 psta->qos_option = _TRUE;
10808
10809                 _rtw_memcpy(&psta->htpriv.ht_cap, &pmlmeinfo->HT_caps, sizeof(struct rtw_ieee80211_ht_cap));
10810         }
10811         else
10812 #endif //CONFIG_80211N_HT
10813         {
10814 #ifdef CONFIG_80211N_HT
10815                 psta->htpriv.ht_option = _FALSE;
10816
10817                 psta->htpriv.ampdu_enable = _FALSE;
10818
10819                 psta->htpriv.sgi_20m = _FALSE;
10820                 psta->htpriv.sgi_40m = _FALSE;
10821 #endif //CONFIG_80211N_HT
10822                 psta->qos_option = _FALSE;
10823
10824         }
10825
10826 #ifdef CONFIG_80211N_HT
10827         psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
10828
10829         psta->htpriv.agg_enable_bitmap = 0x0;//reset
10830         psta->htpriv.candidate_tid_bitmap = 0x0;//reset
10831 #endif //CONFIG_80211N_HT
10832
10833         psta->bw_mode = pmlmeext->cur_bwmode;
10834
10835         //QoS
10836         if(pmlmepriv->qospriv.qos_option)
10837                 psta->qos_option = _TRUE;
10838
10839 #ifdef CONFIG_80211AC_VHT
10840         _rtw_memcpy(&psta->vhtpriv, &pmlmepriv->vhtpriv, sizeof(struct vht_priv));
10841 #endif //CONFIG_80211AC_VHT
10842
10843         _enter_critical_bh(&psta->lock, &irqL);
10844         psta->state = _FW_LINKED;
10845         _exit_critical_bh(&psta->lock, &irqL);
10846
10847 }
10848
10849 static void rtw_mlmeext_disconnect(_adapter *padapter)
10850 {
10851         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
10852         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10853         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10854         u8 state_backup = (pmlmeinfo->state&0x03);
10855
10856         //set_opmode_cmd(padapter, infra_client_with_mlme);
10857
10858         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
10859         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
10860
10861         //set MSR to no link state -> infra. mode
10862         Set_MSR(padapter, _HW_STATE_STATION_);
10863
10864         pmlmeinfo->state = WIFI_FW_NULL_STATE;
10865
10866         if(state_backup == WIFI_FW_STATION_STATE)
10867         {
10868                 if (rtw_port_switch_chk(padapter) == _TRUE) {
10869                         rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
10870                         #ifdef CONFIG_LPS
10871                         {
10872                                 _adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter));
10873                                 if (port0_iface)
10874                                         rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0);
10875                         }
10876                         #endif
10877                 }
10878         }
10879
10880 #ifdef CONFIG_DUALMAC_CONCURRENT
10881         dc_set_channel_bwmode_disconnect(padapter);
10882 #else //!CONFIG_DUALMAC_CONCURRENT
10883 #ifdef CONFIG_CONCURRENT_MODE
10884         if((check_buddy_fwstate(padapter, _FW_LINKED)) != _TRUE)
10885         {
10886 #endif //CONFIG_CONCURRENT_MODE
10887                 //switch to the 20M Hz mode after disconnect
10888                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
10889                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
10890
10891                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
10892 #ifdef CONFIG_CONCURRENT_MODE
10893         }
10894 #endif //CONFIG_CONCURRENT_MODE
10895 #ifdef CONFIG_FCS_MODE
10896         else
10897         {
10898                 PADAPTER pbuddy_adapter;
10899                 struct mlme_ext_priv *pbuddy_mlmeext;
10900
10901                 if(EN_FCS(padapter))
10902                 {
10903                         pbuddy_adapter = padapter->pbuddy_adapter;
10904                         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
10905
10906                         rtw_hal_set_hwreg(padapter, HW_VAR_STOP_FCS_MODE, NULL);
10907
10908                         //switch to buddy's channel setting if buddy is linked
10909                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
10910                 }
10911         }
10912 #endif //!CONFIG_FCS_MODE
10913 #endif //!CONFIG_DUALMAC_CONCURRENT
10914
10915
10916         flush_all_cam_entry(padapter);
10917
10918         _cancel_timer_ex(&pmlmeext->link_timer);
10919
10920         //pmlmepriv->LinkDetectInfo.TrafficBusyState = _FALSE;
10921         pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
10922         pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
10923
10924 }
10925
10926 void mlmeext_joinbss_event_callback(_adapter *padapter, int join_res)
10927 {
10928         struct sta_info         *psta, *psta_bmc;
10929         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10930         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10931         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
10932         struct sta_priv         *pstapriv = &padapter->stapriv;
10933         u8      join_type;
10934
10935         if(join_res < 0)
10936         {
10937                 join_type = 1;
10938                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
10939                 rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
10940
10941                 goto exit_mlmeext_joinbss_event_callback;
10942         }
10943
10944         if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
10945         {
10946                 //update bc/mc sta_info
10947                 update_bmc_sta(padapter);
10948         }
10949
10950
10951         //turn on dynamic functions
10952         Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE);
10953
10954         // update IOT-releated issue
10955         update_IOT_info(padapter);
10956
10957         rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates);
10958
10959         //BCN interval
10960         rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
10961
10962         //udpate capability
10963         update_capinfo(padapter, pmlmeinfo->capability);
10964
10965         //WMM, Update EDCA param
10966         WMMOnAssocRsp(padapter);
10967
10968         //HT
10969         HTOnAssocRsp(padapter);
10970
10971 #ifdef CONFIG_80211AC_VHT
10972         //VHT
10973         VHTOnAssocRsp(padapter);
10974 #endif
10975
10976 #ifndef CONFIG_CONCURRENT_MODE
10977         //      Call set_channel_bwmode when the CONFIG_CONCURRENT_MODE doesn't be defined.
10978         //Set cur_channel&cur_bwmode&cur_ch_offset
10979         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
10980 #endif
10981
10982         psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
10983         if (psta) //only for infra. mode
10984         {
10985                 pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
10986
10987                 //DBG_871X("set_sta_rate\n");
10988
10989                 psta->wireless_mode = pmlmeext->cur_wireless_mode;
10990
10991                 //set per sta rate after updating HT cap.
10992                 set_sta_rate(padapter, psta);
10993
10994                 rtw_sta_media_status_rpt(padapter, psta, 1);
10995         }
10996
10997         if (rtw_port_switch_chk(padapter) == _TRUE)
10998                 rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
10999
11000         join_type = 2;
11001         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
11002
11003         if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
11004         {
11005                 // correcting TSF
11006                 correct_TSF(padapter, pmlmeext);
11007
11008                 //set_link_timer(pmlmeext, DISCONNECT_TO);
11009         }
11010
11011 #ifdef CONFIG_LPS
11012         if(get_iface_type(padapter) == IFACE_PORT0)
11013                 rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
11014 #endif
11015
11016 #ifdef CONFIG_BEAMFORMING
11017         beamforming_wk_cmd(padapter, BEAMFORMING_CTRL_ENTER, (u8 *)psta, sizeof(struct sta_info), 0);
11018 #endif
11019
11020 exit_mlmeext_joinbss_event_callback:
11021
11022 #ifdef CONFIG_DUALMAC_CONCURRENT
11023         dc_handle_join_done(padapter, join_res);
11024 #endif
11025 #ifdef CONFIG_CONCURRENT_MODE
11026         concurrent_chk_joinbss_done(padapter, join_res);
11027 #endif
11028
11029         DBG_871X("=>%s\n", __FUNCTION__);
11030
11031 }
11032
11033 //currently only adhoc mode will go here
11034 void mlmeext_sta_add_event_callback(_adapter *padapter, struct sta_info *psta)
11035 {
11036         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
11037         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11038         u8      join_type;
11039
11040         DBG_871X("%s\n", __FUNCTION__);
11041
11042         if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
11043         {
11044                 if(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)//adhoc master or sta_count>1
11045                 {
11046                         //nothing to do
11047                 }
11048                 else//adhoc client
11049                 {
11050                         //update TSF Value
11051                         //update_TSF(pmlmeext, pframe, len);
11052
11053                         // correcting TSF
11054                         correct_TSF(padapter, pmlmeext);
11055
11056                         //start beacon
11057                         if(send_beacon(padapter)==_FAIL)
11058                         {
11059                                 pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
11060
11061                                 pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
11062
11063                                 return;
11064                         }
11065
11066                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
11067
11068                 }
11069
11070                 join_type = 2;
11071                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
11072         }
11073
11074         pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
11075
11076         psta->bssratelen = rtw_get_rateset_len(pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates);
11077         _rtw_memcpy(psta->bssrateset, pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates, psta->bssratelen);
11078
11079         //update adhoc sta_info
11080         update_sta_info(padapter, psta);
11081
11082         rtw_hal_update_sta_rate_mask(padapter, psta);
11083
11084         // ToDo: HT for Ad-hoc
11085         psta->wireless_mode = rtw_check_network_type(psta->bssrateset, psta->bssratelen, pmlmeext->cur_channel);
11086         psta->raid = rtw_hal_networktype_to_raid(padapter, psta);
11087
11088         //rate radaptive
11089         Update_RA_Entry(padapter, psta);
11090 }
11091
11092 void mlmeext_sta_del_event_callback(_adapter *padapter)
11093 {
11094         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11095         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11096
11097         if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter))
11098         {
11099                 rtw_mlmeext_disconnect(padapter);
11100         }
11101
11102 }
11103
11104 /****************************************************************************
11105
11106 Following are the functions for the timer handlers
11107
11108 *****************************************************************************/
11109 void _linked_info_dump(_adapter *padapter)
11110 {
11111         int i;
11112         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11113       struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11114         u8 mac_id;
11115         int UndecoratedSmoothedPWDB;
11116         struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter);
11117         #if 0
11118         DBG_871X("============ linked status check ===================\n");
11119         DBG_871X("pathA Rx SNRdb:%d, pathB Rx SNRdb:%d\n",padapter->recvpriv.RxSNRdB[0], padapter->recvpriv.RxSNRdB[1]);
11120         DBG_871X("pathA Rx PWDB:%d\n",padapter->recvpriv.rxpwdb);
11121         rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
11122         DBG_871X("UndecoratedSmoothedPWDB:%d\n",UndecoratedSmoothedPWDB);
11123         DBG_871X("Rx RSSI:%d\n",padapter->recvpriv.rssi);
11124         DBG_871X("Rx Signal_strength:%d\n",padapter->recvpriv.signal_strength);
11125         DBG_871X("Rx Signal_qual:%d \n",padapter->recvpriv.signal_qual);
11126         if ( check_fwstate( &padapter->mlmepriv,  _FW_LINKED ) )
11127         {
11128                 DBG_871X("bw mode: %d, channel: %d\n", padapter->mlmeextpriv.cur_bwmode, padapter->mlmeextpriv.cur_channel );
11129                 DBG_871X("received bytes = %d\n", (u32) (padapter->recvpriv.rx_bytes - padapter->recvpriv.last_rx_bytes ) );
11130         }
11131         DBG_871X("============ linked status check ===================\n");
11132         DBG_871X("============ RX GAIN / FALSE ALARM  ===================\n");
11133         DBG_871X(" DIG PATH-A(0x%02x), PATH-B(0x%02x)\n",rtw_read8(padapter,0xc50),rtw_read8(padapter,0xc58));
11134         DBG_871X(" OFDM -Alarm DA2(0x%04x),DA4(0x%04x),DA6(0x%04x),DA8(0x%04x)\n",
11135                 rtw_read16(padapter,0xDA2),rtw_read16(padapter,0xDA4),rtw_read16(padapter,0xDA6),rtw_read16(padapter,0xDA8));
11136
11137         DBG_871X(" CCK -Alarm A5B(0x%02x),A5C(0x%02x)\n",rtw_read8(padapter,0xA5B),rtw_read8(padapter,0xA5C));
11138         #endif
11139
11140         if(padapter->bLinkInfoDump){
11141
11142                 DBG_871X("\n============ linked status check ===================\n");
11143
11144                 if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
11145                 {
11146                         rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
11147
11148                         DBG_871X("UndecoratedSmoothedPWDB:%d\n",UndecoratedSmoothedPWDB);
11149                 }
11150                 else if((pmlmeinfo->state&0x03) == _HW_STATE_AP_)
11151                 {
11152                         _irqL irqL;
11153                         _list   *phead, *plist;
11154
11155                         struct sta_info *psta=NULL;
11156                         struct sta_priv *pstapriv = &padapter->stapriv;
11157
11158                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
11159                         phead = &pstapriv->asoc_list;
11160                         plist = get_next(phead);
11161                         while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
11162                         {
11163                                 psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
11164                                 plist = get_next(plist);
11165
11166                                 DBG_871X("STA:UndecoratedSmoothedPWDB:%d\n",
11167                                         psta->rssi_stat.UndecoratedSmoothedPWDB);
11168                         }
11169                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
11170
11171                 }
11172                 for(i=0; i<NUM_STA; i++)
11173                 {
11174                         if(pdvobj->macid[i] == _TRUE)
11175                         {
11176                                 if(i !=1) //skip bc/mc sta
11177                                         //============  tx info ============
11178                                         rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, &i);
11179                         }
11180                 }
11181         }
11182
11183
11184 }
11185
11186 u8 chk_ap_is_alive(_adapter *padapter, struct sta_info *psta)
11187 {
11188         u8 ret = _FALSE;
11189         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11190         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11191
11192         #ifdef DBG_EXPIRATION_CHK
11193         DBG_871X(FUNC_ADPT_FMT" rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu"
11194                                 /*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/
11195                                 ", retry:%u\n"
11196                 , FUNC_ADPT_ARG(padapter)
11197                 , STA_RX_PKTS_DIFF_ARG(psta)
11198                 , psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts
11199                 , psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts
11200                 /*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts
11201                 , psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts
11202                 , psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts
11203                 , pmlmeinfo->bcn_interval*/
11204                 , pmlmeext->retry
11205         );
11206
11207         DBG_871X(FUNC_ADPT_FMT" tx_pkts:%llu, link_count:%u\n", FUNC_ADPT_ARG(padapter)
11208                 , padapter->xmitpriv.tx_pkts
11209                 , pmlmeinfo->link_count
11210         );
11211         #endif
11212
11213         if((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta))
11214                 && sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta)
11215                 && sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta)
11216         )
11217         {
11218                 ret = _FALSE;
11219         }
11220         else
11221         {
11222                 ret = _TRUE;
11223         }
11224
11225         sta_update_last_rx_pkts(psta);
11226
11227         return ret;
11228 }
11229
11230 void linked_status_chk(_adapter *padapter)
11231 {
11232         u32     i;
11233         struct sta_info         *psta;
11234         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
11235         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11236         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11237         struct sta_priv         *pstapriv = &padapter->stapriv;
11238
11239
11240         if (is_client_associated_to_ap(padapter))
11241         {
11242                 //linked infrastructure client mode
11243
11244                 int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
11245                 int rx_chk_limit;
11246                 int link_count_limit;
11247
11248                 #if defined(DBG_ROAMING_TEST)
11249                 rx_chk_limit = 1;
11250                 #elif defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && !defined(CONFIG_LPS_LCLK_WD_TIMER)
11251                 rx_chk_limit = 4;
11252                 #else
11253                 rx_chk_limit = 8;
11254                 #endif
11255 #ifdef CONFIG_P2P
11256                 if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE))
11257                         link_count_limit = 3; // 8 sec
11258                 else
11259 #endif // CONFIG_P2P
11260                         link_count_limit = 7; // 16 sec
11261
11262                 // Marked by Kurt 20130715
11263                 // For WiDi 3.5 and latered on, they don't ask WiDi sink to do roaming, so we could not check rx limit that strictly.
11264                 // todo: To check why we under miracast session, rx_chk would be _FALSE
11265                 //#ifdef CONFIG_INTEL_WIDI
11266                 //if (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_NONE)
11267                 //      rx_chk_limit = 1;
11268                 //#endif
11269
11270                 if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
11271                 {
11272                         bool is_p2p_enable = _FALSE;
11273                         #ifdef CONFIG_P2P
11274                         is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
11275                         #endif
11276
11277                         if (chk_ap_is_alive(padapter, psta) == _FALSE)
11278                                 rx_chk = _FAIL;
11279
11280                         if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
11281                                 tx_chk = _FAIL;
11282
11283                         #if defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && !defined(CONFIG_LPS_LCLK_WD_TIMER)
11284                         if (pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL)) {
11285                                 u8 backup_oper_channel=0;
11286
11287                                 /* switch to correct channel of current network  before issue keep-alive frames */
11288                                 if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
11289                                         backup_oper_channel = rtw_get_oper_ch(padapter);
11290                                         SelectChannel(padapter, pmlmeext->cur_channel);
11291                                 }
11292
11293                                 if (rx_chk != _SUCCESS)
11294                                         issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
11295
11296                                 if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit) || rx_chk != _SUCCESS) {
11297                                         tx_chk = issue_nulldata(padapter, psta->hwaddr, 0, 3, 1);
11298                                         /* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */
11299                                         if (tx_chk == _SUCCESS && !is_p2p_enable)
11300                                                 rx_chk = _SUCCESS;
11301                                 }
11302
11303                                 /* back to the original operation channel */
11304                                 if(backup_oper_channel>0)
11305                                         SelectChannel(padapter, backup_oper_channel);
11306
11307                         }
11308                         else
11309                         #endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */
11310                         {
11311                                 if (rx_chk != _SUCCESS) {
11312                                         if (pmlmeext->retry == 0) {
11313                                                 #ifdef DBG_EXPIRATION_CHK
11314                                                 DBG_871X("issue_probereq to trigger probersp, retry=%d\n", pmlmeext->retry);
11315                                                 #endif
11316                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
11317                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
11318                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
11319                                         }
11320                                 }
11321
11322                                 if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit) {
11323                                         #ifdef DBG_EXPIRATION_CHK
11324                                         DBG_871X("%s issue_nulldata 0\n", __FUNCTION__);
11325                                         #endif
11326                                         tx_chk = issue_nulldata_in_interrupt(padapter, NULL);
11327                                 }
11328                         }
11329
11330                         if (rx_chk == _FAIL) {
11331                                 pmlmeext->retry++;
11332                                 if (pmlmeext->retry > rx_chk_limit) {
11333                                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n",
11334                                                 FUNC_ADPT_ARG(padapter));
11335                                         receive_disconnect(padapter, pmlmeinfo->network.MacAddress
11336                                                 , WLAN_REASON_EXPIRATION_CHK);
11337                                         return;
11338                                 }
11339                         } else {
11340                                 pmlmeext->retry = 0;
11341                         }
11342
11343                         if (tx_chk == _FAIL) {
11344                                 pmlmeinfo->link_count %= (link_count_limit+1);
11345                         } else {
11346                                 pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
11347                                 pmlmeinfo->link_count = 0;
11348                         }
11349
11350                 } //end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL)
11351         }
11352         else if (is_client_associated_to_ibss(padapter))
11353         {
11354                 //linked IBSS mode
11355                 //for each assoc list entry to check the rx pkt counter
11356                 for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
11357                 {
11358                         if (pmlmeinfo->FW_sta_info[i].status == 1)
11359                         {
11360                                 psta = pmlmeinfo->FW_sta_info[i].psta;
11361
11362                                 if(NULL==psta) continue;
11363
11364                                 if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta))
11365                                 {
11366
11367                                         if(pmlmeinfo->FW_sta_info[i].retry<3)
11368                                         {
11369                                                 pmlmeinfo->FW_sta_info[i].retry++;
11370                                         }
11371                                         else
11372                                         {
11373                                                 pmlmeinfo->FW_sta_info[i].retry = 0;
11374                                                 pmlmeinfo->FW_sta_info[i].status = 0;
11375                                                 report_del_sta_event(padapter, psta->hwaddr
11376                                                         , 65535// indicate disconnect caused by no rx
11377                                                 );
11378                                         }
11379                                 }
11380                                 else
11381                                 {
11382                                         pmlmeinfo->FW_sta_info[i].retry = 0;
11383                                         pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
11384                                 }
11385                         }
11386                 }
11387
11388                 //set_link_timer(pmlmeext, DISCONNECT_TO);
11389
11390         }
11391
11392 }
11393
11394 void survey_timer_hdl(_adapter *padapter)
11395 {
11396         struct cmd_obj  *ph2c;
11397         struct sitesurvey_parm  *psurveyPara;
11398         struct cmd_priv                                 *pcmdpriv=&padapter->cmdpriv;
11399         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
11400 #ifdef CONFIG_P2P
11401         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
11402 #endif
11403
11404         //DBG_871X("marc: survey timer\n");
11405
11406         //issue rtw_sitesurvey_cmd
11407         if (pmlmeext->sitesurvey_res.state > SCAN_START)
11408         {
11409                 if(pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
11410                 {
11411 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
11412                         if( padapter->mlmeextpriv.mlmext_info.scan_cnt != RTW_SCAN_NUM_OF_CH )
11413 #endif
11414                                 pmlmeext->sitesurvey_res.channel_idx++;
11415                 }
11416
11417                 if(pmlmeext->scan_abort == _TRUE)
11418                 {
11419                         #ifdef CONFIG_P2P
11420                         if(!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE))
11421                         {
11422                                 rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
11423                                 pmlmeext->sitesurvey_res.channel_idx = 3;
11424                                 DBG_871X("%s idx:%d, cnt:%u\n", __FUNCTION__
11425                                         , pmlmeext->sitesurvey_res.channel_idx
11426                                         , pwdinfo->find_phase_state_exchange_cnt
11427                                 );
11428                         }
11429                         else
11430                         #endif
11431                         {
11432                                 pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
11433                                 DBG_871X("%s idx:%d\n", __FUNCTION__
11434                                         , pmlmeext->sitesurvey_res.channel_idx
11435                                 );
11436                         }
11437
11438                         pmlmeext->scan_abort = _FALSE;//reset
11439                 }
11440
11441                 if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
11442                 {
11443                         goto exit_survey_timer_hdl;
11444                 }
11445
11446                 if ((psurveyPara = (struct sitesurvey_parm*)rtw_zmalloc(sizeof(struct sitesurvey_parm))) == NULL)
11447                 {
11448                         rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
11449                         goto exit_survey_timer_hdl;
11450                 }
11451
11452                 init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
11453                 rtw_enqueue_cmd(pcmdpriv, ph2c);
11454         }
11455
11456
11457 exit_survey_timer_hdl:
11458
11459         return;
11460 }
11461
11462 void link_timer_hdl(_adapter *padapter)
11463 {
11464         //static unsigned int           rx_pkt = 0;
11465         //static u64                            tx_cnt = 0;
11466         //struct xmit_priv              *pxmitpriv = &(padapter->xmitpriv);
11467         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11468         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11469         //struct sta_priv               *pstapriv = &padapter->stapriv;
11470
11471
11472         if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
11473         {
11474                 DBG_871X("link_timer_hdl:no beacon while connecting\n");
11475                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
11476                 report_join_res(padapter, -3);
11477         }
11478         else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE)
11479         {
11480                 //re-auth timer
11481                 if (++pmlmeinfo->reauth_count > REAUTH_LIMIT)
11482                 {
11483                         //if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto)
11484                         //{
11485                                 pmlmeinfo->state = 0;
11486                                 report_join_res(padapter, -1);
11487                                 return;
11488                         //}
11489                         //else
11490                         //{
11491                         //      pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
11492                         //      pmlmeinfo->reauth_count = 0;
11493                         //}
11494                 }
11495
11496                 DBG_871X("link_timer_hdl: auth timeout and try again\n");
11497                 pmlmeinfo->auth_seq = 1;
11498                 issue_auth(padapter, NULL, 0);
11499                 set_link_timer(pmlmeext, REAUTH_TO);
11500         }
11501         else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)
11502         {
11503                 //re-assoc timer
11504                 if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT)
11505                 {
11506                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
11507                         report_join_res(padapter, -2);
11508                         return;
11509                 }
11510
11511                 DBG_871X("link_timer_hdl: assoc timeout and try again\n");
11512                 issue_assocreq(padapter);
11513                 set_link_timer(pmlmeext, REASSOC_TO);
11514         }
11515 #if 0
11516         else if (is_client_associated_to_ap(padapter))
11517         {
11518                 //linked infrastructure client mode
11519                 if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
11520                 {
11521                         /*to monitor whether the AP is alive or not*/
11522                         if (rx_pkt == psta->sta_stats.rx_pkts)
11523                         {
11524                                 receive_disconnect(padapter, pmlmeinfo->network.MacAddress);
11525                                 return;
11526                         }
11527                         else
11528                         {
11529                                 rx_pkt = psta->sta_stats.rx_pkts;
11530                                 set_link_timer(pmlmeext, DISCONNECT_TO);
11531                         }
11532
11533                         //update the EDCA paramter according to the Tx/RX mode
11534                         update_EDCA_param(padapter);
11535
11536                         /*to send the AP a nulldata if no frame is xmitted in order to keep alive*/
11537                         if (pmlmeinfo->link_count++ == 0)
11538                         {
11539                                 tx_cnt = pxmitpriv->tx_pkts;
11540                         }
11541                         else if ((pmlmeinfo->link_count & 0xf) == 0)
11542                         {
11543                                 if (tx_cnt == pxmitpriv->tx_pkts)
11544                                 {
11545                                         issue_nulldata_in_interrupt(padapter, NULL);
11546                                 }
11547
11548                                 tx_cnt = pxmitpriv->tx_pkts;
11549                         }
11550                 } //end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL)
11551         }
11552         else if (is_client_associated_to_ibss(padapter))
11553         {
11554                 //linked IBSS mode
11555                 //for each assoc list entry to check the rx pkt counter
11556                 for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
11557                 {
11558                         if (pmlmeinfo->FW_sta_info[i].status == 1)
11559                         {
11560                                 psta = pmlmeinfo->FW_sta_info[i].psta;
11561
11562                                 if (pmlmeinfo->FW_sta_info[i].rx_pkt == psta->sta_stats.rx_pkts)
11563                                 {
11564                                         pmlmeinfo->FW_sta_info[i].status = 0;
11565                                         report_del_sta_event(padapter, psta->hwaddr);
11566                                 }
11567                                 else
11568                                 {
11569                                         pmlmeinfo->FW_sta_info[i].rx_pkt = psta->sta_stats.rx_pkts;
11570                                 }
11571                         }
11572                 }
11573
11574                 set_link_timer(pmlmeext, DISCONNECT_TO);
11575         }
11576 #endif
11577
11578         return;
11579 }
11580
11581 void addba_timer_hdl(struct sta_info *psta)
11582 {
11583 #ifdef CONFIG_80211N_HT
11584         struct ht_priv  *phtpriv;
11585
11586         if(!psta)
11587                 return;
11588
11589         phtpriv = &psta->htpriv;
11590
11591         if((phtpriv->ht_option==_TRUE) && (phtpriv->ampdu_enable==_TRUE))
11592         {
11593                 if(phtpriv->candidate_tid_bitmap)
11594                         phtpriv->candidate_tid_bitmap=0x0;
11595
11596         }
11597 #endif //CONFIG_80211N_HT
11598 }
11599
11600 #ifdef CONFIG_IEEE80211W
11601 void sa_query_timer_hdl(_adapter *padapter)
11602 {
11603         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11604         struct mlme_priv * pmlmepriv = &padapter->mlmepriv;
11605         _irqL irqL;
11606         //disconnect
11607         _enter_critical_bh(&pmlmepriv->lock, &irqL);
11608
11609         if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
11610         {
11611                 rtw_disassoc_cmd(padapter, 0, _TRUE);
11612                 rtw_indicate_disconnect(padapter);
11613                 rtw_free_assoc_resources(padapter, 1);
11614         }
11615
11616         _exit_critical_bh(&pmlmepriv->lock, &irqL);
11617         DBG_871X("SA query timeout disconnect\n");
11618 }
11619 #endif //CONFIG_IEEE80211W
11620
11621 u8 NULL_hdl(_adapter *padapter, u8 *pbuf)
11622 {
11623         return H2C_SUCCESS;
11624 }
11625
11626 #ifdef CONFIG_AUTO_AP_MODE
11627 void rtw_start_auto_ap(_adapter *adapter)
11628 {
11629         DBG_871X("%s\n", __FUNCTION__);
11630
11631         rtw_set_802_11_infrastructure_mode(adapter, Ndis802_11APMode);
11632
11633         rtw_setopmode_cmd(adapter, Ndis802_11APMode,_TRUE);
11634 }
11635
11636 static int rtw_auto_ap_start_beacon(_adapter *adapter)
11637 {
11638         int ret=0;
11639         u8 *pbuf = NULL;
11640         uint len;
11641         u8      supportRate[16];
11642         int     sz = 0, rateLen;
11643         u8 *    ie;
11644         u8      wireless_mode, oper_channel;
11645         u8 ssid[3] = {0}; //hidden ssid
11646         u32 ssid_len = sizeof(ssid);
11647         struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
11648
11649
11650         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) != _TRUE)
11651                 return -EINVAL;
11652
11653
11654         len = 128;
11655         pbuf = rtw_zmalloc(len);
11656         if(!pbuf)
11657                 return -ENOMEM;
11658
11659
11660         //generate beacon
11661         ie = pbuf;
11662
11663         //timestamp will be inserted by hardware
11664         sz += 8;
11665         ie += sz;
11666
11667         //beacon interval : 2bytes
11668         *(u16*)ie = cpu_to_le16((u16)100);//BCN_INTERVAL=100;
11669         sz += 2;
11670         ie += 2;
11671
11672         //capability info
11673         *(u16*)ie = 0;
11674         *(u16*)ie |= cpu_to_le16(cap_ESS);
11675         *(u16*)ie |= cpu_to_le16(cap_ShortPremble);
11676         //*(u16*)ie |= cpu_to_le16(cap_Privacy);
11677         sz += 2;
11678         ie += 2;
11679
11680         //SSID
11681         ie = rtw_set_ie(ie, _SSID_IE_, ssid_len, ssid, &sz);
11682
11683         //supported rates
11684         wireless_mode = WIRELESS_11BG_24N;
11685         rtw_set_supported_rate(supportRate, wireless_mode) ;
11686         rateLen = rtw_get_rateset_len(supportRate);
11687         if (rateLen > 8)
11688         {
11689                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, supportRate, &sz);
11690         }
11691         else
11692         {
11693                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, supportRate, &sz);
11694         }
11695
11696
11697         //DS parameter set
11698         if(check_buddy_fwstate(adapter, _FW_LINKED) &&
11699                 check_buddy_fwstate(adapter, WIFI_STATION_STATE))
11700         {
11701                 PADAPTER pbuddy_adapter = adapter->pbuddy_adapter;
11702                 struct mlme_ext_priv *pbuddy_mlmeext  = &pbuddy_adapter->mlmeextpriv;
11703
11704                 oper_channel = pbuddy_mlmeext->cur_channel;
11705         }
11706         else
11707         {
11708                 oper_channel = adapter_to_dvobj(adapter)->oper_channel;
11709         }
11710         ie = rtw_set_ie(ie, _DSSET_IE_, 1, &oper_channel, &sz);
11711
11712         //ext supported rates
11713         if (rateLen > 8)
11714         {
11715                 ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (supportRate + 8), &sz);
11716         }
11717
11718         DBG_871X("%s, start auto ap beacon sz=%d\n", __FUNCTION__, sz);
11719
11720         //lunch ap mode & start to issue beacon
11721         if(rtw_check_beacon_data(adapter, pbuf,  sz) == _SUCCESS)
11722         {
11723
11724         }
11725         else
11726         {
11727                 ret = -EINVAL;
11728         }
11729
11730
11731         rtw_mfree(pbuf, len);
11732
11733         return ret;
11734
11735 }
11736 #endif//CONFIG_AUTO_AP_MODE
11737
11738 u8 setopmode_hdl(_adapter *padapter, u8 *pbuf)
11739 {
11740         u8      type;
11741         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11742         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11743         struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
11744
11745         if(psetop->mode == Ndis802_11APMode)
11746         {
11747                 pmlmeinfo->state = WIFI_FW_AP_STATE;
11748                 type = _HW_STATE_AP_;
11749 #ifdef CONFIG_NATIVEAP_MLME
11750                 //start_ap_mode(padapter);
11751 #endif
11752         }
11753         else if(psetop->mode == Ndis802_11Infrastructure)
11754         {
11755                 pmlmeinfo->state &= ~(BIT(0)|BIT(1));// clear state
11756                 pmlmeinfo->state |= WIFI_FW_STATION_STATE;//set to      STATION_STATE
11757                 type = _HW_STATE_STATION_;
11758         }
11759         else if(psetop->mode == Ndis802_11IBSS)
11760         {
11761                 type = _HW_STATE_ADHOC_;
11762         }
11763         else
11764         {
11765                 type = _HW_STATE_NOLINK_;
11766         }
11767
11768         rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
11769         //Set_NETYPE0_MSR(padapter, type);
11770
11771
11772 #ifdef CONFIG_AUTO_AP_MODE
11773         if(psetop->mode == Ndis802_11APMode)
11774                 rtw_auto_ap_start_beacon(padapter);
11775 #endif
11776
11777         if (rtw_port_switch_chk(padapter) == _TRUE)
11778         {
11779                 rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
11780
11781                 if(psetop->mode == Ndis802_11APMode)
11782                         adapter_to_pwrctl(padapter)->fw_psmode_iface_id = 0xff; //ap mode won't dowload rsvd pages
11783                 else if (psetop->mode == Ndis802_11Infrastructure) {
11784                         #ifdef CONFIG_LPS
11785                         _adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter));
11786                         if (port0_iface)
11787                                 rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0);
11788                         #endif
11789                 }
11790         }
11791
11792 #ifdef CONFIG_BT_COEXIST
11793         if (psetop->mode == Ndis802_11APMode)
11794         {
11795                 // Do this after port switch to
11796                 // prevent from downloading rsvd page to wrong port
11797                 rtw_btcoex_MediaStatusNotify(padapter, 1); //connect
11798         }
11799 #endif // CONFIG_BT_COEXIST
11800
11801         return H2C_SUCCESS;
11802
11803 }
11804
11805 u8 createbss_hdl(_adapter *padapter, u8 *pbuf)
11806 {
11807         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11808         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11809         WLAN_BSSID_EX   *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
11810         struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
11811         //u32   initialgain;
11812
11813 #ifdef CONFIG_AP_MODE
11814         if (pmlmeinfo->state == WIFI_FW_AP_STATE) {
11815                 WLAN_BSSID_EX *network = &padapter->mlmepriv.cur_network.network;
11816                 start_bss_network(padapter, (u8*)network);
11817                 return H2C_SUCCESS;
11818         }
11819 #endif
11820
11821         //below is for ad-hoc master
11822         if(pparm->network.InfrastructureMode == Ndis802_11IBSS)
11823         {
11824                 rtw_joinbss_reset(padapter);
11825
11826                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
11827                 pmlmeext->cur_ch_offset= HAL_PRIME_CHNL_OFFSET_DONT_CARE;
11828                 pmlmeinfo->ERP_enable = 0;
11829                 pmlmeinfo->WMM_enable = 0;
11830                 pmlmeinfo->HT_enable = 0;
11831                 pmlmeinfo->HT_caps_enable = 0;
11832                 pmlmeinfo->HT_info_enable = 0;
11833                 pmlmeinfo->agg_enable_bitmap = 0;
11834                 pmlmeinfo->candidate_tid_bitmap = 0;
11835
11836                 //disable dynamic functions, such as high power, DIG
11837                 Save_DM_Func_Flag(padapter);
11838                 Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
11839
11840                 //config the initial gain under linking, need to write the BB registers
11841                 //initialgain = 0x1E;
11842                 //rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
11843
11844                 //cancel link timer
11845                 _cancel_timer_ex(&pmlmeext->link_timer);
11846
11847                 //clear CAM
11848                 flush_all_cam_entry(padapter);
11849
11850                 _rtw_memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength));
11851                 pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength;
11852
11853                 if(pnetwork->IELength>MAX_IE_SZ)//Check pbuf->IELength
11854                         return H2C_PARAMETERS_ERROR;
11855
11856                 _rtw_memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength);
11857
11858                 start_create_ibss(padapter);
11859
11860         }
11861
11862         return H2C_SUCCESS;
11863
11864 }
11865
11866 u8 join_cmd_hdl(_adapter *padapter, u8 *pbuf)
11867 {
11868         u8      join_type;
11869         PNDIS_802_11_VARIABLE_IEs       pIE;
11870         struct registry_priv    *pregpriv = &padapter->registrypriv;
11871         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11872         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11873         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
11874 #ifdef CONFIG_ANTENNA_DIVERSITY
11875         struct joinbss_parm     *pparm = (struct joinbss_parm *)pbuf;
11876 #endif //CONFIG_ANTENNA_DIVERSITY
11877         u32 i;
11878         u8      cbw40_enable=0;
11879         //u32   initialgain;
11880         //u32   acparm;
11881         u8 ch, bw, offset;
11882
11883
11884 #if 1
11885         /*
11886          * For safety, prevent from keeping macid sleep.
11887          * If we can sure all power mode enter/leave are paired,
11888          * this check can be removed.
11889          * Lucas@20131113
11890          */
11891         {
11892         struct sta_info *psta;
11893         psta = rtw_get_stainfo(&padapter->stapriv, get_my_bssid(pnetwork));
11894         if (psta)
11895                 rtw_hal_macid_wakeup(padapter, psta->mac_id);
11896         }
11897 #endif
11898
11899 #ifdef CONFIG_BT_COEXIST
11900 {
11901         static u8 bw_mode = 0;
11902
11903         if (0 == bw_mode)
11904         {
11905                 bw_mode = pregpriv->bw_mode; // keep original bw_mode
11906                 DBG_871X(FUNC_ADPT_FMT ": [BTCOEX] Save bw_mode(0x%02x)\n",
11907                         FUNC_ADPT_ARG(padapter), pregpriv->bw_mode);
11908         }
11909
11910         if ((rtw_btcoex_1Ant(padapter) == _TRUE)
11911                 && (rtw_btcoex_IsBtLinkExist(padapter) == _TRUE))
11912         {
11913                 pregpriv->bw_mode = 0x00;
11914                 DBG_871X(FUNC_ADPT_FMT ": [BTCOEX] Change bw_mode to 20MHz(0x%02x)\n",
11915                         FUNC_ADPT_ARG(padapter), pregpriv->bw_mode);
11916         }
11917         else
11918         {
11919                 pregpriv->bw_mode = bw_mode;
11920                 DBG_871X(FUNC_ADPT_FMT ": [BTCOEX] Restore bw_mode(0x%02x)\n",
11921                         FUNC_ADPT_ARG(padapter), pregpriv->bw_mode);
11922         }
11923 }
11924 #endif // CONFIG_BT_COEXIST
11925
11926         //check already connecting to AP or not
11927         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
11928         {
11929                 if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
11930                 {
11931                         issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
11932                 }
11933                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
11934
11935                 //clear CAM
11936                 flush_all_cam_entry(padapter);
11937
11938                 _cancel_timer_ex(&pmlmeext->link_timer);
11939
11940                 //set MSR to nolink -> infra. mode
11941                 //Set_MSR(padapter, _HW_STATE_NOLINK_);
11942                 Set_MSR(padapter, _HW_STATE_STATION_);
11943
11944
11945                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
11946         }
11947
11948 #ifdef CONFIG_ANTENNA_DIVERSITY
11949         rtw_antenna_select_cmd(padapter, pparm->network.PhyInfo.Optimum_antenna, _FALSE);
11950 #endif
11951
11952 #ifdef CONFIG_WAPI_SUPPORT
11953         rtw_wapi_clear_all_cam_entry(padapter);
11954 #endif
11955
11956         rtw_joinbss_reset(padapter);
11957
11958         pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
11959         pmlmeext->cur_ch_offset= HAL_PRIME_CHNL_OFFSET_DONT_CARE;
11960         pmlmeinfo->ERP_enable = 0;
11961         pmlmeinfo->WMM_enable = 0;
11962         pmlmeinfo->HT_enable = 0;
11963         pmlmeinfo->HT_caps_enable = 0;
11964         pmlmeinfo->HT_info_enable = 0;
11965         pmlmeinfo->agg_enable_bitmap = 0;
11966         pmlmeinfo->candidate_tid_bitmap = 0;
11967         pmlmeinfo->bwmode_updated = _FALSE;
11968         //pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX;
11969         pmlmeinfo->VHT_enable = 0;
11970
11971         _rtw_memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength));
11972         pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength;
11973
11974         if(pnetwork->IELength>MAX_IE_SZ)//Check pbuf->IELength
11975                 return H2C_PARAMETERS_ERROR;
11976
11977         _rtw_memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength);
11978
11979         pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
11980         pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
11981
11982         //Check AP vendor to move rtw_joinbss_cmd()
11983         //pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->IEs, pnetwork->IELength);
11984
11985         //sizeof(NDIS_802_11_FIXED_IEs)
11986         for (i = _FIXED_IE_LENGTH_; i < pnetwork->IELength;)
11987         {
11988                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pnetwork->IEs + i);
11989
11990                 switch (pIE->ElementID)
11991                 {
11992                         case _VENDOR_SPECIFIC_IE_://Get WMM IE.
11993                                 if ( _rtw_memcmp(pIE->data, WMM_OUI, 4) )
11994                                 {
11995                                         WMM_param_handler(padapter, pIE);
11996                                 }
11997                                 break;
11998
11999                         case _HT_CAPABILITY_IE_:        //Get HT Cap IE.
12000                                 pmlmeinfo->HT_caps_enable = 1;
12001                                 break;
12002
12003                         case _HT_EXTRA_INFO_IE_:        //Get HT Info IE.
12004 #ifdef CONFIG_80211N_HT
12005                                 pmlmeinfo->HT_info_enable = 1;
12006
12007                                 //spec case only for cisco's ap because cisco's ap issue assoc rsp using mcs rate @40MHz or @20MHz
12008 //#if !defined(CONFIG_CONCURRENT_MODE) && !defined(CONFIG_DUALMAC_CONCURRENT)
12009 //                              if(pmlmeinfo->assoc_AP_vendor == ciscoAP)
12010 //#endif
12011                                 {
12012                                         struct HT_info_element *pht_info = (struct HT_info_element *)(pIE->data);
12013
12014                                         if (pnetwork->Configuration.DSConfig > 14) {
12015                                                 if ((pregpriv->bw_mode >> 4) > CHANNEL_WIDTH_20)
12016                                                         cbw40_enable = 1;
12017                                         } else {
12018                                                 if ((pregpriv->bw_mode & 0x0f) > CHANNEL_WIDTH_20)
12019                                                         cbw40_enable = 1;
12020                                         }
12021
12022                                         if ((cbw40_enable) && (pht_info->infos[0] & BIT(2)))
12023                                         {
12024                                                 //switch to the 40M Hz mode according to the AP
12025                                                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_40;
12026                                                 switch (pht_info->infos[0] & 0x3)
12027                                                 {
12028                                                         case 1:
12029                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
12030                                                                 break;
12031
12032                                                         case 3:
12033                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
12034                                                                 break;
12035
12036                                                         default:
12037                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
12038                                                                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
12039                                                                 break;
12040                                                 }
12041
12042                                                 DBG_871X("set HT ch/bw before connected\n");
12043                                         }
12044                                 }
12045 #endif //CONFIG_80211N_HT
12046                                 break;
12047 #ifdef CONFIG_80211AC_VHT
12048                         case EID_VHTCapability://Get VHT Cap IE.
12049                                 pmlmeinfo->VHT_enable = 1;
12050                                 break;
12051
12052                         case EID_VHTOperation://Get VHT Operation IE.
12053                                 if((GET_VHT_OPERATION_ELE_CHL_WIDTH(pIE->data) >= 1)
12054                                         && ((pregpriv->bw_mode >> 4) >= CHANNEL_WIDTH_80)) {
12055                                         pmlmeext->cur_bwmode = CHANNEL_WIDTH_80;
12056                                         DBG_871X("VHT center ch = %d\n", GET_VHT_OPERATION_ELE_CENTER_FREQ1(pIE->data));
12057                                         DBG_871X("set VHT ch/bw before connected\n");
12058                                 }
12059                                 break;
12060 #endif
12061                         default:
12062                                 break;
12063                 }
12064
12065                 i += (pIE->Length + 2);
12066         }
12067 #if 0
12068         if (padapter->registrypriv.wifi_spec) {
12069                 // for WiFi test, follow WMM test plan spec
12070                 acparm = 0x002F431C; // VO
12071                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
12072                 acparm = 0x005E541C; // VI
12073                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
12074                 acparm = 0x0000A525; // BE
12075                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
12076                 acparm = 0x0000A549; // BK
12077                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
12078
12079                 // for WiFi test, mixed mode with intel STA under bg mode throughput issue
12080                 if (padapter->mlmepriv.htpriv.ht_option == _FALSE){
12081                         acparm = 0x00004320;
12082                         rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
12083                 }
12084         }
12085         else {
12086                 acparm = 0x002F3217; // VO
12087                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
12088                 acparm = 0x005E4317; // VI
12089                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
12090                 acparm = 0x00105320; // BE
12091                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
12092                 acparm = 0x0000A444; // BK
12093                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
12094         }
12095 #endif
12096
12097         /* check channel, bandwidth, offset and switch */
12098 #ifdef CONFIG_DUALMAC_CONCURRENT
12099         if(dc_handle_join_request(padapter, &ch, &bw, &offset) == _FAIL) {
12100                 DBG_871X("dc_handle_join_request fail !!!\n");
12101                 return H2C_SUCCESS;
12102         }
12103 #else //NON CONFIG_DUALMAC_CONCURRENT
12104         if(rtw_chk_start_clnt_join(padapter, &ch, &bw, &offset) == _FAIL) {
12105                 report_join_res(padapter, (-4));
12106                 return H2C_SUCCESS;
12107         }
12108 #endif
12109
12110         //disable dynamic functions, such as high power, DIG
12111         //Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
12112
12113         //config the initial gain under linking, need to write the BB registers
12114         //initialgain = 0x1E;
12115         //rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
12116
12117         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
12118         join_type = 0;
12119         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
12120         rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
12121
12122         set_channel_bwmode(padapter, ch, offset, bw);
12123
12124         //cancel link timer
12125         _cancel_timer_ex(&pmlmeext->link_timer);
12126
12127         start_clnt_join(padapter);
12128
12129         return H2C_SUCCESS;
12130
12131 }
12132
12133 u8 disconnect_hdl(_adapter *padapter, unsigned char *pbuf)
12134 {
12135         struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
12136         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12137         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12138         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
12139         u8 val8;
12140
12141
12142 #if 1
12143         /*
12144          * For safety, prevent from keeping macid sleep.
12145          * If we can sure all power mode enter/leave are paired,
12146          * this check can be removed.
12147          * Lucas@20131113
12148          */
12149         {
12150         struct sta_info *psta;
12151         psta = rtw_get_stainfo(&padapter->stapriv, get_my_bssid(pnetwork));
12152         if (psta)
12153                 rtw_hal_macid_wakeup(padapter, psta->mac_id);
12154         }
12155 #endif
12156
12157         if (is_client_associated_to_ap(padapter))
12158         {
12159 #ifdef CONFIG_DFS
12160                 if(padapter->mlmepriv.handle_dfs == _FALSE)
12161 #endif //CONFIG_DFS
12162 #ifdef CONFIG_PLATFORM_ROCKCHIPS
12163                         //To avoid connecting to AP fail during resume process, change retry count from 5 to 1
12164                         issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
12165 #else
12166                         issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100);
12167 #endif //CONFIG_PLATFORM_ROCKCHIPS
12168         }
12169
12170 #ifdef CONFIG_DFS
12171         if( padapter->mlmepriv.handle_dfs == _TRUE )
12172                 padapter->mlmepriv.handle_dfs = _FALSE;
12173 #endif //CONFIG_DFS
12174
12175         if(((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
12176         {
12177                 //Stop BCN
12178                 val8 = 0;
12179                 rtw_hal_set_hwreg(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8));
12180         }
12181
12182         rtw_mlmeext_disconnect(padapter);
12183
12184         rtw_free_uc_swdec_pending_queue(padapter);
12185
12186         return  H2C_SUCCESS;
12187 }
12188
12189 int rtw_scan_ch_decision(_adapter *padapter, struct rtw_ieee80211_channel *out,
12190         u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
12191 {
12192         int i, j;
12193         int scan_ch_num = 0;
12194         int set_idx;
12195         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12196
12197         /* clear first */
12198         _rtw_memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num);
12199
12200         /* acquire channels from in */
12201         j = 0;
12202         for (i=0;i<in_num;i++) {
12203
12204                 if (0)
12205                 DBG_871X(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[i]));
12206
12207                 if(in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED)
12208                         && (set_idx=rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value)) >=0
12209                         && rtw_mlme_band_check(padapter, in[i].hw_value) == _TRUE
12210                 )
12211                 {
12212                         if (j >= out_num) {
12213                                 DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" out_num:%u not enough\n",
12214                                         FUNC_ADPT_ARG(padapter), out_num);
12215                                 break;
12216                         }
12217
12218                         _rtw_memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel));
12219
12220                         if(pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
12221                                 out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
12222
12223                         j++;
12224                 }
12225                 if(j>=out_num)
12226                         break;
12227         }
12228
12229         /* if out is empty, use channel_set as default */
12230         if(j == 0) {
12231                 for (i=0;i<pmlmeext->max_chan_nums;i++) {
12232
12233                         if (0)
12234                         DBG_871X(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter), pmlmeext->channel_set[i].ChannelNum);
12235
12236                         if (rtw_mlme_band_check(padapter, pmlmeext->channel_set[i].ChannelNum) == _TRUE) {
12237
12238                                 if (j >= out_num) {
12239                                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" out_num:%u not enough\n",
12240                                                 FUNC_ADPT_ARG(padapter), out_num);
12241                                         break;
12242                                 }
12243
12244                                 out[j].hw_value = pmlmeext->channel_set[i].ChannelNum;
12245
12246                                 if(pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
12247                                         out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
12248
12249                                 j++;
12250                         }
12251                 }
12252         }
12253
12254 #ifdef CONFIG_SCAN_SPARSE //partial scan, ASUS RK3188 use the feature
12255         /* assume j>6 is normal scan */
12256         if ((j > 6) && (padapter->registrypriv.wifi_spec != 1))
12257         {
12258                 static u8 token = 0;
12259                 u32 interval;
12260
12261                 if (pmlmeext->last_scan_time == 0)
12262                         pmlmeext->last_scan_time = rtw_get_current_time();
12263
12264                 interval = rtw_get_passing_time_ms(pmlmeext->last_scan_time);
12265                 if ((interval > ALLOW_SCAN_INTERVAL)
12266 #if 0 // Miracast can't do AP scan
12267                         || (padapter->mlmepriv.LinkDetectInfo.bBusyTraffic == _TRUE)
12268 #ifdef CONFIG_CONCURRENT_MODE
12269                         || (padapter->pbuddy_adapter
12270                                 && (padapter->pbuddy_adapter->mlmepriv.LinkDetectInfo.bBusyTraffic == _TRUE))
12271 #endif // CONFIG_CONCURRENT_MODE
12272 #endif
12273                         )
12274                 {
12275                         // modify scan plan
12276                         int k = 0;
12277                         _rtw_memset(in, 0, sizeof(struct rtw_ieee80211_channel)*in_num);
12278                         _rtw_memcpy(in, out, sizeof(struct rtw_ieee80211_channel)*j);
12279                         _rtw_memset(out, 0, sizeof(struct rtw_ieee80211_channel)*j);
12280
12281                         for (i=0;i<j;i++) {
12282                                 if (in[i].hw_value && (i%SCAN_DIVISION_NUM) == token) {
12283                                         _rtw_memcpy(&out[k], &in[i], sizeof(struct rtw_ieee80211_channel));
12284                                         k++;
12285                                 }
12286                                 if(k>=out_num)
12287                                         break;
12288                         }
12289
12290                         j = k;
12291                         token  = (token+1)%SCAN_DIVISION_NUM;
12292                 }
12293
12294                 pmlmeext->last_scan_time = rtw_get_current_time();
12295         }
12296 #endif //CONFIG_SCAN_SPARSE
12297
12298         return j;
12299 }
12300
12301 u8 sitesurvey_cmd_hdl(_adapter *padapter, u8 *pbuf)
12302 {
12303         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12304         struct sitesurvey_parm  *pparm = (struct sitesurvey_parm *)pbuf;
12305         u8      bdelayscan = _FALSE;
12306         u8      val8;
12307         u32     initialgain;
12308         u32     i;
12309         struct dvobj_priv *psdpriv = padapter->dvobj;
12310         struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
12311
12312 #ifdef CONFIG_P2P
12313         struct wifidirect_info* pwdinfo = &padapter->wdinfo;
12314 #endif
12315 #ifdef DBG_CHECK_FW_PS_STATE
12316         if(rtw_fw_ps_state(padapter) == _FAIL)
12317         {
12318                 DBG_871X("scan without leave 32k\n");
12319                 pdbgpriv->dbg_scan_pwr_state_cnt++;
12320         }
12321 #endif //DBG_CHECK_FW_PS_STATE
12322
12323         if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE)
12324         {
12325 #ifdef CONFIG_CONCURRENT_MODE
12326                 //for first time sitesurvey_cmd
12327                 rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
12328 #endif //CONFIG_CONCURRENT_MODE
12329
12330                 pmlmeext->sitesurvey_res.state = SCAN_START;
12331                 pmlmeext->sitesurvey_res.bss_cnt = 0;
12332                 pmlmeext->sitesurvey_res.channel_idx = 0;
12333
12334                 for(i=0;i<RTW_SSID_SCAN_AMOUNT;i++){
12335                         if(pparm->ssid[i].SsidLength) {
12336                                 _rtw_memcpy(pmlmeext->sitesurvey_res.ssid[i].Ssid, pparm->ssid[i].Ssid, IW_ESSID_MAX_SIZE);
12337                                 pmlmeext->sitesurvey_res.ssid[i].SsidLength= pparm->ssid[i].SsidLength;
12338                         } else {
12339                                 pmlmeext->sitesurvey_res.ssid[i].SsidLength= 0;
12340                         }
12341                 }
12342
12343                 pmlmeext->sitesurvey_res.ch_num = rtw_scan_ch_decision(padapter
12344                         , pmlmeext->sitesurvey_res.ch, RTW_CHANNEL_SCAN_AMOUNT
12345                         , pparm->ch, pparm->ch_num
12346                 );
12347
12348                 pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
12349
12350 #ifdef CONFIG_DUALMAC_CONCURRENT
12351                 bdelayscan = dc_handle_site_survey(padapter);
12352 #endif
12353
12354                 //issue null data if associating to the AP
12355                 if (is_client_associated_to_ap(padapter) == _TRUE)
12356                 {
12357                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
12358
12359                         issue_nulldata(padapter, NULL, 1, 3, 500);
12360
12361 #ifdef CONFIG_CONCURRENT_MODE
12362                         if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
12363                         {
12364                                 DBG_871X("adapter is scanning(buddy_adapter is linked), issue nulldata(pwrbit=1)\n");
12365
12366                                 issue_nulldata(padapter->pbuddy_adapter, NULL, 1, 3, 500);
12367                         }
12368 #endif
12369                         bdelayscan = _TRUE;
12370                 }
12371 #ifdef CONFIG_CONCURRENT_MODE
12372                 else if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
12373                 {
12374                         #ifdef CONFIG_TDLS
12375                         if(padapter->pbuddy_adapter->wdinfo.wfd_tdls_enable == 1)
12376                         {
12377                                 issue_tunneled_probe_req(padapter->pbuddy_adapter);
12378                         }
12379                         #endif //CONFIG_TDLS
12380
12381                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
12382
12383                         issue_nulldata(padapter->pbuddy_adapter, NULL, 1, 3, 500);
12384
12385                         bdelayscan = _TRUE;
12386                 }
12387 #endif
12388                 if(bdelayscan)
12389                 {
12390                         //delay 50ms to protect nulldata(1).
12391                         set_survey_timer(pmlmeext, 50);
12392                         return H2C_SUCCESS;
12393                 }
12394         }
12395
12396         if ((pmlmeext->sitesurvey_res.state == SCAN_START) || (pmlmeext->sitesurvey_res.state == SCAN_TXNULL))
12397         {
12398 #ifdef CONFIG_FIND_BEST_CHANNEL
12399 #if 0
12400                 for (i=0; pmlmeext->channel_set[i].ChannelNum !=0; i++) {
12401                         pmlmeext->channel_set[i].rx_count = 0;
12402                 }
12403 #endif
12404 #endif /* CONFIG_FIND_BEST_CHANNEL */
12405
12406                 //disable dynamic functions, such as high power, DIG
12407                 Save_DM_Func_Flag(padapter);
12408                 Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
12409
12410                 //config the initial gain under scaning, need to write the BB registers
12411 #ifdef CONFIG_P2P
12412 #ifdef CONFIG_IOCTL_CFG80211
12413                 if(adapter_wdev_data(padapter)->p2p_enabled == _TRUE && pwdinfo->driver_interface == DRIVER_CFG80211 )
12414                         initialgain = 0x30;
12415                 else
12416 #endif //CONFIG_IOCTL_CFG80211
12417                 if ( !rtw_p2p_chk_state( pwdinfo, P2P_STATE_NONE ) )
12418                         initialgain = 0x28;
12419                 else
12420 #endif //CONFIG_P2P
12421                         initialgain = 0x1e;
12422
12423                 rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
12424
12425                 //set MSR to no link state
12426                 Set_MSR(padapter, _HW_STATE_NOLINK_);
12427
12428                 val8 = 1; //under site survey
12429                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
12430
12431                 pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
12432         }
12433
12434         site_survey(padapter);
12435
12436         return H2C_SUCCESS;
12437
12438 }
12439
12440 u8 setauth_hdl(_adapter *padapter, unsigned char *pbuf)
12441 {
12442         struct setauth_parm             *pparm = (struct setauth_parm *)pbuf;
12443         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12444         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12445
12446         if (pparm->mode < 4)
12447         {
12448                 pmlmeinfo->auth_algo = pparm->mode;
12449         }
12450
12451         return  H2C_SUCCESS;
12452 }
12453
12454 u8 setkey_hdl(_adapter *padapter, u8 *pbuf)
12455 {
12456         unsigned short                          ctrl;
12457         struct setkey_parm              *pparm = (struct setkey_parm *)pbuf;
12458         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12459         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12460         unsigned char                                   null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
12461
12462         //main tx key for wep.
12463         if(pparm->set_tx)
12464                 pmlmeinfo->key_index = pparm->keyid;
12465
12466         //write cam
12467         ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;
12468
12469         DBG_871X_LEVEL(_drv_always_, "set group key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) "
12470                         "keyid:%d\n", pparm->algorithm, pparm->keyid);
12471         write_cam(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
12472
12473         //allow multicast packets to driver
12474         padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_ON_RCR_AM, null_addr);
12475
12476         return H2C_SUCCESS;
12477 }
12478
12479 u8 set_stakey_hdl(_adapter *padapter, u8 *pbuf)
12480 {
12481         u16 ctrl=0;
12482         u8 cam_id = 0;//cam_entry
12483         u8 ret = H2C_SUCCESS;
12484         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12485         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12486         struct set_stakey_parm  *pparm = (struct set_stakey_parm *)pbuf;
12487         struct sta_priv *pstapriv = &padapter->stapriv;
12488         struct sta_info *psta;
12489 #ifdef CONFIG_TDLS
12490         struct tdls_info        *ptdlsinfo = &padapter->tdlsinfo;
12491 #endif //CONFIG_TDLS
12492
12493         //cam_entry:
12494         //0~3 for default key
12495
12496         //for concurrent mode (ap+sta, sta+sta):
12497         //default key is disable, using sw encrypt/decrypt
12498         //camid 0, 1, 2, 3 is default entry for default key/group key
12499         //macid = 1 is for bc/mc stainfo, no mapping to camid
12500         //macid = 0 mapping to camid 4
12501         //for macid >=2, camid = macid+3;
12502
12503
12504         if(pparm->algorithm == _NO_PRIVACY_)    // clear cam entry
12505         {
12506                 clear_cam_entry(padapter, pparm->id);
12507                 ret = H2C_SUCCESS;
12508                 goto exit_set_stakey_hdl;
12509         }
12510
12511         if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
12512         {
12513                 psta = rtw_get_stainfo(pstapriv, pparm->addr);
12514                 if(psta)
12515                 {
12516                         ctrl = (BIT(15) | ((pparm->algorithm) << 2));
12517
12518                         DBG_871X("r871x_set_stakey_hdl(): enc_algorithm=%d\n", pparm->algorithm);
12519
12520                         if((psta->mac_id == 1) || (psta->mac_id>(NUM_STA-4)))
12521                         {
12522                                 DBG_871X("r871x_set_stakey_hdl():set_stakey failed, mac_id(aid)=%d\n", psta->mac_id);
12523                                 ret = H2C_REJECTED;
12524                                 goto exit_set_stakey_hdl;
12525                         }
12526
12527                         cam_id = (u8)rtw_get_camid(psta->mac_id);//0~3 for default key, cmd_id=macid + 3;
12528
12529                         DBG_871X("Write CAM, mac_addr=%x:%x:%x:%x:%x:%x, cam_entry=%d\n", pparm->addr[0],
12530                                                 pparm->addr[1], pparm->addr[2], pparm->addr[3], pparm->addr[4],
12531                                                 pparm->addr[5], cam_id);
12532
12533                         write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
12534
12535                         ret = H2C_SUCCESS_RSP;
12536                         goto exit_set_stakey_hdl;
12537
12538                 }
12539                 else
12540                 {
12541                         DBG_871X("r871x_set_stakey_hdl(): sta has been free\n");
12542                         ret = H2C_REJECTED;
12543                         goto exit_set_stakey_hdl;
12544                 }
12545
12546         }
12547
12548
12549         //below for sta mode
12550
12551         if((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)))
12552                 cam_id = (u8)rtw_get_camid(psta->mac_id);
12553         else
12554                 cam_id = 4;
12555
12556         ctrl = BIT(15) | ((pparm->algorithm) << 2);
12557
12558 #ifdef CONFIG_TDLS
12559         if(ptdlsinfo->clear_cam!=0){
12560                 clear_cam_entry(padapter, ptdlsinfo->clear_cam);
12561                 ptdlsinfo->clear_cam=0;
12562                 ret = H2C_SUCCESS;
12563                 goto exit_set_stakey_hdl;
12564         }
12565
12566         psta = rtw_get_stainfo(pstapriv, pparm->addr);//Get TDLS Peer STA
12567         if( psta->tdls_sta_state&TDLS_LINKED_STATE ){
12568                 write_cam(padapter, psta->mac_id, ctrl, pparm->addr, pparm->key);
12569         }
12570         else
12571 #endif //CONFIG_TDLS
12572                 write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
12573
12574         pmlmeinfo->enc_algo = pparm->algorithm;
12575
12576 exit_set_stakey_hdl:
12577
12578         DBG_871X_LEVEL(_drv_always_, "set pairwise key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) camid:%d\n",
12579                         pparm->algorithm, cam_id);
12580
12581         return ret;
12582
12583 }
12584
12585 u8 add_ba_hdl(_adapter *padapter, unsigned char *pbuf)
12586 {
12587         struct addBaReq_parm    *pparm = (struct addBaReq_parm *)pbuf;
12588         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12589         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12590
12591         struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr);
12592
12593         if(!psta)
12594                 return  H2C_SUCCESS;
12595
12596 #ifdef CONFIG_80211N_HT
12597         if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
12598                 ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
12599         {
12600                 //pmlmeinfo->ADDBA_retry_count = 0;
12601                 //pmlmeinfo->candidate_tid_bitmap |= (0x1 << pparm->tid);
12602                 //psta->htpriv.candidate_tid_bitmap |= BIT(pparm->tid);
12603                 issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
12604                 //_set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO);
12605                 _set_timer(&psta->addba_retry_timer, ADDBA_TO);
12606         }
12607 #ifdef CONFIG_TDLS
12608         else if((psta->tdls_sta_state & TDLS_LINKED_STATE)&&
12609                 (psta->htpriv.ht_option==_TRUE) &&
12610                 (psta->htpriv.ampdu_enable==_TRUE) )
12611         {
12612                 issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
12613                 //_set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO);
12614                 _set_timer(&psta->addba_retry_timer, ADDBA_TO);
12615         }
12616 #endif //CONFIG
12617         else
12618         {
12619                 psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
12620         }
12621 #endif //CONFIG_80211N_HT
12622         return  H2C_SUCCESS;
12623 }
12624
12625 u8 set_tx_beacon_cmd(_adapter* padapter)
12626 {
12627         struct cmd_obj  *ph2c;
12628         struct Tx_Beacon_param  *ptxBeacon_parm;
12629         struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
12630         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12631         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12632         u8      res = _SUCCESS;
12633         int len_diff = 0;
12634
12635 _func_enter_;
12636
12637         if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
12638         {
12639                 res= _FAIL;
12640                 goto exit;
12641         }
12642
12643         if ((ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param))) == NULL)
12644         {
12645                 rtw_mfree((unsigned char *)ph2c, sizeof(struct  cmd_obj));
12646                 res= _FAIL;
12647                 goto exit;
12648         }
12649
12650         _rtw_memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
12651
12652         len_diff = update_hidden_ssid(
12653                 ptxBeacon_parm->network.IEs+_BEACON_IE_OFFSET_
12654                 , ptxBeacon_parm->network.IELength-_BEACON_IE_OFFSET_
12655                 , pmlmeinfo->hidden_ssid_mode
12656         );
12657         ptxBeacon_parm->network.IELength += len_diff;
12658
12659         init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
12660
12661         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
12662
12663
12664 exit:
12665
12666 _func_exit_;
12667
12668         return res;
12669 }
12670
12671
12672 u8 mlme_evt_hdl(_adapter *padapter, unsigned char *pbuf)
12673 {
12674         u8 evt_code, evt_seq;
12675         u16 evt_sz;
12676         uint    *peventbuf;
12677         void (*event_callback)(_adapter *dev, u8 *pbuf);
12678         struct evt_priv *pevt_priv = &(padapter->evtpriv);
12679
12680         peventbuf = (uint*)pbuf;
12681         evt_sz = (u16)(*peventbuf&0xffff);
12682         evt_seq = (u8)((*peventbuf>>24)&0x7f);
12683         evt_code = (u8)((*peventbuf>>16)&0xff);
12684
12685
12686         #ifdef CHECK_EVENT_SEQ
12687         // checking event sequence...
12688         if (evt_seq != (ATOMIC_READ(&pevt_priv->event_seq) & 0x7f) )
12689         {
12690                 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)));
12691
12692                 pevt_priv->event_seq = (evt_seq+1)&0x7f;
12693
12694                 goto _abort_event_;
12695         }
12696         #endif
12697
12698         // checking if event code is valid
12699         if (evt_code >= MAX_C2HEVT)
12700         {
12701                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nEvent Code(%d) mismatch!\n", evt_code));
12702                 goto _abort_event_;
12703         }
12704
12705         // checking if event size match the event parm size
12706         if ((wlanevents[evt_code].parmsize != 0) &&
12707                         (wlanevents[evt_code].parmsize != evt_sz))
12708         {
12709
12710                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
12711                         evt_code, wlanevents[evt_code].parmsize, evt_sz));
12712                 goto _abort_event_;
12713
12714         }
12715
12716         ATOMIC_INC(&pevt_priv->event_seq);
12717
12718         peventbuf += 2;
12719
12720         if(peventbuf)
12721         {
12722                 event_callback = wlanevents[evt_code].event_callback;
12723                 event_callback(padapter, (u8*)peventbuf);
12724
12725                 pevt_priv->evt_done_cnt++;
12726         }
12727
12728
12729 _abort_event_:
12730
12731
12732         return H2C_SUCCESS;
12733
12734 }
12735
12736 u8 h2c_msg_hdl(_adapter *padapter, unsigned char *pbuf)
12737 {
12738         if(!pbuf)
12739                 return H2C_PARAMETERS_ERROR;
12740
12741         return H2C_SUCCESS;
12742 }
12743
12744 u8 tx_beacon_hdl(_adapter *padapter, unsigned char *pbuf)
12745 {
12746         if(send_beacon(padapter)==_FAIL)
12747         {
12748                 DBG_871X("issue_beacon, fail!\n");
12749                 return H2C_PARAMETERS_ERROR;
12750         }
12751 #ifdef CONFIG_AP_MODE
12752         else //tx bc/mc frames after update TIM
12753         {
12754                 _irqL irqL;
12755                 struct sta_info *psta_bmc;
12756                 _list   *xmitframe_plist, *xmitframe_phead;
12757                 struct xmit_frame *pxmitframe=NULL;
12758                 struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
12759                 struct sta_priv  *pstapriv = &padapter->stapriv;
12760
12761                 //for BC/MC Frames
12762                 psta_bmc = rtw_get_bcmc_stainfo(padapter);
12763                 if(!psta_bmc)
12764                         return H2C_SUCCESS;
12765
12766                 if((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len>0))
12767                 {
12768 #ifndef CONFIG_PCI_HCI
12769                         rtw_msleep_os(10);// 10ms, ATIM(HIQ) Windows
12770 #endif
12771                         //_enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
12772                         _enter_critical_bh(&pxmitpriv->lock, &irqL);
12773
12774                         xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
12775                         xmitframe_plist = get_next(xmitframe_phead);
12776
12777                         while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
12778                         {
12779                                 pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
12780
12781                                 xmitframe_plist = get_next(xmitframe_plist);
12782
12783                                 rtw_list_delete(&pxmitframe->list);
12784
12785                                 psta_bmc->sleepq_len--;
12786                                 if(psta_bmc->sleepq_len>0)
12787                                         pxmitframe->attrib.mdata = 1;
12788                                 else
12789                                         pxmitframe->attrib.mdata = 0;
12790
12791                                 pxmitframe->attrib.triggered=1;
12792
12793                                 pxmitframe->attrib.qsel = 0x11;//HIQ
12794
12795 #if 0
12796                                 _exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
12797                                 if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE)
12798                                 {
12799                                         rtw_os_xmit_complete(padapter, pxmitframe);
12800                                 }
12801                                 _enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
12802
12803 #endif
12804                                 rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
12805
12806                                 //pstapriv->tim_bitmap &= ~BIT(0);
12807
12808                         }
12809
12810                         //_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
12811                         _exit_critical_bh(&pxmitpriv->lock, &irqL);
12812
12813 //#if defined(CONFIG_PCI_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
12814 #if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
12815                         rtw_chk_hi_queue_cmd(padapter);
12816 #endif
12817
12818                 }
12819
12820         }
12821 #endif
12822
12823         return H2C_SUCCESS;
12824
12825 }
12826
12827 void change_band_update_ie(_adapter *padapter, WLAN_BSSID_EX *pnetwork)
12828 {
12829         u8      network_type,rate_len, total_rate_len,remainder_rate_len;
12830         struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
12831         struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
12832         u8      erpinfo=0x4;
12833
12834         //DBG_871X("%s\n", __FUNCTION__);
12835
12836         if(pmlmeext->cur_channel >= 36)
12837         {
12838                 network_type = WIRELESS_11A;
12839                 total_rate_len = IEEE80211_NUM_OFDM_RATESLEN;
12840                 DBG_871X("%s(): change to 5G Band\n",__FUNCTION__);
12841                 rtw_remove_bcn_ie(padapter, pnetwork, _ERPINFO_IE_);
12842         }
12843         else
12844         {
12845                 network_type = WIRELESS_11BG;
12846                 total_rate_len = IEEE80211_CCK_RATE_LEN+IEEE80211_NUM_OFDM_RATESLEN;
12847                 DBG_871X("%s(): change to 2.4G Band\n",__FUNCTION__);
12848                 rtw_add_bcn_ie(padapter, pnetwork, _ERPINFO_IE_, &erpinfo, 1);
12849         }
12850
12851         rtw_set_supported_rate(pnetwork->SupportedRates, network_type);
12852
12853         UpdateBrateTbl(padapter, pnetwork->SupportedRates);
12854         rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
12855
12856         if(total_rate_len > 8)
12857         {
12858                 rate_len = 8;
12859                 remainder_rate_len = total_rate_len - 8;
12860         }
12861         else
12862         {
12863                 rate_len = total_rate_len;
12864                 remainder_rate_len = 0;
12865         }
12866
12867         rtw_add_bcn_ie(padapter, pnetwork, _SUPPORTEDRATES_IE_, pnetwork->SupportedRates, rate_len);
12868
12869         if(remainder_rate_len)
12870         {
12871                 rtw_add_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_, (pnetwork->SupportedRates+8), remainder_rate_len);
12872         }
12873         else
12874         {
12875                 rtw_remove_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_);
12876         }
12877 }
12878
12879 #ifdef CONFIG_DUALMAC_CONCURRENT
12880 void dc_SelectChannel(_adapter *padapter, unsigned char channel)
12881 {
12882         PADAPTER ptarget_adapter;
12883
12884         if( (padapter->pbuddy_adapter != NULL) &&
12885                 (padapter->DualMacConcurrent == _TRUE) &&
12886                 (padapter->adapter_type == SECONDARY_ADAPTER))
12887         {
12888                 // only mac0 could control BB&RF
12889                 ptarget_adapter = padapter->pbuddy_adapter;
12890         }
12891         else
12892         {
12893                 ptarget_adapter = padapter;
12894         }
12895
12896         _enter_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setch_mutex), NULL);
12897
12898         rtw_hal_set_chan(ptarget_adapter, channel);
12899
12900         _exit_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setch_mutex), NULL);
12901 }
12902
12903 void dc_SetBWMode(_adapter *padapter, unsigned short bwmode, unsigned char channel_offset)
12904 {
12905         PADAPTER ptarget_adapter;
12906
12907         if( (padapter->pbuddy_adapter != NULL) &&
12908                 (padapter->DualMacConcurrent == _TRUE) &&
12909                 (padapter->adapter_type == SECONDARY_ADAPTER))
12910         {
12911                 // only mac0 could control BB&RF
12912                 ptarget_adapter = padapter->pbuddy_adapter;
12913         }
12914         else
12915         {
12916                 ptarget_adapter = padapter;
12917         }
12918
12919         _enter_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setbw_mutex), NULL);
12920
12921         rtw_hal_set_bwmode(ptarget_adapter, (CHANNEL_WIDTH)bwmode, channel_offset);
12922
12923         _exit_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setbw_mutex), NULL);
12924 }
12925
12926 void dc_set_channel_bwmode_disconnect(_adapter *padapter)
12927 {
12928         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12929         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
12930         struct mlme_priv *pbuddy_mlmepriv = NULL;
12931
12932         if(pbuddy_adapter != NULL &&
12933                 padapter->DualMacConcurrent == _TRUE)
12934         {
12935                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
12936                 if((check_fwstate(pbuddy_mlmepriv, _FW_LINKED)) != _TRUE)
12937                 {
12938                         //switch to the 20M Hz mode after disconnect
12939                         pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
12940                         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
12941
12942                         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
12943                 }
12944         }
12945         else
12946         {
12947                 //switch to the 20M Hz mode after disconnect
12948                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
12949                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
12950
12951                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
12952         }
12953 }
12954
12955 u8 dc_handle_join_request(_adapter *padapter, u8 *ch, u8 *bw, u8 *offset)
12956 {
12957         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12958         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12959         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
12960         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
12961         struct mlme_ext_priv *pbuddy_mlmeext = NULL;
12962         struct mlme_priv        *pbuddy_mlmepriv = NULL;
12963         u8      ret = _SUCCESS;
12964
12965         if(pbuddy_adapter != NULL &&
12966                 padapter->DualMacConcurrent == _TRUE)
12967         {
12968                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
12969                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
12970
12971                 if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel ||
12972                         pmlmeext->cur_bwmode != pbuddy_mlmeext->cur_bwmode ||
12973                         pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
12974                 {
12975                         if((check_fwstate(pbuddy_mlmepriv, WIFI_AP_STATE)) == _TRUE)
12976                         {
12977                                 //issue deauth to all stas if if2 is at ap mode
12978                                 rtw_sta_flush(pbuddy_adapter);
12979
12980                                 //rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
12981                                 rtw_hal_set_hwreg(pbuddy_adapter, HW_VAR_CHECK_TXBUF, 0);
12982                         }
12983                         else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED) == _TRUE)
12984                         {
12985                                 if(pmlmeext->cur_channel == pbuddy_mlmeext->cur_channel)
12986                                 {
12987                                         // CHANNEL_WIDTH_40 or CHANNEL_WIDTH_20 but channel offset is different
12988                                         if((pmlmeext->cur_bwmode == pbuddy_mlmeext->cur_bwmode) &&
12989                                                 (pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset) )
12990                                         {
12991                                                 report_join_res(padapter, -4);
12992                                                 ret = _FAIL;
12993                                         }
12994                                 }
12995                                 else
12996                                 {
12997                                         report_join_res(padapter, -4);
12998                                         ret = _FAIL;
12999                                 }
13000                         }
13001                 }
13002                 else     if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
13003                 {
13004                         issue_nulldata(pbuddy_adapter, NULL, 1, 0, 0);
13005                 }
13006         }
13007
13008         if (!ch || !bw || !offset) {
13009                 rtw_warn_on(1);
13010                 ret = _FAIL;
13011         }
13012
13013         if (ret == _SUCCESS) {
13014                 *ch = pmlmeext->cur_channel;
13015                 *bw = pmlmeext->cur_bwmode;
13016                 *offset = pmlmeext->cur_ch_offset;
13017         }
13018
13019 exit:
13020         return ret;
13021 }
13022
13023 void dc_handle_join_done(_adapter *padapter, u8 join_res)
13024 {
13025         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13026         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13027         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13028         struct mlme_priv *pbuddy_mlmepriv = NULL;
13029         struct mlme_ext_priv *pbuddy_mlmeext = NULL;
13030         struct mlme_ext_info *pbuddy_mlmeinfo = NULL;
13031         WLAN_BSSID_EX *pbuddy_network_mlmeext = NULL;
13032         u8      change_band = _FALSE;
13033
13034
13035         if(pbuddy_adapter != NULL &&
13036                 padapter->DualMacConcurrent == _TRUE)
13037         {
13038                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13039                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13040                 pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
13041                 pbuddy_network_mlmeext = &(pbuddy_mlmeinfo->network);
13042
13043                 if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
13044                                 check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
13045                 {
13046                         //restart and update beacon
13047                         DBG_871X("after join, current adapter, CH=%d, BW=%d, offset=%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
13048
13049                         if(join_res >= 0)
13050                         {
13051                                 u8 *p;
13052                                 int     ie_len;
13053                                 struct HT_info_element *pht_info=NULL;
13054
13055                                 if((pbuddy_mlmeext->cur_channel <= 14 && pmlmeext->cur_channel >= 36) ||
13056                                         (pbuddy_mlmeext->cur_channel >= 36 && pmlmeext->cur_channel <= 14))
13057                                 {
13058                                         change_band = _TRUE;
13059                                 }
13060
13061                                 //sync channel/bwmode/ch_offset with another adapter
13062                                 pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
13063
13064                                 if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40)
13065                                 {
13066                                         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)));
13067                                         if( p && ie_len)
13068                                         {
13069                                                 pht_info = (struct HT_info_element *)(p+2);
13070                                                 pht_info->infos[0] &= ~(BIT(0)|BIT(1)); //no secondary channel is present
13071                                         }
13072
13073                                         if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_40)
13074                                         {
13075                                                 pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
13076
13077                                                 //to update cur_ch_offset value in beacon
13078                                                 if( pht_info )
13079                                                 {
13080                                                         switch(pmlmeext->cur_ch_offset)
13081                                                         {
13082                                                                 case HAL_PRIME_CHNL_OFFSET_LOWER:
13083                                                                         pht_info->infos[0] |= 0x1;
13084                                                                         break;
13085                                                                 case HAL_PRIME_CHNL_OFFSET_UPPER:
13086                                                                         pht_info->infos[0] |= 0x3;
13087                                                                         break;
13088                                                                 case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
13089                                                                 default:
13090                                                                         break;
13091                                                         }
13092                                                 }
13093                                         }
13094                                         else if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_20)
13095                                         {
13096                                                 pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_20;
13097                                                 pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13098
13099                                                 if(pmlmeext->cur_channel>0 && pmlmeext->cur_channel<5)
13100                                                 {
13101                                                         if(pht_info)
13102                                                                 pht_info->infos[0] |= 0x1;
13103
13104                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
13105                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
13106                                                 }
13107
13108                                                 if(pmlmeext->cur_channel>7 && pmlmeext->cur_channel<(14+1))
13109                                                 {
13110                                                         if(pht_info)
13111                                                                 pht_info->infos[0] |= 0x3;
13112
13113                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
13114                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
13115                                                 }
13116
13117                                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13118                                         }
13119                                 }
13120
13121                                 // to update channel value in beacon
13122                                 pbuddy_network_mlmeext->Configuration.DSConfig = pmlmeext->cur_channel;
13123                                 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)));
13124                                 if(p && ie_len>0)
13125                                         *(p + 2) = pmlmeext->cur_channel;
13126
13127                                 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)));
13128                                 if( p && ie_len)
13129                                 {
13130                                         pht_info = (struct HT_info_element *)(p+2);
13131                                         pht_info->primary_channel = pmlmeext->cur_channel;
13132                                 }
13133
13134                                 // update mlmepriv's cur_network
13135                                 _rtw_memcpy(&pbuddy_mlmepriv->cur_network.network, pbuddy_network_mlmeext, pbuddy_network_mlmeext->Length);
13136                         }
13137                         else
13138                         {
13139                                 // switch back to original channel/bwmode/ch_offset;
13140                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13141                         }
13142
13143                         DBG_871X("after join, another adapter, CH=%d, BW=%d, offset=%d\n", pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
13144
13145                         if(change_band == _TRUE)
13146                                 change_band_update_ie(pbuddy_adapter, pbuddy_network_mlmeext);
13147
13148                         DBG_871X("update pbuddy_adapter's beacon\n");
13149
13150                         update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
13151                 }
13152                 else     if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
13153                 {
13154                         if((pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
13155                                 (pmlmeext->cur_bwmode == CHANNEL_WIDTH_20))
13156                         {
13157                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13158                         }
13159
13160                         issue_nulldata(pbuddy_adapter, NULL, 0, 0, 0);
13161                 }
13162         }
13163 }
13164
13165 sint    dc_check_fwstate(_adapter *padapter, sint fw_state)
13166 {
13167         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13168         struct mlme_priv *pbuddy_mlmepriv = NULL;
13169
13170         if(padapter->pbuddy_adapter != NULL &&
13171                 padapter->DualMacConcurrent == _TRUE)
13172
13173         {
13174                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13175
13176                 return check_fwstate(pbuddy_mlmepriv, fw_state);
13177         }
13178
13179         return _FALSE;
13180 }
13181
13182 u8 dc_handle_site_survey(_adapter *padapter)
13183 {
13184         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13185         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13186         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13187
13188         // only mac0 can do scan request, help issue nulldata(1) for mac1
13189         if(pbuddy_adapter != NULL &&
13190                 padapter->DualMacConcurrent == _TRUE)
13191         {
13192                 if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
13193                 {
13194                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
13195
13196                         issue_nulldata(pbuddy_adapter, NULL, 1, 2, 0);
13197
13198                         return _TRUE;
13199                 }
13200         }
13201
13202         return _FALSE;
13203 }
13204
13205 void    dc_report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
13206 {
13207         if(padapter->pbuddy_adapter != NULL &&
13208                 padapter->DualMacConcurrent == _TRUE)
13209         {
13210                 report_survey_event(padapter->pbuddy_adapter, precv_frame);
13211         }
13212 }
13213
13214 void dc_set_channel_bwmode_survey_done(_adapter *padapter)
13215 {
13216         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
13217         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13218         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13219         struct mlme_priv *pbuddy_mlmepriv = NULL;
13220         struct mlme_ext_priv *pbuddy_mlmeext = NULL;
13221         struct mlme_ext_info *pbuddy_mlmeinfo = NULL;
13222         u8 cur_channel;
13223         u8 cur_bwmode;
13224         u8 cur_ch_offset;
13225
13226         if(pbuddy_adapter != NULL &&
13227                 padapter->DualMacConcurrent == _TRUE)
13228         {
13229                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13230                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13231                 pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
13232
13233                 if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
13234                 {
13235                         if(check_fwstate(pmlmepriv, _FW_LINKED) &&
13236                                 (pmlmeext->cur_bwmode == CHANNEL_WIDTH_40))
13237                         {
13238                                 cur_channel = pmlmeext->cur_channel;
13239                                 cur_bwmode = pmlmeext->cur_bwmode;
13240                                 cur_ch_offset = pmlmeext->cur_ch_offset;
13241                         }
13242                         else
13243                         {
13244                                 cur_channel = pbuddy_mlmeext->cur_channel;
13245                                 cur_bwmode = pbuddy_mlmeext->cur_bwmode;
13246                                 cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
13247                         }
13248                 }
13249                 else
13250                 {
13251                         cur_channel = pmlmeext->cur_channel;
13252                         cur_bwmode = pmlmeext->cur_bwmode;
13253                         cur_ch_offset = pmlmeext->cur_ch_offset;
13254                 }
13255
13256                 set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
13257
13258                 if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
13259                 {
13260                         //issue null data
13261                         issue_nulldata(pbuddy_adapter, NULL, 0, 0, 0);
13262                 }
13263
13264                 if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
13265                         check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
13266                 {
13267
13268                         DBG_871X("survey done, current CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
13269
13270                         DBG_871X("restart pbuddy_adapter's beacon\n");
13271
13272                         update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
13273                 }
13274         }
13275         else
13276         {
13277                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
13278         }
13279 }
13280
13281 void dc_set_ap_channel_bandwidth(_adapter *padapter, u8 channel, u8 channel_offset, u8 bwmode)
13282 {
13283         u8      *p;
13284         u8      val8, cur_channel, cur_bwmode, cur_ch_offset, change_band;
13285         int     ie_len;
13286         struct registry_priv    *pregpriv = &padapter->registrypriv;
13287         struct mlme_priv                *pmlmepriv = &(padapter->mlmepriv);
13288         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;
13289         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
13290         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13291         struct HT_info_element  *pht_info=NULL;
13292         _adapter        *pbuddy_adapter = padapter->pbuddy_adapter;
13293         struct mlme_priv        *pbuddy_mlmepriv = NULL;
13294         struct mlme_ext_priv    *pbuddy_mlmeext = NULL;
13295
13296         DBG_871X("dualmac_concurrent_ap_set_channel_bwmode ==>\n");
13297
13298         cur_channel = channel;
13299         cur_bwmode = bwmode;
13300         cur_ch_offset = channel_offset;
13301         change_band = _FALSE;
13302
13303         p = rtw_get_ie((pnetwork->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
13304         if( p && ie_len)
13305         {
13306                 pht_info = (struct HT_info_element *)(p+2);
13307         }
13308
13309         if(pbuddy_adapter != NULL &&
13310                 padapter->DualMacConcurrent == _TRUE)
13311         {
13312                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13313                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13314
13315                 if(!check_fwstate(pbuddy_mlmepriv, _FW_LINKED|_FW_UNDER_LINKING|_FW_UNDER_SURVEY))
13316                 {
13317                         set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
13318                 }
13319                 else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED)==_TRUE)
13320                 {
13321                         //To sync cur_channel/cur_bwmode/cur_ch_offset with another adapter
13322                         DBG_871X("Another iface is at linked state, sync cur_channel/cur_bwmode/cur_ch_offset\n");
13323                         DBG_871X("Another adapter, CH=%d, BW=%d, offset=%d\n", pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
13324                         DBG_871X("Current adapter, CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
13325
13326                         cur_channel = pbuddy_mlmeext->cur_channel;
13327                         if(cur_bwmode == CHANNEL_WIDTH_40)
13328                         {
13329                                 if(pht_info)
13330                                         pht_info->infos[0] &= ~(BIT(0)|BIT(1));
13331
13332                                 if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40)
13333                                 {
13334                                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
13335
13336                                         //to update cur_ch_offset value in beacon
13337                                         if(pht_info)
13338                                         {
13339                                                 switch(cur_ch_offset)
13340                                                 {
13341                                                         case HAL_PRIME_CHNL_OFFSET_LOWER:
13342                                                                 pht_info->infos[0] |= 0x1;
13343                                                                 break;
13344                                                         case HAL_PRIME_CHNL_OFFSET_UPPER:
13345                                                                 pht_info->infos[0] |= 0x3;
13346                                                                 break;
13347                                                         case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
13348                                                         default:
13349                                                                 break;
13350                                                 }
13351                                         }
13352                                 }
13353                                 else if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_20)
13354                                 {
13355                                         cur_bwmode = CHANNEL_WIDTH_20;
13356                                         cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13357
13358                                         if(cur_channel>0 && cur_channel<5)
13359                                         {
13360                                                 if(pht_info)
13361                                                         pht_info->infos[0] |= 0x1;
13362
13363                                                 cur_bwmode = CHANNEL_WIDTH_40;
13364                                                 cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
13365                                         }
13366
13367                                         if(cur_channel>7 && cur_channel<(14+1))
13368                                         {
13369                                                 if(pht_info)
13370                                                         pht_info->infos[0] |= 0x3;
13371
13372                                                 cur_bwmode = CHANNEL_WIDTH_40;
13373                                                 cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
13374                                         }
13375
13376                                         set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
13377                                 }
13378                         }
13379
13380                         // to update channel value in beacon
13381                         pnetwork->Configuration.DSConfig = cur_channel;
13382                         p = rtw_get_ie((pnetwork->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (pnetwork->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
13383                         if(p && ie_len>0)
13384                                 *(p + 2) = cur_channel;
13385
13386                         if(pht_info)
13387                                 pht_info->primary_channel = cur_channel;
13388                 }
13389         }
13390         else
13391         {
13392                 set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
13393         }
13394
13395         DBG_871X("CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
13396
13397         if((channel <= 14 && cur_channel >= 36) ||
13398                 (channel >= 36 && cur_channel <= 14))
13399         {
13400                 change_band = _TRUE;
13401         }
13402
13403         pmlmeext->cur_channel = cur_channel;
13404         pmlmeext->cur_bwmode = cur_bwmode;
13405         pmlmeext->cur_ch_offset = cur_ch_offset;
13406
13407         if(change_band == _TRUE)
13408                 change_band_update_ie(padapter, pnetwork);
13409
13410         DBG_871X("dualmac_concurrent_ap_set_channel_bwmode <==\n");
13411 }
13412
13413 void dc_resume_xmit(_adapter *padapter)
13414 {
13415         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13416
13417         if(pbuddy_adapter != NULL &&
13418                 padapter->DualMacConcurrent == _TRUE)
13419         {
13420                 DBG_871X("dc_resume_xmit,  resume pbuddy_adapter Tx\n");
13421                 rtw_os_xmit_schedule(pbuddy_adapter);
13422         }
13423 }
13424
13425 u8      dc_check_xmit(_adapter *padapter)
13426 {
13427         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13428         struct mlme_priv *pbuddy_mlmepriv = NULL;
13429
13430         if(pbuddy_adapter != NULL &&
13431                 padapter->DualMacConcurrent == _TRUE)
13432         {
13433                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13434                 if (check_fwstate(pbuddy_mlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == _TRUE)
13435                 {
13436                         DBG_871X("dc_check_xmit  pbuddy_adapter is under survey or under linking\n");
13437                         return _FALSE;
13438                 }
13439         }
13440
13441         return _TRUE;
13442 }
13443 #endif
13444
13445 #ifdef CONFIG_CONCURRENT_MODE
13446 sint check_buddy_mlmeinfo_state(_adapter *padapter, u32 state)
13447 {
13448         PADAPTER pbuddy_adapter;
13449         struct mlme_ext_priv *pbuddy_mlmeext;
13450         struct mlme_ext_info *pbuddy_mlmeinfo;
13451
13452         if(padapter == NULL)
13453                 return _FALSE;
13454
13455         pbuddy_adapter = padapter->pbuddy_adapter;
13456
13457         if(pbuddy_adapter == NULL)
13458                 return _FALSE;
13459
13460
13461         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13462         pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
13463
13464         if((pbuddy_mlmeinfo->state&0x03) == state)
13465                 return _TRUE;
13466
13467         return _FALSE;
13468
13469 }
13470
13471 void concurrent_chk_joinbss_done(_adapter *padapter, int join_res)
13472 {
13473         struct mlme_ext_priv    *pmlmeext;
13474         struct mlme_ext_info    *pmlmeinfo;
13475         PADAPTER pbuddy_adapter;
13476         struct mlme_priv *pbuddy_mlmepriv;
13477         struct mlme_ext_priv *pbuddy_mlmeext;
13478         struct mlme_ext_info *pbuddy_mlmeinfo;
13479         WLAN_BSSID_EX *pbuddy_network_mlmeext;
13480         WLAN_BSSID_EX *pnetwork;
13481
13482         pmlmeext = &padapter->mlmeextpriv;
13483         pmlmeinfo = &(pmlmeext->mlmext_info);
13484
13485
13486         if(!rtw_buddy_adapter_up(padapter))
13487         {
13488                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
13489                 return;
13490         }
13491
13492         pbuddy_adapter = padapter->pbuddy_adapter;
13493         pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13494         pnetwork = (WLAN_BSSID_EX *)&pbuddy_mlmepriv->cur_network.network;
13495         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13496         pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
13497         pbuddy_network_mlmeext = &(pbuddy_mlmeinfo->network);
13498
13499         if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
13500                         check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
13501         {
13502                 //restart and update beacon
13503
13504                 DBG_871X("after join,primary adapter, CH=%d, BW=%d, offset=%d\n"
13505                         , pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
13506
13507
13508                 if(join_res >= 0)
13509                 {
13510                         u8 *p;
13511                         int     ie_len;
13512                         u8      change_band = _FALSE;
13513                         struct HT_info_element *pht_info=NULL;
13514
13515                         if((pmlmeext->cur_channel <= 14 && pbuddy_mlmeext->cur_channel >= 36) ||
13516                                 (pmlmeext->cur_channel >= 36 && pbuddy_mlmeext->cur_channel <= 14))
13517                                 change_band = _TRUE;
13518
13519                         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)));
13520                         if( p && ie_len)
13521                         {
13522                                 pht_info = (struct HT_info_element *)(p+2);
13523                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK; //no secondary channel is present
13524                         }
13525
13526                         pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
13527 #ifdef CONFIG_80211AC_VHT
13528                         if(pbuddy_mlmeext->cur_bwmode  == CHANNEL_WIDTH_80)
13529                         {
13530                                 u8 *pvht_cap_ie, *pvht_op_ie;
13531                                 int vht_cap_ielen, vht_op_ielen;
13532
13533                                 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)));
13534                                 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)));
13535
13536                                 if(pmlmeext->cur_channel <= 14) // downgrade to 20/40Mhz
13537                                 {
13538                                         //modify vht cap ie
13539                                         if( pvht_cap_ie && vht_cap_ielen)
13540                                         {
13541                                                 SET_VHT_CAPABILITY_ELE_SHORT_GI80M(pvht_cap_ie+2, 0);
13542                                         }
13543
13544                                         //modify vht op ie
13545                                         if( pvht_op_ie && vht_op_ielen)
13546                                         {
13547                                                 SET_VHT_OPERATION_ELE_CHL_WIDTH(pvht_op_ie+2, 0); //change to 20/40Mhz
13548                                                 SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(pvht_op_ie+2, 0);
13549                                                 SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ2(pvht_op_ie+2, 0);
13550                                                 //SET_VHT_OPERATION_ELE_BASIC_MCS_SET(p+2, 0xFFFF);
13551                                                 pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
13552                                         }
13553                                 }
13554                                 else
13555                                 {
13556                                         u8      center_freq;
13557
13558                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_80;
13559
13560                                         if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
13561                                                 pmlmeext->cur_bwmode == CHANNEL_WIDTH_80)
13562                                         {
13563                                                 pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
13564                                         }
13565                                         else if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_20)
13566                                         {
13567                                                 pbuddy_mlmeext->cur_ch_offset =  rtw_get_offset_by_ch(pmlmeext->cur_channel);
13568                                         }
13569
13570                                         //modify ht info ie
13571                                         if(pht_info)
13572                                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK;
13573
13574                                         switch(pbuddy_mlmeext->cur_ch_offset)
13575                                         {
13576                                                 case HAL_PRIME_CHNL_OFFSET_LOWER:
13577                                                         if(pht_info)
13578                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;
13579                                                         //cur_bwmode = CHANNEL_WIDTH_40;
13580                                                         break;
13581                                                 case HAL_PRIME_CHNL_OFFSET_UPPER:
13582                                                         if(pht_info)
13583                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
13584                                                         //cur_bwmode = CHANNEL_WIDTH_40;
13585                                                         break;
13586                                                 case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
13587                                                 default:
13588                                                         if(pht_info)
13589                                                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK;
13590                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_20;
13591                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13592                                                         break;
13593                                         }
13594
13595                                         //modify vht op ie
13596                                         center_freq = rtw_get_center_ch(pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, HAL_PRIME_CHNL_OFFSET_LOWER);
13597                                         if( pvht_op_ie && vht_op_ielen)
13598                                                 SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(pvht_op_ie+2, center_freq);
13599
13600                                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13601
13602                                 }
13603
13604                         }
13605 #endif //CONFIG_80211AC_VHT
13606
13607                         if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40)
13608                         {
13609                                 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)));
13610                                 if( p && ie_len)
13611                                 {
13612                                         pht_info = (struct HT_info_element *)(p+2);
13613                                         pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK; //no secondary channel is present
13614                                 }
13615
13616                                 if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
13617                                         pmlmeext->cur_bwmode == CHANNEL_WIDTH_80)
13618                                 {
13619                                         pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
13620
13621                                         //to update cur_ch_offset value in beacon
13622                                         if( pht_info )
13623                                         {
13624                                                 switch(pmlmeext->cur_ch_offset)
13625                                                 {
13626                                                         case HAL_PRIME_CHNL_OFFSET_LOWER:
13627                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;
13628                                                                 break;
13629                                                         case HAL_PRIME_CHNL_OFFSET_UPPER:
13630                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
13631                                                                 break;
13632                                                         case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
13633                                                         default:
13634                                                                 break;
13635                                                 }
13636
13637                                         }
13638
13639                                 }
13640                                 else if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_20)
13641                                 {
13642                                         pbuddy_mlmeext->cur_ch_offset =  rtw_get_offset_by_ch(pmlmeext->cur_channel);
13643
13644                                         switch(pbuddy_mlmeext->cur_ch_offset)
13645                                         {
13646                                                 case HAL_PRIME_CHNL_OFFSET_LOWER:
13647                                                         if(pht_info)
13648                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;
13649                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
13650                                                         break;
13651                                                 case HAL_PRIME_CHNL_OFFSET_UPPER:
13652                                                         if(pht_info)
13653                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
13654                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
13655                                                         break;
13656                                                 case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
13657                                                 default:
13658                                                         if(pht_info)
13659                                                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK;
13660                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_20;
13661                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13662                                                         break;
13663                                         }
13664
13665                                 }
13666
13667                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13668
13669                         }
13670                         else
13671                         {
13672                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
13673                         }
13674
13675
13676                         // to update channel value in beacon
13677                         pbuddy_network_mlmeext->Configuration.DSConfig = pmlmeext->cur_channel;
13678                         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)));
13679                         if(p && ie_len>0)
13680                                 *(p + 2) = pmlmeext->cur_channel;
13681
13682                         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)));
13683                         if( p && ie_len)
13684                         {
13685                                 pht_info = (struct HT_info_element *)(p+2);
13686                                 pht_info->primary_channel = pmlmeext->cur_channel;
13687                         }
13688
13689                         //buddy interface band is different from current interface, update ERP, support rate, ext support rate IE
13690                         if(change_band == _TRUE)
13691                                 change_band_update_ie(pbuddy_adapter, pbuddy_network_mlmeext);
13692                 }
13693                 else
13694                 {
13695                         // switch back to original channel/bwmode/ch_offset;
13696                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13697                 }
13698
13699                 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);
13700
13701                 DBG_871X("update pbuddy_adapter's beacon\n");
13702
13703                 _rtw_memcpy(pnetwork, pbuddy_network_mlmeext, sizeof(WLAN_BSSID_EX));
13704                 //update bmc rate to avoid bb cck hang
13705                 update_bmc_sta(pbuddy_adapter);
13706                 update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
13707
13708         }
13709         else if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) &&
13710                         check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
13711         {
13712                 if(join_res >= 0)
13713                 {
13714                         pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
13715                         if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
13716                                 pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80)
13717                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13718                         else if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
13719                                 pmlmeext->cur_bwmode == CHANNEL_WIDTH_80)
13720                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
13721                         else
13722                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
13723                 }
13724                 else
13725                 {
13726                         // switch back to original channel/bwmode/ch_offset;
13727                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13728                 }
13729         }
13730         else
13731         {
13732                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
13733         }
13734
13735 }
13736 #endif //CONFIG_CONCURRENT_MODE
13737
13738 int rtw_chk_start_clnt_join(_adapter *padapter, u8 *ch, u8 *bw, u8 *offset)
13739 {
13740         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13741         unsigned char   cur_ch = pmlmeext->cur_channel;
13742         unsigned char   cur_bw = pmlmeext->cur_bwmode;
13743         unsigned char   cur_ch_offset = pmlmeext->cur_ch_offset;
13744         bool chbw_allow = _TRUE;
13745         bool connect_allow = _TRUE;
13746
13747 #ifdef CONFIG_CONCURRENT_MODE
13748         PADAPTER pbuddy_adapter;
13749         struct mlme_ext_priv *pbuddy_mlmeext;
13750         struct mlme_ext_info    *pbuddy_pmlmeinfo;
13751         struct mlme_priv *pbuddy_mlmepriv;
13752
13753         if (!rtw_buddy_adapter_up(padapter)) {
13754                 goto exit;
13755         }
13756
13757         pbuddy_adapter = padapter->pbuddy_adapter;
13758         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13759         pbuddy_pmlmeinfo = &(pbuddy_mlmeext->mlmext_info);
13760         pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13761
13762         if((pbuddy_pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)//for AP MODE
13763         {
13764                 DBG_871X("start_clnt_join: "ADPT_FMT"(ch=%d, bw=%d, ch_offset=%d)"
13765                         ", "ADPT_FMT" AP mode(ch=%d, bw=%d, ch_offset=%d)\n",
13766                         ADPT_ARG(padapter), pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset,
13767                         ADPT_ARG(pbuddy_adapter), pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
13768
13769                 if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel)
13770                 {
13771                         chbw_allow = _FALSE;
13772                 }
13773                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
13774                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
13775                         (pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset))
13776                 {
13777                         chbw_allow = _FALSE;
13778                 }
13779                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_20) &&
13780                         ((pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40)||
13781                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80)) )
13782                 {
13783                         cur_ch = pmlmeext->cur_channel;
13784                         cur_bw = pbuddy_mlmeext->cur_bwmode;
13785                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
13786                 }
13787
13788                 DBG_871X("start_clnt_join: connect_allow:%d, chbw_allow:%d\n", connect_allow, chbw_allow);
13789                 if (chbw_allow == _FALSE) {
13790                         #ifdef CONFIG_SPCT_CH_SWITCH
13791                         if (1) {
13792                                 rtw_ap_inform_ch_switch(pbuddy_adapter, pmlmeext->cur_channel , pmlmeext->cur_ch_offset);
13793                         } else
13794                         #endif
13795                         {
13796                                 //issue deauth to all stas if if2 is at ap mode
13797                                 rtw_sta_flush(pbuddy_adapter);
13798                         }
13799                         rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
13800                 }
13801         }
13802         else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED) == _TRUE &&
13803                 check_fwstate(pbuddy_mlmepriv, WIFI_STATION_STATE) == _TRUE) //for Client Mode/p2p client
13804         {
13805                 DBG_871X("start_clnt_join: "ADPT_FMT"(ch=%d, bw=%d, ch_offset=%d)"
13806                         ", "ADPT_FMT" STA mode(ch=%d, bw=%d, ch_offset=%d)\n",
13807                         ADPT_ARG(padapter), pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset,
13808                         ADPT_ARG(pbuddy_adapter), pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
13809
13810                 if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel)
13811                 {
13812                         chbw_allow = _FALSE;
13813                 }
13814                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_20) &&
13815                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40))
13816                 {
13817                         cur_bw = CHANNEL_WIDTH_40;
13818                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
13819                 }
13820                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_20) &&
13821                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80))
13822                 {
13823                         cur_bw = CHANNEL_WIDTH_80;
13824                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
13825                 }
13826                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
13827                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
13828                         (pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset))
13829                 {
13830                         chbw_allow = _FALSE;
13831                 }
13832                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
13833                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80))
13834                 {
13835                         if(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
13836                         {
13837                                 chbw_allow = _FALSE;
13838                         }
13839                         else
13840                         {
13841                                 cur_bw = CHANNEL_WIDTH_80;
13842                         }
13843                 }
13844                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_80) &&
13845                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40))
13846                 {
13847                         if(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
13848                         {
13849                                 chbw_allow = _FALSE;
13850                         }
13851                 }
13852                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_80) &&
13853                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80))
13854                 {
13855                         if(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
13856                         {
13857                                 chbw_allow = _FALSE;
13858                         }
13859                 }
13860
13861
13862                 connect_allow = chbw_allow;
13863
13864                 #if defined(CONFIG_P2P) && defined(CONFIG_IOCTL_CFG80211)
13865                 /* wlan0-sta mode has higher priority than p2p0-p2p client */
13866                 if (!rtw_p2p_chk_state(&(pbuddy_adapter->wdinfo), P2P_STATE_NONE)
13867                         && pbuddy_adapter->wdinfo.driver_interface == DRIVER_CFG80211)
13868                 {
13869                         connect_allow = _TRUE;
13870                 }
13871                 #endif /* CONFIG_P2P && CONFIG_IOCTL_CFG80211 */
13872
13873                 DBG_871X("start_clnt_join: connect_allow:%d, chbw_allow:%d\n", connect_allow, chbw_allow);
13874                 if (connect_allow == _TRUE && chbw_allow == _FALSE) {
13875                         /* disconnect buddy's connection */
13876                         rtw_disassoc_cmd(pbuddy_adapter, 500, _FALSE);
13877                         rtw_indicate_disconnect(pbuddy_adapter);
13878                         rtw_free_assoc_resources(pbuddy_adapter, 1);
13879                 }
13880         }
13881
13882 exit:
13883 #endif /* CONFIG_CONCURRENT_MODE */
13884
13885         if (!ch || !bw || !offset) {
13886                 rtw_warn_on(1);
13887                 connect_allow = _FALSE;
13888         }
13889
13890         if (connect_allow == _TRUE) {
13891                 DBG_871X("start_join_set_ch_bw: ch=%d, bwmode=%d, ch_offset=%d\n", cur_ch, cur_bw, cur_ch_offset);
13892                 *ch = cur_ch;
13893                 *bw = cur_bw;
13894                 *offset = cur_ch_offset;
13895         }
13896
13897         return connect_allow == _TRUE ? _SUCCESS : _FAIL;
13898 }
13899
13900 /* Find union about ch, bw, ch_offset of all linked interfaces */
13901 int rtw_get_ch_setting_union(_adapter *adapter, u8 *ch, u8 *bw, u8 *offset)
13902 {
13903         struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
13904         _adapter *iface;
13905         struct mlme_ext_priv *mlmeext;
13906         int i;
13907         u8 ch_ret = 0;
13908         u8 bw_ret = CHANNEL_WIDTH_20;
13909         u8 offset_ret = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13910         int num = 0;
13911
13912         if (ch) *ch = 0;
13913         if (bw) *bw = CHANNEL_WIDTH_20;
13914         if (offset) *offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13915
13916         for (i = 0; i<dvobj->iface_nums; i++) {
13917                 iface = dvobj->padapters[i];
13918                 mlmeext = &iface->mlmeextpriv;
13919
13920                 if (!check_fwstate(&iface->mlmepriv, _FW_LINKED))
13921                         continue;
13922
13923                 if (num == 0) {
13924                         ch_ret = mlmeext->cur_channel;
13925                         bw_ret = mlmeext->cur_bwmode;
13926                         offset_ret = mlmeext->cur_ch_offset;
13927                         num++;
13928                         continue;
13929                 }
13930
13931                 if (ch_ret != mlmeext->cur_channel) {
13932                         num = 0;
13933                         break;
13934                 }
13935
13936                 if (bw_ret < mlmeext->cur_bwmode) {
13937                         bw_ret = mlmeext->cur_bwmode;
13938                         offset_ret = mlmeext->cur_ch_offset;
13939                 } else if (bw_ret == mlmeext->cur_bwmode && offset_ret != mlmeext->cur_ch_offset) {
13940                         num = 0;
13941                         break;
13942                 }
13943
13944                 num++;
13945         }
13946
13947         if (num) {
13948                 if (ch) *ch = ch_ret;
13949                 if (bw) *bw = bw_ret;
13950                 if (offset) *offset = offset_ret;
13951         }
13952
13953         return num;
13954 }
13955
13956 u8 set_ch_hdl(_adapter *padapter, u8 *pbuf)
13957 {
13958         struct set_ch_parm *set_ch_parm;
13959         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
13960         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13961
13962         if(!pbuf)
13963                 return H2C_PARAMETERS_ERROR;
13964
13965         set_ch_parm = (struct set_ch_parm *)pbuf;
13966
13967         DBG_871X(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
13968                 FUNC_NDEV_ARG(padapter->pnetdev),
13969                 set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
13970
13971         pmlmeext->cur_channel = set_ch_parm->ch;
13972         pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
13973         pmlmeext->cur_bwmode = set_ch_parm->bw;
13974
13975         set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
13976
13977         return  H2C_SUCCESS;
13978 }
13979
13980 u8 set_chplan_hdl(_adapter *padapter, unsigned char *pbuf)
13981 {
13982         struct SetChannelPlan_param *setChannelPlan_param;
13983         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
13984         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13985
13986         if(!pbuf)
13987                 return H2C_PARAMETERS_ERROR;
13988
13989         setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
13990
13991         pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
13992         init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
13993
13994 #ifdef CONFIG_IOCTL_CFG80211
13995         if ((padapter->rtw_wdev != NULL) && (padapter->rtw_wdev->wiphy)) {
13996                 struct regulatory_request request;
13997                 request.initiator = NL80211_REGDOM_SET_BY_DRIVER;
13998                 rtw_reg_notifier(padapter->rtw_wdev->wiphy, &request);
13999         }
14000 #endif //CONFIG_IOCTL_CFG80211
14001
14002         return  H2C_SUCCESS;
14003 }
14004
14005 u8 led_blink_hdl(_adapter *padapter, unsigned char *pbuf)
14006 {
14007         struct LedBlink_param *ledBlink_param;
14008
14009         if(!pbuf)
14010                 return H2C_PARAMETERS_ERROR;
14011
14012         ledBlink_param = (struct LedBlink_param *)pbuf;
14013
14014         #ifdef CONFIG_LED_HANDLED_BY_CMD_THREAD
14015         BlinkHandler((PLED_DATA)ledBlink_param->pLed);
14016         #endif
14017
14018         return  H2C_SUCCESS;
14019 }
14020
14021 u8 set_csa_hdl(_adapter *padapter, unsigned char *pbuf)
14022 {
14023 #ifdef CONFIG_DFS
14024         struct SetChannelSwitch_param *setChannelSwitch_param;
14025         u8 new_ch_no;
14026         u8 gval8 = 0x00, sval8 = 0xff;
14027
14028         if(!pbuf)
14029                 return H2C_PARAMETERS_ERROR;
14030
14031         setChannelSwitch_param = (struct SetChannelSwitch_param *)pbuf;
14032         new_ch_no = setChannelSwitch_param->new_ch_no;
14033
14034         rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
14035
14036         rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &sval8);
14037
14038         DBG_871X("DFS detected! Swiching channel to %d!\n", new_ch_no);
14039         SelectChannel(padapter, new_ch_no);
14040
14041         rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
14042
14043         rtw_disassoc_cmd(padapter, 0, _FALSE);
14044         rtw_indicate_disconnect(padapter);
14045         rtw_free_assoc_resources(padapter, 1);
14046         rtw_free_network_queue(padapter, _TRUE);
14047
14048         if ( ((new_ch_no >= 52) && (new_ch_no <= 64)) ||((new_ch_no >= 100) && (new_ch_no <= 140)) ) {
14049                 DBG_871X("Switched to DFS band (ch %02x) again!!\n", new_ch_no);
14050         }
14051
14052         return  H2C_SUCCESS;
14053 #else
14054         return  H2C_REJECTED;
14055 #endif //CONFIG_DFS
14056
14057 }
14058
14059 // TDLS_WRCR            : write RCR DATA BIT
14060 // TDLS_SD_PTI          : issue peer traffic indication
14061 // TDLS_CS_OFF          : go back to the channel linked with AP, terminating channel switch procedure
14062 // TDLS_INIT_CH_SEN     : init channel sensing, receive all data and mgnt frame
14063 // TDLS_DONE_CH_SEN: channel sensing and report candidate channel
14064 // TDLS_OFF_CH          : first time set channel to off channel
14065 // TDLS_BASE_CH         : go back tp the channel linked with AP when set base channel as target channel
14066 // TDLS_P_OFF_CH        : periodically go to off channel
14067 // TDLS_P_BASE_CH       : periodically go back to base channel
14068 // TDLS_RS_RCR          : restore RCR
14069 // TDLS_CKALV_PH1       : check alive timer phase1
14070 // TDLS_CKALV_PH2       : check alive timer phase2
14071 // TDLS_FREE_STA        : free tdls sta
14072 u8 tdls_hdl(_adapter *padapter, unsigned char *pbuf)
14073 {
14074 #ifdef CONFIG_TDLS
14075         _irqL irqL;
14076         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
14077         struct TDLSoption_param *TDLSoption;
14078         struct sta_info *ptdls_sta;
14079         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
14080         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
14081         u8 survey_channel, i, min, option;
14082
14083         if(!pbuf)
14084                 return H2C_PARAMETERS_ERROR;
14085
14086         TDLSoption = (struct TDLSoption_param *)pbuf;
14087
14088         ptdls_sta = rtw_get_stainfo( &(padapter->stapriv), TDLSoption->addr );
14089         option = TDLSoption->option;
14090
14091         if( ptdls_sta == NULL )
14092         {
14093                 if( option != TDLS_RS_RCR )
14094                         return H2C_REJECTED;
14095         }
14096
14097         //_enter_critical_bh(&(ptdlsinfo->hdl_lock), &irqL);
14098         DBG_871X("[%s] option:%d\n", __FUNCTION__, option);
14099
14100         switch(option){
14101                 case TDLS_WRCR:
14102                         //As long as TDLS handshake success, we should set RCR_CBSSID_DATA bit to 0
14103                         //such we can receive all kinds of data frames.
14104                         rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_WRCR, 0);
14105                         DBG_871X("TDLS with "MAC_FMT"\n", MAC_ARG(ptdls_sta->hwaddr));
14106
14107                         pmlmeinfo->FW_sta_info[ptdls_sta->mac_id].psta = ptdls_sta;
14108                         //set TDLS sta rate.
14109                         set_sta_rate(padapter, ptdls_sta);
14110                         break;
14111                 case TDLS_SD_PTI:
14112                         issue_tdls_peer_traffic_indication(padapter, ptdls_sta);
14113                         break;
14114                 case TDLS_CS_OFF:
14115                         _cancel_timer_ex(&ptdls_sta->base_ch_timer);
14116                         _cancel_timer_ex(&ptdls_sta->off_ch_timer);
14117                         SelectChannel(padapter, pmlmeext->cur_channel);
14118                         ptdls_sta->tdls_sta_state &= ~(TDLS_CH_SWITCH_ON_STATE |
14119                                                                 TDLS_PEER_AT_OFF_STATE |
14120                                                                 TDLS_AT_OFF_CH_STATE);
14121                         DBG_871X("go back to base channel\n ");
14122                         issue_nulldata(padapter, NULL, 0, 0, 0);
14123                         break;
14124                 case TDLS_INIT_CH_SEN:
14125                         rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_INIT_CH_SEN, 0);
14126                         pmlmeext->sitesurvey_res.channel_idx = 0;
14127                         ptdls_sta->option = TDLS_DONE_CH_SEN;
14128                         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_DONE_CH_SEN);
14129                         break;
14130                 case TDLS_DONE_CH_SEN:
14131                         survey_channel = pmlmeext->channel_set[pmlmeext->sitesurvey_res.channel_idx].ChannelNum;
14132                         if(survey_channel){
14133                                 SelectChannel(padapter, survey_channel);
14134                                 ptdlsinfo->cur_channel = survey_channel;
14135                                 pmlmeext->sitesurvey_res.channel_idx++;
14136                                 _set_timer(&ptdls_sta->option_timer, SURVEY_TO);
14137                         }else{
14138                                 SelectChannel(padapter, pmlmeext->cur_channel);
14139
14140                                 rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_DONE_CH_SEN, 0);
14141
14142                                 if(ptdlsinfo->ch_sensing==1){
14143                                         ptdlsinfo->ch_sensing=0;
14144                                         ptdlsinfo->cur_channel=1;
14145                                         min=ptdlsinfo->collect_pkt_num[0];
14146                                         for(i=1; i<MAX_CHANNEL_NUM-1; i++){
14147                                                 if(min > ptdlsinfo->collect_pkt_num[i]){
14148                                                         ptdlsinfo->cur_channel=i+1;
14149                                                         min=ptdlsinfo->collect_pkt_num[i];
14150                                                 }
14151                                                 ptdlsinfo->collect_pkt_num[i]=0;
14152                                         }
14153                                         ptdlsinfo->collect_pkt_num[0]=0;
14154                                         ptdlsinfo->candidate_ch=ptdlsinfo->cur_channel;
14155                                         DBG_871X("TDLS channel sensing done, candidate channel: %02x\n", ptdlsinfo->candidate_ch);
14156                                         ptdlsinfo->cur_channel=0;
14157
14158                                 }
14159
14160                                 if(ptdls_sta->tdls_sta_state & TDLS_PEER_SLEEP_STATE){
14161                                         ptdls_sta->tdls_sta_state |= TDLS_APSD_CHSW_STATE;
14162                                 }else{
14163                                         //send null data with pwrbit==1 before send ch_switching_req to peer STA.
14164                                         issue_nulldata(padapter, NULL, 1, 0, 0);
14165
14166                                         ptdls_sta->tdls_sta_state |= TDLS_CH_SW_INITIATOR_STATE;
14167
14168                                         issue_tdls_ch_switch_req(padapter, ptdls_sta->hwaddr);
14169                                         DBG_871X("issue tdls ch switch req\n");
14170                                 }
14171                         }
14172                         break;
14173                 case TDLS_OFF_CH:
14174                         issue_nulldata(padapter, NULL, 1, 0, 0);
14175                         SelectChannel(padapter, ptdls_sta->off_ch);
14176
14177                         DBG_871X("change channel to tar ch:%02x\n", ptdls_sta->off_ch);
14178                         ptdls_sta->tdls_sta_state |= TDLS_AT_OFF_CH_STATE;
14179                         ptdls_sta->tdls_sta_state &= ~(TDLS_PEER_AT_OFF_STATE);
14180                         _set_timer(&ptdls_sta->option_timer, (u32)ptdls_sta->ch_switch_time);
14181                         break;
14182                 case TDLS_BASE_CH:
14183                         _cancel_timer_ex(&ptdls_sta->base_ch_timer);
14184                         _cancel_timer_ex(&ptdls_sta->off_ch_timer);
14185                         SelectChannel(padapter, pmlmeext->cur_channel);
14186                         ptdls_sta->tdls_sta_state &= ~(TDLS_CH_SWITCH_ON_STATE |
14187                                                                 TDLS_PEER_AT_OFF_STATE |
14188                                                                 TDLS_AT_OFF_CH_STATE);
14189                         DBG_871X("go back to base channel\n ");
14190                         issue_nulldata(padapter, NULL, 0, 0, 0);
14191                         _set_timer(&ptdls_sta->option_timer, (u32)ptdls_sta->ch_switch_time);
14192                         break;
14193                 case TDLS_P_OFF_CH:
14194                         SelectChannel(padapter, pmlmeext->cur_channel);
14195                         issue_nulldata(padapter, NULL, 0, 0, 0);
14196                         DBG_871X("change channel to base ch:%02x\n", pmlmeext->cur_channel);
14197                         ptdls_sta->tdls_sta_state &= ~(TDLS_PEER_AT_OFF_STATE| TDLS_AT_OFF_CH_STATE);
14198                         _set_timer(&ptdls_sta->off_ch_timer, TDLS_STAY_TIME);
14199                         break;
14200                 case TDLS_P_BASE_CH:
14201                         issue_nulldata(ptdls_sta->padapter, NULL, 1, 0, 0);
14202                         SelectChannel(padapter, ptdls_sta->off_ch);
14203                         DBG_871X("change channel to off ch:%02x\n", ptdls_sta->off_ch);
14204                         ptdls_sta->tdls_sta_state |= TDLS_AT_OFF_CH_STATE;
14205                         if((ptdls_sta->tdls_sta_state & TDLS_PEER_AT_OFF_STATE) != TDLS_PEER_AT_OFF_STATE){
14206                                 issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta, 0);
14207                         }
14208                         _set_timer(&ptdls_sta->base_ch_timer, TDLS_STAY_TIME);
14209                         break;
14210                 case TDLS_RS_RCR:
14211                         rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_RS_RCR, 0);
14212                         DBG_871X("wirte REG_RCR, set bit6 on\n");
14213                         break;
14214                 case TDLS_CKALV_PH1:
14215                         _set_timer(&ptdls_sta->alive_timer2, TDLS_ALIVE_TIMER_PH2);
14216                         break;
14217                 case TDLS_CKALV_PH2:
14218                         _set_timer(&ptdls_sta->alive_timer1, TDLS_ALIVE_TIMER_PH1);
14219                         break;
14220                 case TDLS_FREE_STA:
14221                         free_tdls_sta(padapter, ptdls_sta);
14222                         break;
14223
14224         }
14225
14226         //_exit_critical_bh(&(ptdlsinfo->hdl_lock), &irqL);
14227
14228         return H2C_SUCCESS;
14229 #else
14230         return H2C_REJECTED;
14231 #endif //CONFIG_TDLS
14232
14233 }
14234