drivers/net/wireless/rockchip_wlan/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         {WIFI_ACTION_NOACK,"OnActionNoAck",     &OnAction},
  48 };
  49
  50 #ifdef _CONFIG_NATIVEAP_MLME_
  51 struct mlme_handler mlme_ap_tbl[]={
  52         {WIFI_ASSOCREQ,         "OnAssocReq",   &OnAssocReq},
  53         {WIFI_ASSOCRSP,         "OnAssocRsp",   &OnAssocRsp},
  54         {WIFI_REASSOCREQ,       "OnReAssocReq", &OnAssocReq},
  55         {WIFI_REASSOCRSP,       "OnReAssocRsp", &OnAssocRsp},
  56         {WIFI_PROBEREQ,         "OnProbeReq",   &OnProbeReq},
  57         {WIFI_PROBERSP,         "OnProbeRsp",           &OnProbeRsp},
  58
  59         /*----------------------------------------------------------
  60                                         below 2 are reserved
  61         -----------------------------------------------------------*/
  62         {0,                                     "DoReserved",           &DoReserved},
  63         {0,                                     "DoReserved",           &DoReserved},
  64         {WIFI_BEACON,           "OnBeacon",             &OnBeacon},
  65         {WIFI_ATIM,                     "OnATIM",               &OnAtim},
  66         {WIFI_DISASSOC,         "OnDisassoc",           &OnDisassoc},
  67         {WIFI_AUTH,                     "OnAuth",               &OnAuth},
  68         {WIFI_DEAUTH,           "OnDeAuth",             &OnDeAuth},
  69         {WIFI_ACTION,           "OnAction",             &OnAction},
  70         {WIFI_ACTION_NOACK,"OnActionNoAck",     &OnAction},
  71 };
  72 #endif
  73
  74 struct action_handler OnAction_tbl[]={
  75         {RTW_WLAN_CATEGORY_SPECTRUM_MGMT,        "ACTION_SPECTRUM_MGMT", on_action_spct},
  76         {RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos},
  77         {RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls},
  78         {RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
  79         {RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
  80         {RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
  81         {RTW_WLAN_CATEGORY_FT, "ACTION_FT",     &DoReserved},
  82         {RTW_WLAN_CATEGORY_HT,  "ACTION_HT",    &OnAction_ht},
  83 #ifdef CONFIG_IEEE80211W
  84         {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &OnAction_sa_query},
  85 #else
  86         {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved},
  87 #endif //CONFIG_IEEE80211W
  88         //add for CONFIG_IEEE80211W
  89         {RTW_WLAN_CATEGORY_UNPROTECTED_WNM, "ACTION_UNPROTECTED_WNM", &DoReserved},
  90         {RTW_WLAN_CATEGORY_SELF_PROTECTED, "ACTION_SELF_PROTECTED", &DoReserved},
  91         {RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm},
  92         {RTW_WLAN_CATEGORY_VHT, "ACTION_VHT", &OnAction_vht},
  93         {RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p},
  94 };
  95
  96
  97 u8      null_addr[ETH_ALEN]= {0,0,0,0,0,0};
  98
  99 /**************************************************
 100 OUI definitions for the vendor specific IE
 101 ***************************************************/
 102 unsigned char   RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
 103 unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
 104 unsigned char   WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
 105 unsigned char   P2P_OUI[] = {0x50,0x6F,0x9A,0x09};
 106 unsigned char   WFD_OUI[] = {0x50,0x6F,0x9A,0x0A};
 107
 108 unsigned char   WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
 109 unsigned char   WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
 110
 111 unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
 112 unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
 113
 114 extern unsigned char REALTEK_96B_IE[];
 115
 116
 117 /********************************************************
 118 ChannelPlan definitions
 119 *********************************************************/
 120 /*static RT_CHANNEL_PLAN        DefaultChannelPlan[RT_CHANNEL_DOMAIN_MAX] = {
 121         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},32},                                     // 0x00, RT_CHANNEL_DOMAIN_FCC
 122         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},31},                                         // 0x01, RT_CHANNEL_DOMAIN_IC
 123         {{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},32},                                               // 0x02, RT_CHANNEL_DOMAIN_ETSI
 124         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x03, RT_CHANNEL_DOMAIN_SPAIN
 125         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x04, RT_CHANNEL_DOMAIN_FRANCE
 126         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x05, RT_CHANNEL_DOMAIN_MKK
 127         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x06, RT_CHANNEL_DOMAIN_MKK1
 128         {{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64},21},                                                                                                                           // 0x07, RT_CHANNEL_DOMAIN_ISRAEL
 129         {{1,2,3,4,5,6,7,8,9,10,11,12,13,14,36,40,44,48,52,56,60,64},22},                                                                                                                        // 0x08, RT_CHANNEL_DOMAIN_TELEC
 130         {{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},                                                                                                                                                                // 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN
 131         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},                                                                                                                                                                   // 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13
 132         {{1,2,3,4,5,6,7,8,9,10,11,56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},26},                                                                        // 0x0B, RT_CHANNEL_DOMAIN_TAIWAN
 133         {{1,2,3,4,5,6,7,8,9,10,11,12,13,149,153,157,161,165},18},                                                                                                                               // 0x0C, RT_CHANNEL_DOMAIN_CHINA
 134         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,149,153,157,161,165},24},                                                                                             // 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO
 135         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,149,153,157,161,165},31},                                         // 0x0E, RT_CHANNEL_DOMAIN_KOREA
 136         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64},19},                                                                                                                                 // 0x0F, RT_CHANNEL_DOMAIN_TURKEY
 137         {{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},32},                                               // 0x10, RT_CHANNEL_DOMAIN_JAPAN
 138         {{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,149,153,157,161,165},20},                                                                                                                 // 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS
 139         {{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48},17},                                                                                                                                               // 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS
 140         {{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140,149,153,157,161,165},37},   // 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G
 141         {{1,2,3,4,5,6,7,8,9,10,11,56,60,64,149,153,157,161,165},19},                                                                                                                            // 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS
 142 };*/
 143
 144 static RT_CHANNEL_PLAN_2G       RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
 145         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},           // 0x00, RT_CHANNEL_DOMAIN_2G_WORLD , Passive scan CH 12, 13
 146         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},           // 0x01, RT_CHANNEL_DOMAIN_2G_ETSI1
 147         {{1,2,3,4,5,6,7,8,9,10,11},11},                 // 0x02, RT_CHANNEL_DOMAIN_2G_FCC1
 148         {{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},        // 0x03, RT_CHANNEL_DOMAIN_2G_MIKK1
 149         {{10,11,12,13},4},                                              // 0x04, RT_CHANNEL_DOMAIN_2G_ETSI2
 150         {{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},        // 0x05, RT_CHANNEL_DOMAIN_2G_GLOBAL , Passive scan CH 12, 13, 14
 151         {{},0},                                                         // 0x06, RT_CHANNEL_DOMAIN_2G_NULL
 152 };
 153
 154 static RT_CHANNEL_PLAN_5G       RTW_ChannelPlan5G[RT_CHANNEL_DOMAIN_5G_MAX] = {
 155         {{},0},                                                                                                                                                                 // 0x00, RT_CHANNEL_DOMAIN_5G_NULL
 156         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},19},                                             // 0x01, RT_CHANNEL_DOMAIN_5G_ETSI1
 157         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140,149,153,157,161,165},24}, // 0x02, RT_CHANNEL_DOMAIN_5G_ETSI2
 158         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,149,153,157,161,165},22},                 // 0x03, RT_CHANNEL_DOMAIN_5G_ETSI3
 159         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140,149,153,157,161,165},24}, // 0x04, RT_CHANNEL_DOMAIN_5G_FCC1
 160         {{36,40,44,48,149,153,157,161,165},9},                                                                                                          // 0x05, RT_CHANNEL_DOMAIN_5G_FCC2
 161         {{36,40,44,48,52,56,60,64,149,153,157,161,165},13},                                                                                     // 0x06, RT_CHANNEL_DOMAIN_5G_FCC3
 162         {{36,40,44,48,52,56,60,64,149,153,157,161},12},                                                                                         // 0x07, RT_CHANNEL_DOMAIN_5G_FCC4
 163         {{149,153,157,161,165},5},                                                                                                                                      // 0x08, RT_CHANNEL_DOMAIN_5G_FCC5
 164         {{36,40,44,48,52,56,60,64},8},                                                                                                                          // 0x09, RT_CHANNEL_DOMAIN_5G_FCC6
 165         {{36,40,44,48,52,56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},20},                                 // 0x0A, RT_CHANNEL_DOMAIN_5G_FCC7_IC1
 166         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,149,153,157,161,165},20},                                 // 0x0B, RT_CHANNEL_DOMAIN_5G_KCC1
 167         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},19},                                             // 0x0C, RT_CHANNEL_DOMAIN_5G_MKK1
 168         {{36,40,44,48,52,56,60,64},8},                                                                                                                          // 0x0D, RT_CHANNEL_DOMAIN_5G_MKK2
 169         {{100,104,108,112,116,120,124,128,132,136,140},11},                                                                                     // 0x0E, RT_CHANNEL_DOMAIN_5G_MKK3
 170         {{56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},15},                                                                // 0x0F, RT_CHANNEL_DOMAIN_5G_NCC1
 171         {{56,60,64,149,153,157,161,165},8},                                                                                                                     // 0x10, RT_CHANNEL_DOMAIN_5G_NCC2
 172         {{149,153,157,161,165},5},                                                                                                                                      // 0x11, RT_CHANNEL_DOMAIN_5G_NCC3
 173         {{36,40,44,48},4},                                                                                                                                                      // 0x12, RT_CHANNEL_DOMAIN_5G_ETSI4
 174         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},21},                     // 0x13, RT_CHANNEL_DOMAIN_5G_ETSI5
 175         {{149,153,157,161},4},                                                                                                                                          // 0x14, RT_CHANNEL_DOMAIN_5G_FCC8
 176         {{36,40,44,48,52,56,60,64},8},                                                                                                                          // 0x15, RT_CHANNEL_DOMAIN_5G_ETSI6
 177         {{36,40,44,48,52,56,60,64,149,153,157,161,165},13},                                                                                     // 0x16, RT_CHANNEL_DOMAIN_5G_ETSI7
 178         {{36,40,44,48,149,153,157,161,165},9},                                                                                                          // 0x17, RT_CHANNEL_DOMAIN_5G_ETSI8
 179         {{100,104,108,112,116,120,124,128,132,136,140},11},                                                                                     // 0x18, RT_CHANNEL_DOMAIN_5G_ETSI9
 180         {{149,153,157,161,165},5},                                                                                                                                      // 0x19, RT_CHANNEL_DOMAIN_5G_ETSI10
 181         {{36,40,44,48,52,56,60,64,132,136,140,149,153,157,161,165},16},                                                                 // 0x1A, RT_CHANNEL_DOMAIN_5G_ETSI11
 182         {{52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},17},                                                 // 0x1B, RT_CHANNEL_DOMAIN_5G_NCC4
 183         {{149,153,157,161},4},                                                                                                                                          // 0x1C, RT_CHANNEL_DOMAIN_5G_ETSI12
 184         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},21},                     // 0x1D, RT_CHANNEL_DOMAIN_5G_FCC9
 185         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140},16},                                                         // 0x1E, RT_CHANNEL_DOMAIN_5G_ETSI13
 186         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161},20},                         // 0x1F, RT_CHANNEL_DOMAIN_5G_FCC10
 187         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,149,153,157,161},19},                                     // 0x20, RT_CHANNEL_DOMAIN_5G_KCC2
 188         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},21},                     // 0x21, RT_CHANNEL_DOMAIN_5G_FCC11
 189         {{56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},16},                                            // 0x22, RT_CHANNEL_DOMAIN_5G_NCC5
 190         {{36,40,44,48},4},                                                                                                                                                      // 0x23, RT_CHANNEL_DOMAIN_5G_MKK4
 191         //===== Driver self defined for old channel plan Compatible ,Remember to modify if have new channel plan definition =====
 192         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},21},                     // 0x30, RT_CHANNEL_DOMAIN_5G_FCC
 193         {{36,40,44,48},4},                                                                                                                                                      // 0x31, RT_CHANNEL_DOMAIN_5G_JAPAN_NO_DFS
 194         {{36,40,44,48,149,153,157,161},8},                                                                                                                      // 0x32, RT_CHANNEL_DOMAIN_5G_FCC4_NO_DFS
 195 };
 196
 197 static RT_CHANNEL_PLAN_MAP      RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
 198         //===== 0x00 ~ 0x1F , Old Define =====
 199         {0x02,0x20},    //0x00, RT_CHANNEL_DOMAIN_FCC
 200         {0x02,0x0A},    //0x01, RT_CHANNEL_DOMAIN_IC
 201         {0x01,0x01},    //0x02, RT_CHANNEL_DOMAIN_ETSI
 202         {0x01,0x00},    //0x03, RT_CHANNEL_DOMAIN_SPAIN
 203         {0x01,0x00},    //0x04, RT_CHANNEL_DOMAIN_FRANCE
 204         {0x03,0x00},    //0x05, RT_CHANNEL_DOMAIN_MKK
 205         {0x03,0x00},    //0x06, RT_CHANNEL_DOMAIN_MKK1
 206         {0x01,0x09},    //0x07, RT_CHANNEL_DOMAIN_ISRAEL
 207         {0x03,0x09},    //0x08, RT_CHANNEL_DOMAIN_TELEC
 208         {0x03,0x00},    //0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN
 209         {0x00,0x00},    //0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13
 210         {0x02,0x0F},    //0x0B, RT_CHANNEL_DOMAIN_TAIWAN
 211         {0x01,0x08},    //0x0C, RT_CHANNEL_DOMAIN_CHINA
 212         {0x02,0x06},    //0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO
 213         {0x02,0x0B},    //0x0E, RT_CHANNEL_DOMAIN_KOREA
 214         {0x02,0x09},    //0x0F, RT_CHANNEL_DOMAIN_TURKEY
 215         {0x01,0x01},    //0x10, RT_CHANNEL_DOMAIN_JAPAN
 216         {0x02,0x05},    //0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS
 217         {0x01,0x21},    //0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS
 218         {0x00,0x04},    //0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G
 219         {0x02,0x10},    //0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS
 220         {0x00,0x21},    //0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS
 221         {0x00,0x22},    //0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS
 222         {0x03,0x21},    //0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS
 223         {0x06,0x08},    //0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS
 224         {0x02,0x08},    //0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS
 225         {0x00,0x00},    //0x1A,
 226         {0x00,0x00},    //0x1B,
 227         {0x00,0x00},    //0x1C,
 228         {0x00,0x00},    //0x1D,
 229         {0x00,0x00},    //0x1E,
 230         {0x06,0x04},    //0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G
 231         //===== 0x20 ~ 0x7F ,New Define =====
 232         {0x00,0x00},    //0x20, RT_CHANNEL_DOMAIN_WORLD_NULL
 233         {0x01,0x00},    //0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL
 234         {0x02,0x00},    //0x22, RT_CHANNEL_DOMAIN_FCC1_NULL
 235         {0x03,0x00},    //0x23, RT_CHANNEL_DOMAIN_MKK1_NULL
 236         {0x04,0x00},    //0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL
 237         {0x02,0x04},    //0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1
 238         {0x00,0x01},    //0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1
 239         {0x03,0x0C},    //0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1
 240         {0x00,0x20},    //0x28, RT_CHANNEL_DOMAIN_5G_KCC2
 241         {0x00,0x05},    //0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2
 242         {0x00,0x00},    //0x2A,
 243         {0x00,0x00},    //0x2B,
 244         {0x00,0x00},    //0x2C,
 245         {0x00,0x00},    //0x2D,
 246         {0x00,0x00},    //0x2E,
 247         {0x00,0x00},    //0x2F,
 248         {0x00,0x06},    //0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3
 249         {0x00,0x07},    //0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4
 250         {0x00,0x08},    //0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5
 251         {0x00,0x09},    //0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6
 252         {0x02,0x21},    //0x34, RT_CHANNEL_DOMAIN_5G_FCC11
 253         {0x00,0x02},    //0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2
 254         {0x00,0x03},    //0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3
 255         {0x03,0x0D},    //0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2
 256         {0x03,0x0E},    //0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3
 257         {0x02,0x22},    //0x39, RT_CHANNEL_DOMAIN_5G_NCC5
 258         {0x00,0x00},    //0x3A,
 259         {0x00,0x00},    //0x3B,
 260         {0x00,0x00},    //0x3C,
 261         {0x00,0x00},    //0x3D,
 262         {0x00,0x00},    //0x3E,
 263         {0x00,0x00},    //0x3F,
 264         {0x02,0x10},    //0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2
 265         {0x05,0x00},    //0x41, RT_CHANNEL_DOMAIN_GLOBAL_NULL
 266         {0x01,0x12},    //0x42, RT_CHANNEL_DOMAIN_ETSI1_ETSI4
 267         {0x02,0x05},    //0x43, RT_CHANNEL_DOMAIN_FCC1_FCC2
 268         {0x02,0x11},    //0x44, RT_CHANNEL_DOMAIN_FCC1_NCC3
 269         {0x00,0x13},    //0x45, RT_CHANNEL_DOMAIN_WORLD_ETSI5
 270         {0x02,0x14},    //0x46, RT_CHANNEL_DOMAIN_FCC1_FCC8
 271         {0x00,0x15},    //0x47, RT_CHANNEL_DOMAIN_WORLD_ETSI6
 272         {0x00,0x16},    //0x48, RT_CHANNEL_DOMAIN_WORLD_ETSI7
 273         {0x00,0x17},    //0x49, RT_CHANNEL_DOMAIN_WORLD_ETSI8
 274         {0x00,0x00},    //0x4A,
 275         {0x00,0x00},    //0x4B,
 276         {0x00,0x00},    //0x4C,
 277         {0x00,0x00},    //0x4D,
 278         {0x00,0x00},    //0x4E,
 279         {0x00,0x00},    //0x4F,
 280         {0x00,0x18},    //0x50, RT_CHANNEL_DOMAIN_WORLD_ETSI9
 281         {0x00,0x19},    //0x51, RT_CHANNEL_DOMAIN_WORLD_ETSI10
 282         {0x00,0x1A},    //0x52, RT_CHANNEL_DOMAIN_WORLD_ETSI11
 283         {0x02,0x1B},    //0x53, RT_CHANNEL_DOMAIN_FCC1_NCC4
 284         {0x00,0x1C},    //0x54, RT_CHANNEL_DOMAIN_WORLD_ETSI12
 285         {0x02,0x1D},    //0x55, RT_CHANNEL_DOMAIN_FCC1_FCC9
 286         {0x00,0x1E},    //0x56, RT_CHANNEL_DOMAIN_WORLD_ETSI13
 287         {0x02,0x1F},    //0x57, RT_CHANNEL_DOMAIN_FCC1_FCC10
 288         {0x01,0x23},    //0x58, RT_CHANNEL_DOMAIN_WORLD_MKK4
 289 };
 290
 291 static RT_CHANNEL_PLAN_MAP      RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x01,0x02}; //use the conbination for max channel numbers
 292
 293 /*
 294  * Search the @param ch in given @param ch_set
 295  * @ch_set: the given channel set
 296  * @ch: the given channel number
 297  *
 298  * return the index of channel_num in channel_set, -1 if not found
 299  */
 300 int rtw_ch_set_search_ch(RT_CHANNEL_INFO *ch_set, const u32 ch)
 301 {
 302         int i;
 303         for(i=0;ch_set[i].ChannelNum!=0;i++){
 304                 if(ch == ch_set[i].ChannelNum)
 305                         break;
 306         }
 307
 308         if(i >= ch_set[i].ChannelNum)
 309                 return -1;
 310         return i;
 311 }
 312
 313 /*
 314  * Check the @param ch is fit with setband setting of @param adapter
 315  * @adapter: the given adapter
 316  * @ch: the given channel number
 317  *
 318  * return _TRUE when check valid, _FALSE not valid
 319  */
 320 bool rtw_mlme_band_check(_adapter *adapter, const u32 ch)
 321 {
 322         if (adapter->setband == GHZ24_50 /* 2.4G and 5G */
 323                 || (adapter->setband == GHZ_24 && ch < 35) /* 2.4G only */
 324                 || (adapter->setband == GHZ_50 && ch > 35) /* 5G only */
 325         ) {
 326                 return _TRUE;
 327         }
 328         return _FALSE;
 329 }
 330
 331 /****************************************************************************
 332
 333 Following are the initialization functions for WiFi MLME
 334
 335 *****************************************************************************/
 336
 337 int init_hw_mlme_ext(_adapter *padapter)
 338 {
 339         struct  mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 340
 341         //set_opmode_cmd(padapter, infra_client_with_mlme);//removed
 342
 343         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 344
 345         return _SUCCESS;
 346 }
 347
 348 void init_mlme_default_rate_set(_adapter* padapter)
 349 {
 350         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 351
 352         unsigned char   mixed_datarate[NumRates] = {_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,_9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_, _48M_RATE_, _54M_RATE_, 0xff};
 353         unsigned char   mixed_basicrate[NumRates] ={_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_, _12M_RATE_, _24M_RATE_, 0xff,};
 354         unsigned char   supported_mcs_set[16] = {0xff, 0xff, 0x00, 0x00, 0x00, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 355
 356         _rtw_memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
 357         _rtw_memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
 358
 359         _rtw_memcpy(pmlmeext->default_supported_mcs_set, supported_mcs_set, sizeof(pmlmeext->default_supported_mcs_set));
 360 }
 361
 362 static void init_mlme_ext_priv_value(_adapter* padapter)
 363 {
 364         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 365         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 366
 367         ATOMIC_SET(&pmlmeext->event_seq, 0);
 368         pmlmeext->mgnt_seq = 0;//reset to zero when disconnect at client mode
 369 #ifdef CONFIG_IEEE80211W
 370         pmlmeext->sa_query_seq = 0;
 371         pmlmeext->mgnt_80211w_IPN=0;
 372         pmlmeext->mgnt_80211w_IPN_rx=0;
 373 #endif //CONFIG_IEEE80211W
 374         pmlmeext->cur_channel = padapter->registrypriv.channel;
 375         pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
 376         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 377
 378         pmlmeext->retry = 0;
 379
 380         pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
 381
 382         init_mlme_default_rate_set(padapter);
 383
 384         if(pmlmeext->cur_channel > 14)
 385                 pmlmeext->tx_rate = IEEE80211_OFDM_RATE_6MB;
 386         else
 387                 pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
 388
 389         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
 390         pmlmeext->sitesurvey_res.channel_idx = 0;
 391         pmlmeext->sitesurvey_res.bss_cnt = 0;
 392         pmlmeext->scan_abort = _FALSE;
 393
 394         pmlmeinfo->state = WIFI_FW_NULL_STATE;
 395         pmlmeinfo->reauth_count = 0;
 396         pmlmeinfo->reassoc_count = 0;
 397         pmlmeinfo->link_count = 0;
 398         pmlmeinfo->auth_seq = 0;
 399         pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
 400         pmlmeinfo->key_index = 0;
 401         pmlmeinfo->iv = 0;
 402
 403         pmlmeinfo->enc_algo = _NO_PRIVACY_;
 404         pmlmeinfo->authModeToggle = 0;
 405
 406         _rtw_memset(pmlmeinfo->chg_txt, 0, 128);
 407
 408         pmlmeinfo->slotTime = SHORT_SLOT_TIME;
 409         pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
 410
 411         pmlmeinfo->dialogToken = 0;
 412
 413         pmlmeext->action_public_rxseq = 0xffff;
 414         pmlmeext->action_public_dialog_token = 0xff;
 415 }
 416
 417 static int has_channel(RT_CHANNEL_INFO *channel_set,
 418                                            u8 chanset_size,
 419                                            u8 chan) {
 420         int i;
 421
 422         for (i = 0; i < chanset_size; i++) {
 423                 if (channel_set[i].ChannelNum == chan) {
 424                         return 1;
 425                 }
 426         }
 427
 428         return 0;
 429 }
 430
 431 static void init_channel_list(_adapter *padapter, RT_CHANNEL_INFO *channel_set,
 432                                                           u8 chanset_size,
 433                                                           struct p2p_channels *channel_list) {
 434
 435         struct p2p_oper_class_map op_class[] = {
 436                 { IEEE80211G,  81,   1,  13,  1, BW20 },
 437                 { IEEE80211G,  82,  14,  14,  1, BW20 },
 438 #if 0 /* Do not enable HT40 on 2 GHz */
 439                 { IEEE80211G,  83,   1,   9,  1, BW40PLUS },
 440                 { IEEE80211G,  84,   5,  13,  1, BW40MINUS },
 441 #endif
 442                 { IEEE80211A, 115,  36,  48,  4, BW20 },
 443                 { IEEE80211A, 116,  36,  44,  8, BW40PLUS },
 444                 { IEEE80211A, 117,  40,  48,  8, BW40MINUS },
 445                 { IEEE80211A, 124, 149, 161,  4, BW20 },
 446                 { IEEE80211A, 125, 149, 169,  4, BW20 },
 447                 { IEEE80211A, 126, 149, 157,  8, BW40PLUS },
 448                 { IEEE80211A, 127, 153, 161,  8, BW40MINUS },
 449                 { -1, 0, 0, 0, 0, BW20 }
 450         };
 451
 452         int cla, op;
 453
 454         cla = 0;
 455
 456         for (op = 0; op_class[op].op_class; op++) {
 457                 u8 ch;
 458                 struct p2p_oper_class_map *o = &op_class[op];
 459                 struct p2p_reg_class *reg = NULL;
 460
 461                 for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
 462                         if (!has_channel(channel_set, chanset_size, ch)) {
 463                                 continue;
 464                         }
 465
 466                         if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
 467                                 continue;
 468
 469                         if ((0 < (padapter->registrypriv.bw_mode & 0xf0)) &&
 470                                 ((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
 471                                 continue;
 472
 473                         if (reg == NULL) {
 474                                 reg = &channel_list->reg_class[cla];
 475                                 cla++;
 476                                 reg->reg_class = o->op_class;
 477                                 reg->channels = 0;
 478                         }
 479                         reg->channel[reg->channels] = ch;
 480                         reg->channels++;
 481                 }
 482         }
 483         channel_list->reg_classes = cla;
 484
 485 }
 486
 487 static u8 init_channel_set(_adapter* padapter, u8 ChannelPlan, RT_CHANNEL_INFO *channel_set)
 488 {
 489         u8      index,chanset_size = 0;
 490         u8      b5GBand = _FALSE, b2_4GBand = _FALSE;
 491         u8      Index2G = 0, Index5G=0;
 492
 493         if (!rtw_is_channel_plan_valid(ChannelPlan)) {
 494                 DBG_871X("ChannelPlan ID %x error !!!!!\n",ChannelPlan);
 495                 return chanset_size;
 496         }
 497
 498         _rtw_memset(channel_set, 0, sizeof(RT_CHANNEL_INFO)*MAX_CHANNEL_NUM);
 499
 500         if(IsSupported24G(padapter->registrypriv.wireless_mode))
 501         {
 502                 b2_4GBand = _TRUE;
 503                 if(RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
 504                         Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
 505                 else
 506                         Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
 507         }
 508
 509         if(IsSupported5G(padapter->registrypriv.wireless_mode))
 510         {
 511                 b5GBand = _TRUE;
 512                 if(RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
 513                         Index5G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index5G;
 514                 else
 515                         Index5G = RTW_ChannelPlanMap[ChannelPlan].Index5G;
 516         }
 517
 518         if(b2_4GBand)
 519         {
 520                 for(index=0;index<RTW_ChannelPlan2G[Index2G].Len;index++)
 521                 {
 522                         channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
 523
 524                         if(     (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||//Channel 1~11 is active, and 12~14 is passive
 525                                 (RT_CHANNEL_DOMAIN_GLOBAL_NULL == ChannelPlan)  )
 526                         {
 527                                 if(channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
 528                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 529                                 else if((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
 530                                         channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
 531                         }
 532                         else if(RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
 533                                 RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan ||
 534                                 RT_CHANNEL_DOMAIN_2G_WORLD == Index2G)// channel 12~13, passive scan
 535                         {
 536                                 if(channel_set[chanset_size].ChannelNum <= 11)
 537                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 538                                 else
 539                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 540                         }
 541                         else
 542                         {
 543                                 channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 544                         }
 545
 546                         chanset_size++;
 547                 }
 548         }
 549
 550         if(b5GBand)
 551         {
 552                 for(index=0;index<RTW_ChannelPlan5G[Index5G].Len;index++)
 553                 {
 554 #ifdef CONFIG_DFS
 555                         channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
 556                         if ( channel_set[chanset_size].ChannelNum <= 48
 557                                 || channel_set[chanset_size].ChannelNum >= 149 )
 558                         {
 559                                 if(RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)//passive scan for all 5G channels
 560                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 561                                 else
 562                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 563                         }
 564                         else
 565                         {
 566                                 channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 567                         }
 568                         chanset_size++;
 569 #else /* CONFIG_DFS */
 570                         if ( RTW_ChannelPlan5G[Index5G].Channel[index] <= 48
 571                                 || RTW_ChannelPlan5G[Index5G].Channel[index] >= 149 ) {
 572                                 channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
 573                                 if(RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)//passive scan for all 5G channels
 574                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 575                                 else
 576                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 577                                 DBG_871X("%s(): channel_set[%d].ChannelNum = %d\n", __FUNCTION__, chanset_size, channel_set[chanset_size].ChannelNum);
 578                                 chanset_size++;
 579                         }
 580 #endif /* CONFIG_DFS */
 581                 }
 582         }
 583
 584         Hal_ChannelPlanToRegulation(padapter, ChannelPlan);
 585
 586         DBG_871X("%s ChannelPlan ID %x Chan num:%d  \n",__FUNCTION__,ChannelPlan,chanset_size);
 587         return chanset_size;
 588 }
 589
 590 int     init_mlme_ext_priv(_adapter* padapter)
 591 {
 592         int     res = _SUCCESS;
 593         struct registry_priv* pregistrypriv = &padapter->registrypriv;
 594         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 595         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
 596         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 597
 598         // We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc().
 599         //_rtw_memset((u8 *)pmlmeext, 0, sizeof(struct mlme_ext_priv));
 600
 601         pmlmeext->padapter = padapter;
 602
 603         //fill_fwpriv(padapter, &(pmlmeext->fwpriv));
 604
 605         init_mlme_ext_priv_value(padapter);
 606         pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
 607
 608         init_mlme_ext_timer(padapter);
 609
 610 #ifdef CONFIG_AP_MODE
 611         init_mlme_ap_info(padapter);
 612 #endif
 613
 614         pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan,pmlmeext->channel_set);
 615         init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
 616         pmlmeext->last_scan_time = 0;
 617         pmlmeext->chan_scan_time = SURVEY_TO;
 618         pmlmeext->mlmeext_init = _TRUE;
 619
 620
 621 #ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
 622         pmlmeext->active_keep_alive_check = _TRUE;
 623 #else
 624         pmlmeext->active_keep_alive_check = _FALSE;
 625 #endif
 626
 627 #ifdef DBG_FIXED_CHAN
 628         pmlmeext->fixed_chan = 0xFF;
 629 #endif
 630
 631         return res;
 632
 633 }
 634
 635 void free_mlme_ext_priv (struct mlme_ext_priv *pmlmeext)
 636 {
 637         _adapter *padapter = pmlmeext->padapter;
 638
 639         if (!padapter)
 640                 return;
 641
 642         if (padapter->bDriverStopped == _TRUE)
 643         {
 644                 _cancel_timer_ex(&pmlmeext->survey_timer);
 645                 _cancel_timer_ex(&pmlmeext->link_timer);
 646                 //_cancel_timer_ex(&pmlmeext->ADDBA_timer);
 647         }
 648 }
 649
 650 static u8 cmp_pkt_chnl_diff(_adapter *padapter,u8* pframe,uint packet_len)
 651 {       // if the channel is same, return 0. else return channel differential
 652         uint len;
 653         u8 channel;
 654         u8 *p;
 655         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_, _DSSET_IE_, &len, packet_len - _BEACON_IE_OFFSET_);
 656         if (p)
 657         {
 658                 channel = *(p + 2);
 659                 if(padapter->mlmeextpriv.cur_channel >= channel)
 660                 {
 661                         return (padapter->mlmeextpriv.cur_channel - channel);
 662                 }
 663                 else
 664                 {
 665                         return (channel-padapter->mlmeextpriv.cur_channel);
 666                 }
 667         }
 668         else
 669         {
 670                 return 0;
 671         }
 672 }
 673
 674 static void _mgt_dispatcher(_adapter *padapter, struct mlme_handler *ptable, union recv_frame *precv_frame)
 675 {
 676         u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
 677         u8 *pframe = precv_frame->u.hdr.rx_data;
 678
 679           if(ptable->func)
 680         {
 681          //receive the frames that ra(a1) is my address or ra(a1) is bc address.
 682                 if (!_rtw_memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
 683                         !_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
 684                 {
 685                         return;
 686                 }
 687
 688                 ptable->func(padapter, precv_frame);
 689         }
 690
 691 }
 692
 693 void mgt_dispatcher(_adapter *padapter, union recv_frame *precv_frame)
 694 {
 695         int index;
 696         struct mlme_handler *ptable;
 697 #ifdef CONFIG_AP_MODE
 698         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 699 #endif //CONFIG_AP_MODE
 700         u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
 701         u8 *pframe = precv_frame->u.hdr.rx_data;
 702         struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));
 703         struct dvobj_priv *psdpriv = padapter->dvobj;
 704         struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
 705
 706         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 707                  ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
 708                   GetFrameType(pframe), GetFrameSubType(pframe)));
 709
 710 #if 0
 711         {
 712                 u8 *pbuf;
 713                 pbuf = GetAddr1Ptr(pframe);
 714                 DBG_871X("A1-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
 715                 pbuf = GetAddr2Ptr(pframe);
 716                 DBG_871X("A2-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
 717                 pbuf = GetAddr3Ptr(pframe);
 718                 DBG_871X("A3-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
 719         }
 720 #endif
 721
 722         if (GetFrameType(pframe) != WIFI_MGT_TYPE)
 723         {
 724                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
 725                 return;
 726         }
 727
 728         //receive the frames that ra(a1) is my address or ra(a1) is bc address.
 729         if (!_rtw_memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
 730                 !_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
 731         {
 732                 return;
 733         }
 734
 735         ptable = mlme_sta_tbl;
 736
 737         index = GetFrameSubType(pframe) >> 4;
 738
 739 #ifdef CONFIG_TDLS
 740         if((index << 4)==WIFI_ACTION){
 741                 //category==public (4), action==TDLS_DISCOVERY_RESPONSE
 742                 if(*(pframe+24)==0x04 && *(pframe+25)==TDLS_DISCOVERY_RESPONSE){
 743                         DBG_871X("recv tdls discovery response frame from "MAC_FMT"\n", MAC_ARG(GetAddr2Ptr(pframe)));
 744                         On_TDLS_Dis_Rsp(padapter, precv_frame);
 745                 }
 746         }
 747 #endif //CONFIG_TDLS
 748
 749         if (index >= (sizeof(mlme_sta_tbl) /sizeof(struct mlme_handler)))
 750         {
 751                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Currently we do not support reserved sub-fr-type=%d\n", index));
 752                 return;
 753         }
 754         ptable += index;
 755
 756 #if 1
 757         if (psta != NULL)
 758         {
 759                 if (GetRetry(pframe))
 760                 {
 761                         if (precv_frame->u.hdr.attrib.seq_num == psta->RxMgmtFrameSeqNum)
 762                         {
 763                                 /* drop the duplicate management frame */
 764                                 pdbgpriv->dbg_rx_dup_mgt_frame_drop_count++;
 765                                 DBG_871X("Drop duplicate management frame with seq_num = %d.\n", precv_frame->u.hdr.attrib.seq_num);
 766                                 return;
 767                         }
 768                 }
 769                 psta->RxMgmtFrameSeqNum = precv_frame->u.hdr.attrib.seq_num;
 770         }
 771 #else
 772
 773         if(GetRetry(pframe))
 774         {
 775                 //RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("drop due to decache!\n"));
 776                 //return;
 777         }
 778 #endif
 779
 780 #ifdef CONFIG_AP_MODE
 781         switch (GetFrameSubType(pframe))
 782         {
 783                 case WIFI_AUTH:
 784                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 785                                 ptable->func = &OnAuth;
 786                         else
 787                                 ptable->func = &OnAuthClient;
 788                         //pass through
 789                 case WIFI_ASSOCREQ:
 790                 case WIFI_REASSOCREQ:
 791                         _mgt_dispatcher(padapter, ptable, precv_frame);
 792 #ifdef CONFIG_HOSTAPD_MLME
 793                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 794                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 795 #endif
 796                         break;
 797                 case WIFI_PROBEREQ:
 798                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 799                         {
 800 #ifdef CONFIG_HOSTAPD_MLME
 801                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 802 #else
 803                                 _mgt_dispatcher(padapter, ptable, precv_frame);
 804 #endif
 805                         }
 806                         else
 807                                 _mgt_dispatcher(padapter, ptable, precv_frame);
 808                         break;
 809                 case WIFI_BEACON:
 810                         _mgt_dispatcher(padapter, ptable, precv_frame);
 811                         break;
 812                 case WIFI_ACTION:
 813                         //if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 814                         _mgt_dispatcher(padapter, ptable, precv_frame);
 815                         break;
 816                 default:
 817                         _mgt_dispatcher(padapter, ptable, precv_frame);
 818                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 819                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 820                         break;
 821         }
 822 #else
 823
 824         _mgt_dispatcher(padapter, ptable, precv_frame);
 825
 826 #endif
 827
 828 }
 829
 830 #ifdef CONFIG_P2P
 831 u32 p2p_listen_state_process(_adapter *padapter, unsigned char *da)
 832 {
 833         bool response = _TRUE;
 834
 835 #ifdef CONFIG_IOCTL_CFG80211
 836         if( padapter->wdinfo.driver_interface == DRIVER_CFG80211 )
 837         {
 838                 if(padapter->cfg80211_wdinfo.is_ro_ch == _FALSE
 839                         || rtw_get_oper_ch(padapter) != padapter->wdinfo.listen_channel
 840                         || adapter_wdev_data(padapter)->p2p_enabled == _FALSE
 841                         || padapter->mlmepriv.wps_probe_resp_ie == NULL
 842                         || padapter->mlmepriv.p2p_probe_resp_ie == NULL
 843                 )
 844                 {
 845 #ifdef CONFIG_DEBUG_CFG80211
 846                         DBG_871X("DON'T issue_probersp_p2p: p2p_enabled:%d, wps_probe_resp_ie:%p, p2p_probe_resp_ie:%p, ",
 847                                 adapter_wdev_data(padapter)->p2p_enabled,
 848                                 padapter->mlmepriv.wps_probe_resp_ie,
 849                                 padapter->mlmepriv.p2p_probe_resp_ie);
 850                         DBG_871X("is_ro_ch:%d, op_ch:%d, p2p_listen_channel:%d\n",
 851                                 padapter->cfg80211_wdinfo.is_ro_ch,
 852                                 rtw_get_oper_ch(padapter),
 853                                 padapter->wdinfo.listen_channel);
 854 #endif
 855                         response = _FALSE;
 856                 }
 857         }
 858         else
 859 #endif //CONFIG_IOCTL_CFG80211
 860         if( padapter->wdinfo.driver_interface == DRIVER_WEXT )
 861         {
 862                 //      do nothing if the device name is empty
 863                 if ( !padapter->wdinfo.device_name_len )
 864                 {
 865                         response        = _FALSE;
 866                 }
 867         }
 868
 869         if (response == _TRUE)
 870                 issue_probersp_p2p( padapter, da);
 871
 872         return _SUCCESS;
 873 }
 874 #endif //CONFIG_P2P
 875
 876
 877 /****************************************************************************
 878
 879 Following are the callback functions for each subtype of the management frames
 880
 881 *****************************************************************************/
 882
 883 unsigned int OnProbeReq(_adapter *padapter, union recv_frame *precv_frame)
 884 {
 885         unsigned int    ielen;
 886         unsigned char   *p;
 887         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 888         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 889         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 890         WLAN_BSSID_EX   *cur = &(pmlmeinfo->network);
 891         u8 *pframe = precv_frame->u.hdr.rx_data;
 892         uint len = precv_frame->u.hdr.len;
 893         u8 is_valid_p2p_probereq = _FALSE;
 894
 895 #ifdef CONFIG_ATMEL_RC_PATCH
 896         u8 *target_ie=NULL, *wps_ie=NULL;
 897         u8 *start;
 898         uint search_len = 0, wps_ielen = 0, target_ielen = 0;
 899         struct sta_info *psta;
 900         struct sta_priv *pstapriv = &padapter->stapriv;
 901 #endif
 902
 903
 904 #ifdef CONFIG_P2P
 905         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
 906         struct rx_pkt_attrib    *pattrib = &precv_frame->u.hdr.attrib;
 907         u8 wifi_test_chk_rate = 1;
 908
 909         if (    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
 910                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) &&
 911                 !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
 912                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
 913                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)
 914            )
 915         {
 916                 //      Commented by Albert 2011/03/17
 917                 //      mcs_rate = 0 -> CCK 1M rate
 918                 //      mcs_rate = 1 -> CCK 2M rate
 919                 //      mcs_rate = 2 -> CCK 5.5M rate
 920                 //      mcs_rate = 3 -> CCK 11M rate
 921                 //      In the P2P mode, the driver should not support the CCK rate
 922
 923                 //      Commented by Kurt 2012/10/16
 924                 //      IOT issue: Google Nexus7 use 1M rate to send p2p_probe_req after GO nego completed and Nexus7 is client
 925         if (padapter->registrypriv.wifi_spec == 1)
 926         {
 927             if ( pattrib->data_rate <= 3 )
 928             {
 929                 wifi_test_chk_rate = 0;
 930             }
 931         }
 932
 933                 if( wifi_test_chk_rate == 1 )
 934                 {
 935                         if((is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len)) == _TRUE)
 936                         {
 937                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE))
 938                                 {
 939                                         // FIXME
 940                                         if( padapter->wdinfo.driver_interface == DRIVER_WEXT )
 941                                                 report_survey_event(padapter, precv_frame);
 942
 943                                         p2p_listen_state_process( padapter,  get_sa(pframe));
 944
 945                                         return _SUCCESS;
 946                                 }
 947
 948                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
 949                                 {
 950                                         goto _continue;
 951                                 }
 952                         }
 953                 }
 954         }
 955
 956 _continue:
 957 #endif //CONFIG_P2P
 958
 959         if(check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 960         {
 961                 return _SUCCESS;
 962         }
 963
 964         if(check_fwstate(pmlmepriv, _FW_LINKED) == _FALSE &&
 965                 check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE)==_FALSE)
 966         {
 967                 return _SUCCESS;
 968         }
 969
 970
 971         //DBG_871X("+OnProbeReq\n");
 972
 973
 974 #ifdef CONFIG_ATMEL_RC_PATCH
 975                 if ((wps_ie = rtw_get_wps_ie(
 976                         pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_,
 977                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_,
 978                          NULL, &wps_ielen))) {
 979
 980                         target_ie = rtw_get_wps_attr_content( wps_ie, wps_ielen, WPS_ATTR_MANUFACTURER, NULL, &target_ielen);
 981                 }
 982                 if ((target_ie && (target_ielen == 4)) && (_TRUE ==_rtw_memcmp((void *)target_ie, "Ozmo",4 ))) {
 983                         //psta->flag_atmel_rc = 1;
 984                         unsigned char *sa_addr = get_sa(pframe);
 985                         printk("%s: Find Ozmo RC -- %02x:%02x:%02x:%02x:%02x:%02x  \n\n",
 986                                 __func__, *sa_addr, *(sa_addr+1), *(sa_addr+2), *(sa_addr+3), *(sa_addr+4), *(sa_addr+5));
 987                         _rtw_memcpy(  pstapriv->atmel_rc_pattern, get_sa(pframe), ETH_ALEN);
 988                 }
 989 #endif
 990
 991
 992 #ifdef CONFIG_AUTO_AP_MODE
 993         if(check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE &&
 994                         pmlmepriv->cur_network.join_res == _TRUE)
 995         {
 996                 _irqL   irqL;
 997                 struct sta_info *psta;
 998                 u8 *mac_addr, *peer_addr;
 999                 struct sta_priv *pstapriv = &padapter->stapriv;
1000                 u8 RC_OUI[4]={0x00,0xE0,0x4C,0x0A};
1001                 //EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2]
1002
1003                 p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _VENDOR_SPECIFIC_IE_, (int *)&ielen,
1004                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
1005
1006                 if(!p || ielen !=14)
1007                         goto _non_rc_device;
1008
1009                 if(!_rtw_memcmp(p+2, RC_OUI, sizeof(RC_OUI)))
1010                         goto _non_rc_device;
1011
1012                 if(!_rtw_memcmp(p+6, get_sa(pframe), ETH_ALEN))
1013                 {
1014                         DBG_871X("%s, do rc pairing ("MAC_FMT"), but mac addr mismatch!("MAC_FMT")\n", __FUNCTION__,
1015                                 MAC_ARG(get_sa(pframe)), MAC_ARG(p+6));
1016
1017                         goto _non_rc_device;
1018                 }
1019
1020                 DBG_871X("%s, got the pairing device("MAC_FMT")\n", __FUNCTION__,  MAC_ARG(get_sa(pframe)));
1021
1022                 //new a station
1023                 psta = rtw_get_stainfo(pstapriv, get_sa(pframe));
1024                 if (psta == NULL)
1025                 {
1026                         // allocate a new one
1027                         DBG_871X("going to alloc stainfo for rc="MAC_FMT"\n",  MAC_ARG(get_sa(pframe)));
1028                         psta = rtw_alloc_stainfo(pstapriv, get_sa(pframe));
1029                         if (psta == NULL)
1030                         {
1031                                 //TODO:
1032                                 DBG_871X(" Exceed the upper limit of supported clients...\n");
1033                                 return _SUCCESS;
1034                         }
1035
1036                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1037                         if (rtw_is_list_empty(&psta->asoc_list))
1038                         {
1039                                 psta->expire_to = pstapriv->expire_to;
1040                                 rtw_list_insert_tail(&psta->asoc_list, &pstapriv->asoc_list);
1041                                 pstapriv->asoc_list_cnt++;
1042                         }
1043                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1044
1045                         //generate pairing ID
1046                         mac_addr = myid(&(padapter->eeprompriv));
1047                         peer_addr = psta->hwaddr;
1048                         psta->pid = (u16)(((mac_addr[4]<<8) + mac_addr[5]) + ((peer_addr[4]<<8) + peer_addr[5]));
1049
1050                         //update peer stainfo
1051                         psta->isrc = _TRUE;
1052                         //psta->aid = 0;
1053                         //psta->mac_id = 2;
1054
1055                         /* get a unique AID */
1056                         if (psta->aid > 0) {
1057                                 DBG_871X("old AID %d\n", psta->aid);
1058                         } else {
1059                                 for (psta->aid = 1; psta->aid <= NUM_STA; psta->aid++)
1060                                         if (pstapriv->sta_aid[psta->aid - 1] == NULL)
1061                                                 break;
1062
1063                                 if (psta->aid > pstapriv->max_num_sta) {
1064                                         psta->aid = 0;
1065                                         DBG_871X("no room for more AIDs\n");
1066                                         return _SUCCESS;
1067                                 } else {
1068                                         pstapriv->sta_aid[psta->aid - 1] = psta;
1069                                         DBG_871X("allocate new AID = (%d)\n", psta->aid);
1070                                 }
1071                         }
1072
1073                         psta->qos_option = 1;
1074                         psta->bw_mode = CHANNEL_WIDTH_20;
1075                         psta->ieee8021x_blocked = _FALSE;
1076 #ifdef CONFIG_80211N_HT
1077                         psta->htpriv.ht_option = _TRUE;
1078                         psta->htpriv.ampdu_enable = _FALSE;
1079                         psta->htpriv.sgi_20m = _FALSE;
1080                         psta->htpriv.sgi_40m = _FALSE;
1081                         psta->htpriv.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
1082                         psta->htpriv.agg_enable_bitmap = 0x0;//reset
1083                         psta->htpriv.candidate_tid_bitmap = 0x0;//reset
1084 #endif
1085
1086                         rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, _TRUE);
1087
1088                         _rtw_memset((void*)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
1089
1090                         _enter_critical_bh(&psta->lock, &irqL);
1091                         psta->state |= _FW_LINKED;
1092                         _exit_critical_bh(&psta->lock, &irqL);
1093
1094                         report_add_sta_event(padapter, psta->hwaddr, psta->aid);
1095
1096                 }
1097
1098                 issue_probersp(padapter, get_sa(pframe), _FALSE);
1099
1100                 return _SUCCESS;
1101
1102         }
1103
1104 _non_rc_device:
1105
1106         return _SUCCESS;
1107
1108 #endif //CONFIG_AUTO_AP_MODE
1109
1110
1111 #ifdef CONFIG_CONCURRENT_MODE
1112         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
1113                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
1114         {
1115                 //don't process probe req
1116                 return _SUCCESS;
1117         }
1118 #endif
1119
1120         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
1121                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
1122
1123
1124         //check (wildcard) SSID
1125         if (p != NULL)
1126         {
1127                 if(is_valid_p2p_probereq == _TRUE)
1128                 {
1129                         goto _issue_probersp;
1130                 }
1131
1132                 if ( (ielen != 0 && _FALSE ==_rtw_memcmp((void *)(p+2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength))
1133                         || (ielen == 0 && pmlmeinfo->hidden_ssid_mode)
1134                 )
1135                 {
1136                         return _SUCCESS;
1137                 }
1138
1139 _issue_probersp:
1140                 if(((check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE &&
1141                         pmlmepriv->cur_network.join_res == _TRUE)) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
1142                 {
1143                         //DBG_871X("+issue_probersp during ap mode\n");
1144                         issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
1145                 }
1146
1147         }
1148
1149         return _SUCCESS;
1150
1151 }
1152
1153 unsigned int OnProbeRsp(_adapter *padapter, union recv_frame *precv_frame)
1154 {
1155         struct sta_info         *psta;
1156         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1157         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1158         struct sta_priv         *pstapriv = &padapter->stapriv;
1159         u8      *pframe = precv_frame->u.hdr.rx_data;
1160 #ifdef CONFIG_P2P
1161         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
1162 #endif
1163
1164
1165 #ifdef CONFIG_P2P
1166         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
1167         {
1168                 if ( _TRUE == pwdinfo->tx_prov_disc_info.benable )
1169                 {
1170                         if( _rtw_memcmp( pwdinfo->tx_prov_disc_info.peerIFAddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
1171                         {
1172                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT))
1173                                 {
1174                                         pwdinfo->tx_prov_disc_info.benable = _FALSE;
1175                                         issue_p2p_provision_request( padapter,
1176                                                                                                 pwdinfo->tx_prov_disc_info.ssid.Ssid,
1177                                                                                                 pwdinfo->tx_prov_disc_info.ssid.SsidLength,
1178                                                                                                 pwdinfo->tx_prov_disc_info.peerDevAddr );
1179                                 }
1180                                 else if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
1181                                 {
1182                                         pwdinfo->tx_prov_disc_info.benable = _FALSE;
1183                                         issue_p2p_provision_request( padapter,
1184                                                                                                 NULL,
1185                                                                                                 0,
1186                                                                                                 pwdinfo->tx_prov_disc_info.peerDevAddr );
1187                                 }
1188                         }
1189                 }
1190                 return _SUCCESS;
1191         }
1192         else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
1193         {
1194                 if ( _TRUE == pwdinfo->nego_req_info.benable )
1195                 {
1196                         DBG_871X( "[%s] P2P State is GONEGO ING!\n", __FUNCTION__ );
1197                         if( _rtw_memcmp( pwdinfo->nego_req_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
1198                         {
1199                                 pwdinfo->nego_req_info.benable = _FALSE;
1200                                 issue_p2p_GO_request( padapter, pwdinfo->nego_req_info.peerDevAddr);
1201                         }
1202                 }
1203         }
1204         else if( rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ ) )
1205         {
1206                 if ( _TRUE == pwdinfo->invitereq_info.benable )
1207                 {
1208                         DBG_871X( "[%s] P2P_STATE_TX_INVITE_REQ!\n", __FUNCTION__ );
1209                         if( _rtw_memcmp( pwdinfo->invitereq_info.peer_macaddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
1210                         {
1211                                 pwdinfo->invitereq_info.benable = _FALSE;
1212                                 issue_p2p_invitation_request( padapter, pwdinfo->invitereq_info.peer_macaddr );
1213                         }
1214                 }
1215         }
1216 #endif
1217
1218
1219         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
1220         {
1221                 report_survey_event(padapter, precv_frame);
1222 #ifdef CONFIG_CONCURRENT_MODE
1223                 report_survey_event(padapter->pbuddy_adapter, precv_frame);
1224 #endif
1225 #ifdef CONFIG_DUALMAC_CONCURRENT
1226                 dc_report_survey_event(padapter, precv_frame);
1227 #endif
1228                 return _SUCCESS;
1229         }
1230
1231         #if 0 //move to validate_recv_mgnt_frame
1232         if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
1233         {
1234                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
1235                 {
1236                         if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
1237                         {
1238                                 psta->sta_stats.rx_mgnt_pkts++;
1239                         }
1240                 }
1241         }
1242         #endif
1243
1244         return _SUCCESS;
1245
1246 }
1247
1248 unsigned int OnBeacon(_adapter *padapter, union recv_frame *precv_frame)
1249 {
1250         int cam_idx;
1251         struct sta_info *psta;
1252         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1253         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1254         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1255         struct sta_priv *pstapriv = &padapter->stapriv;
1256         u8 *pframe = precv_frame->u.hdr.rx_data;
1257         uint len = precv_frame->u.hdr.len;
1258         WLAN_BSSID_EX *pbss;
1259         int ret = _SUCCESS;
1260         u8 *p = NULL;
1261         u32 ielen = 0;
1262
1263 #ifdef CONFIG_ATTEMPT_TO_FIX_AP_BEACON_ERROR
1264         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_);
1265         if ((p != NULL) && (ielen > 0))
1266         {
1267                 if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D))
1268                 {
1269                         /* Invalid value 0x2D is detected in Extended Supported Rates (ESR) IE. Try to fix the IE length to avoid failed Beacon parsing. */
1270                         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)));
1271                         *(p + 1) = ielen - 1;
1272                 }
1273         }
1274 #endif
1275
1276         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
1277         {
1278                 report_survey_event(padapter, precv_frame);
1279 #ifdef CONFIG_CONCURRENT_MODE
1280                 report_survey_event(padapter->pbuddy_adapter, precv_frame);
1281 #endif
1282
1283 #ifdef CONFIG_DUALMAC_CONCURRENT
1284                 dc_report_survey_event(padapter, precv_frame);
1285 #endif
1286
1287                 return _SUCCESS;
1288         }
1289
1290         if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
1291         {
1292                 if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
1293                 {
1294                         //we should update current network before auth, or some IE is wrong
1295                         pbss = (WLAN_BSSID_EX*)rtw_malloc(sizeof(WLAN_BSSID_EX));
1296                         if (pbss) {
1297                                 if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
1298                                         struct beacon_keys recv_beacon;
1299
1300                                         update_network(&(pmlmepriv->cur_network.network), pbss, padapter, _TRUE);
1301                                         rtw_get_bcn_info(&(pmlmepriv->cur_network));
1302
1303                                         // update bcn keys
1304                                         if (rtw_get_bcn_keys(padapter, pframe, len, &recv_beacon) == _TRUE) {
1305                                                 DBG_871X("%s: beacon keys ready\n", __func__);
1306                                                 _rtw_memcpy(&pmlmepriv->cur_beacon_keys,
1307                                                         &recv_beacon, sizeof(recv_beacon));
1308                                                 pmlmepriv->new_beacon_cnts = 0;
1309                                         }
1310                                         else {
1311                                                 DBG_871X_LEVEL(_drv_err_, "%s: get beacon keys failed\n", __func__);
1312                                                 _rtw_memset(&pmlmepriv->cur_beacon_keys, 0, sizeof(recv_beacon));
1313                                                 pmlmepriv->new_beacon_cnts = 0;
1314                                         }
1315                                 }
1316                                 rtw_mfree((u8*)pbss, sizeof(WLAN_BSSID_EX));
1317                         }
1318
1319                         //check the vendor of the assoc AP
1320                         pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct rtw_ieee80211_hdr_3addr), len-sizeof(struct rtw_ieee80211_hdr_3addr));
1321
1322                         //update TSF Value
1323                         update_TSF(pmlmeext, pframe, len);
1324
1325                         //reset for adaptive_early_32k
1326                         pmlmeext->adaptive_tsf_done = _FALSE;
1327                         pmlmeext->DrvBcnEarly = 0xff;
1328                         pmlmeext->DrvBcnTimeOut = 0xff;
1329                         pmlmeext->bcn_cnt = 0;
1330                         _rtw_memset(pmlmeext->bcn_delay_cnt, 0, sizeof(pmlmeext->bcn_delay_cnt));
1331                         _rtw_memset(pmlmeext->bcn_delay_ratio, 0, sizeof(pmlmeext->bcn_delay_ratio));
1332
1333 #ifdef CONFIG_P2P_PS
1334                         process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
1335 #endif //CONFIG_P2P_PS
1336
1337                         //start auth
1338                         start_clnt_auth(padapter);
1339
1340                         return _SUCCESS;
1341                 }
1342
1343                 if(((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
1344                 {
1345                         if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
1346                         {
1347                                 #ifdef CONFIG_PATCH_JOIN_WRONG_CHANNEL
1348                                 //Merge from 8712 FW code
1349                                 if (cmp_pkt_chnl_diff(padapter,pframe,len) != 0)
1350                                 {            // join wrong channel, deauth and reconnect
1351                                         issue_deauth(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
1352
1353                                         report_del_sta_event(padapter,(&(pmlmeinfo->network))->MacAddress, WLAN_REASON_JOIN_WRONG_CHANNEL);
1354                                         pmlmeinfo->state &= (~WIFI_FW_ASSOC_SUCCESS);
1355                                         return _SUCCESS;
1356                                 }
1357                                 #endif //CONFIG_PATCH_JOIN_WRONG_CHANNEL
1358
1359                                 ret = rtw_check_bcn_info(padapter, pframe, len);
1360                                 if (!ret) {
1361                                                 DBG_871X_LEVEL(_drv_always_, "ap has changed, disconnect now\n ");
1362                                                 receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 0);
1363                                                 return _SUCCESS;
1364                                 }
1365                                 //update WMM, ERP in the beacon
1366                                 //todo: the timer is used instead of the number of the beacon received
1367                                 if ((sta_rx_pkts(psta) & 0xf) == 0)
1368                                 {
1369                                         //DBG_871X("update_bcn_info\n");
1370                                         update_beacon_info(padapter, pframe, len, psta);
1371                                 }
1372
1373                                 adaptive_early_32k(pmlmeext, pframe, len);
1374
1375 #ifdef CONFIG_DFS
1376                                 process_csa_ie(padapter, pframe, len);  //channel switch announcement
1377 #endif //CONFIG_DFS
1378
1379 #ifdef CONFIG_P2P_PS
1380                                 process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
1381 #endif //CONFIG_P2P_PS
1382
1383                                 #if 0 //move to validate_recv_mgnt_frame
1384                                 psta->sta_stats.rx_mgnt_pkts++;
1385                                 #endif
1386                         }
1387                 }
1388                 else if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
1389                 {
1390                         if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
1391                         {
1392                                 //update WMM, ERP in the beacon
1393                                 //todo: the timer is used instead of the number of the beacon received
1394                                 if ((sta_rx_pkts(psta) & 0xf) == 0)
1395                                 {
1396                                         //DBG_871X("update_bcn_info\n");
1397                                         update_beacon_info(padapter, pframe, len, psta);
1398                                 }
1399
1400                                 #if 0 //move to validate_recv_mgnt_frame
1401                                 psta->sta_stats.rx_mgnt_pkts++;
1402                                 #endif
1403                         }
1404                         else
1405                         {
1406                                 //allocate a new CAM entry for IBSS station
1407                                 if ((cam_idx = allocate_fw_sta_entry(padapter)) == NUM_STA)
1408                                 {
1409                                         goto _END_ONBEACON_;
1410                                 }
1411
1412                                 //get supported rate
1413                                 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)
1414                                 {
1415                                         pmlmeinfo->FW_sta_info[cam_idx].status = 0;
1416                                         goto _END_ONBEACON_;
1417                                 }
1418
1419                                 //update TSF Value
1420                                 update_TSF(pmlmeext, pframe, len);
1421
1422                                 //report sta add event
1423                                 report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
1424                         }
1425                 }
1426         }
1427
1428 _END_ONBEACON_:
1429
1430         return _SUCCESS;
1431
1432 }
1433
1434 unsigned int OnAuth(_adapter *padapter, union recv_frame *precv_frame)
1435 {
1436 #ifdef CONFIG_AP_MODE
1437         _irqL irqL;
1438         unsigned int    auth_mode, seq, ie_len;
1439         unsigned char   *sa, *p;
1440         u16     algorithm;
1441         int     status;
1442         static struct sta_info stat;
1443         struct  sta_info        *pstat=NULL;
1444         struct  sta_priv *pstapriv = &padapter->stapriv;
1445         struct security_priv *psecuritypriv = &padapter->securitypriv;
1446         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1447         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1448         u8 *pframe = precv_frame->u.hdr.rx_data;
1449         uint len = precv_frame->u.hdr.len;
1450         u8      offset = 0;
1451
1452
1453 #ifdef CONFIG_CONCURRENT_MODE
1454         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
1455                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
1456         {
1457                 //don't process auth request;
1458                 return _SUCCESS;
1459         }
1460 #endif //CONFIG_CONCURRENT_MODE
1461
1462         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
1463                 return _FAIL;
1464
1465         DBG_871X("+OnAuth\n");
1466
1467         sa = GetAddr2Ptr(pframe);
1468
1469         auth_mode = psecuritypriv->dot11AuthAlgrthm;
1470
1471         if (GetPrivacy(pframe))
1472         {
1473                 u8      *iv;
1474                 struct rx_pkt_attrib     *prxattrib = &(precv_frame->u.hdr.attrib);
1475
1476                 prxattrib->hdrlen = WLAN_HDR_A3_LEN;
1477                 prxattrib->encrypt = _WEP40_;
1478
1479                 iv = pframe+prxattrib->hdrlen;
1480                 prxattrib->key_index = ((iv[3]>>6)&0x3);
1481
1482                 prxattrib->iv_len = 4;
1483                 prxattrib->icv_len = 4;
1484
1485                 rtw_wep_decrypt(padapter, (u8 *)precv_frame);
1486
1487                 offset = 4;
1488         }
1489
1490         algorithm = le16_to_cpu(*(u16*)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset));
1491         seq     = le16_to_cpu(*(u16*)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2));
1492
1493         DBG_871X("auth alg=%x, seq=%X\n", algorithm, seq);
1494
1495         if (auth_mode == 2 &&
1496                         psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
1497                         psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
1498                 auth_mode = 0;
1499
1500         if ((algorithm > 0 && auth_mode == 0) ||        // rx a shared-key auth but shared not enabled
1501                 (algorithm == 0 && auth_mode == 1) )    // rx a open-system auth but shared-key is enabled
1502         {
1503                 DBG_871X("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n",
1504                         algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
1505
1506                 status = _STATS_NO_SUPP_ALG_;
1507
1508                 goto auth_fail;
1509         }
1510
1511 #if 0 //ACL control
1512         phead = &priv->wlan_acl_list;
1513         plist = phead->next;
1514         //check sa
1515         if (acl_mode == 1)              // 1: positive check, only those on acl_list can be connected.
1516                 res = FAIL;
1517         else
1518                 res = SUCCESS;
1519
1520         while(plist != phead)
1521         {
1522                 paclnode = list_entry(plist, struct rtw_wlan_acl_node, list);
1523                 plist = plist->next;
1524                 if (!memcmp((void *)sa, paclnode->addr, 6)) {
1525                         if (paclnode->mode & 2) { // deny
1526                                 res = FAIL;
1527                                 break;
1528                         }
1529                         else {
1530                                 res = SUCCESS;
1531                                 break;
1532                         }
1533                 }
1534         }
1535
1536         if (res != SUCCESS) {
1537                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,"auth abort because ACL!\n");
1538                 return FAIL;
1539         }
1540 #else
1541         if(rtw_access_ctrl(padapter, sa) == _FALSE)
1542         {
1543                 status = _STATS_UNABLE_HANDLE_STA_;
1544                 goto auth_fail;
1545         }
1546 #endif
1547
1548         pstat = rtw_get_stainfo(pstapriv, sa);
1549         if (pstat == NULL)
1550         {
1551
1552                 // allocate a new one
1553                 DBG_871X("going to alloc stainfo for sa="MAC_FMT"\n",  MAC_ARG(sa));
1554                 pstat = rtw_alloc_stainfo(pstapriv, sa);
1555                 if (pstat == NULL)
1556                 {
1557                         DBG_871X(" Exceed the upper limit of supported clients...\n");
1558                         status = _STATS_UNABLE_HANDLE_STA_;
1559                         goto auth_fail;
1560                 }
1561
1562                 pstat->state = WIFI_FW_AUTH_NULL;
1563                 pstat->auth_seq = 0;
1564
1565                 //pstat->flags = 0;
1566                 //pstat->capability = 0;
1567         }
1568         else
1569         {
1570
1571                 _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1572                 if(rtw_is_list_empty(&pstat->asoc_list)==_FALSE)
1573                 {
1574                         rtw_list_delete(&pstat->asoc_list);
1575                         pstapriv->asoc_list_cnt--;
1576                         if (pstat->expire_to > 0)
1577                         {
1578                                 //TODO: STA re_auth within expire_to
1579                         }
1580                 }
1581                 _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
1582
1583                 if (seq==1) {
1584                         //TODO: STA re_auth and auth timeout
1585                 }
1586         }
1587
1588         _enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
1589         if (rtw_is_list_empty(&pstat->auth_list))
1590         {
1591
1592                 rtw_list_insert_tail(&pstat->auth_list, &pstapriv->auth_list);
1593                 pstapriv->auth_list_cnt++;
1594         }
1595         _exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
1596
1597         if (pstat->auth_seq == 0)
1598                 pstat->expire_to = pstapriv->auth_to;
1599
1600
1601         if ((pstat->auth_seq + 1) != seq)
1602         {
1603                 DBG_871X("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1604                         seq, pstat->auth_seq+1);
1605                 status = _STATS_OUT_OF_AUTH_SEQ_;
1606                 goto auth_fail;
1607         }
1608
1609         if (algorithm==0 && (auth_mode == 0 || auth_mode == 2 || auth_mode == 3))
1610         {
1611                 if (seq == 1)
1612                 {
1613                         pstat->state &= ~WIFI_FW_AUTH_NULL;
1614                         pstat->state |= WIFI_FW_AUTH_SUCCESS;
1615                         pstat->expire_to = pstapriv->assoc_to;
1616                         pstat->authalg = algorithm;
1617                 }
1618                 else
1619                 {
1620                         DBG_871X("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1621                                 seq, pstat->auth_seq+1);
1622                         status = _STATS_OUT_OF_AUTH_SEQ_;
1623                         goto auth_fail;
1624                 }
1625         }
1626         else // shared system or auto authentication
1627         {
1628                 if (seq == 1)
1629                 {
1630                         //prepare for the challenging txt...
1631
1632                         //get_random_bytes((void *)pstat->chg_txt, 128);//TODO:
1633                         _rtw_memset((void *)pstat->chg_txt, 78, 128);
1634
1635                         pstat->state &= ~WIFI_FW_AUTH_NULL;
1636                         pstat->state |= WIFI_FW_AUTH_STATE;
1637                         pstat->authalg = algorithm;
1638                         pstat->auth_seq = 2;
1639                 }
1640                 else if (seq == 3)
1641                 {
1642                         //checking for challenging txt...
1643                         DBG_871X("checking for challenging txt...\n");
1644
1645                         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len,
1646                                         len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
1647
1648                         if((p==NULL) || (ie_len<=0))
1649                         {
1650                                 DBG_871X("auth rejected because challenge failure!(1)\n");
1651                                 status = _STATS_CHALLENGE_FAIL_;
1652                                 goto auth_fail;
1653                         }
1654
1655                         if (_rtw_memcmp((void *)(p + 2), pstat->chg_txt, 128))
1656                         {
1657                                 pstat->state &= (~WIFI_FW_AUTH_STATE);
1658                                 pstat->state |= WIFI_FW_AUTH_SUCCESS;
1659                                 // challenging txt is correct...
1660                                 pstat->expire_to =  pstapriv->assoc_to;
1661                         }
1662                         else
1663                         {
1664                                 DBG_871X("auth rejected because challenge failure!\n");
1665                                 status = _STATS_CHALLENGE_FAIL_;
1666                                 goto auth_fail;
1667                         }
1668                 }
1669                 else
1670                 {
1671                         DBG_871X("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1672                                 seq, pstat->auth_seq+1);
1673                         status = _STATS_OUT_OF_AUTH_SEQ_;
1674                         goto auth_fail;
1675                 }
1676         }
1677
1678
1679         // Now, we are going to issue_auth...
1680         pstat->auth_seq = seq + 1;
1681
1682 #ifdef CONFIG_NATIVEAP_MLME
1683         issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
1684 #endif
1685
1686         if (pstat->state & WIFI_FW_AUTH_SUCCESS)
1687                 pstat->auth_seq = 0;
1688
1689
1690         return _SUCCESS;
1691
1692 auth_fail:
1693
1694         if(pstat)
1695                 rtw_free_stainfo(padapter , pstat);
1696
1697         pstat = &stat;
1698         _rtw_memset((char *)pstat, '\0', sizeof(stat));
1699         pstat->auth_seq = 2;
1700         _rtw_memcpy(pstat->hwaddr, sa, 6);
1701
1702 #ifdef CONFIG_NATIVEAP_MLME
1703         issue_auth(padapter, pstat, (unsigned short)status);
1704 #endif
1705
1706 #endif
1707         return _FAIL;
1708
1709 }
1710
1711 unsigned int OnAuthClient(_adapter *padapter, union recv_frame *precv_frame)
1712 {
1713         unsigned int    seq, len, status, algthm, offset;
1714         unsigned char   *p;
1715         unsigned int    go2asoc = 0;
1716         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1717         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1718         u8 *pframe = precv_frame->u.hdr.rx_data;
1719         uint pkt_len = precv_frame->u.hdr.len;
1720
1721         DBG_871X("%s\n", __FUNCTION__);
1722
1723         //check A1 matches or not
1724         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
1725                 return _SUCCESS;
1726
1727         if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
1728                 return _SUCCESS;
1729
1730         offset = (GetPrivacy(pframe))? 4: 0;
1731
1732         algthm  = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset));
1733         seq     = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2));
1734         status  = le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 4));
1735
1736         if (status != 0)
1737         {
1738                 DBG_871X("clnt auth fail, status: %d\n", status);
1739                 if(status == 13)//&& pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto)
1740                 {
1741                         if(pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1742                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
1743                         else
1744                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
1745                         //pmlmeinfo->reauth_count = 0;
1746                 }
1747
1748                 set_link_timer(pmlmeext, 1);
1749                 goto authclnt_fail;
1750         }
1751
1752         if (seq == 2)
1753         {
1754                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1755                 {
1756                          // legendary shared system
1757                         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
1758                                 pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
1759
1760                         if (p == NULL)
1761                         {
1762                                 //DBG_871X("marc: no challenge text?\n");
1763                                 goto authclnt_fail;
1764                         }
1765
1766                         _rtw_memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
1767                         pmlmeinfo->auth_seq = 3;
1768                         issue_auth(padapter, NULL, 0);
1769                         set_link_timer(pmlmeext, REAUTH_TO);
1770
1771                         return _SUCCESS;
1772                 }
1773                 else
1774                 {
1775                         // open system
1776                         go2asoc = 1;
1777                 }
1778         }
1779         else if (seq == 4)
1780         {
1781                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1782                 {
1783                         go2asoc = 1;
1784                 }
1785                 else
1786                 {
1787                         goto authclnt_fail;
1788                 }
1789         }
1790         else
1791         {
1792                 // this is also illegal
1793                 //DBG_871X("marc: clnt auth failed due to illegal seq=%x\n", seq);
1794                 goto authclnt_fail;
1795         }
1796
1797         if (go2asoc)
1798         {
1799                 DBG_871X_LEVEL(_drv_always_, "auth success, start assoc\n");
1800                 start_clnt_assoc(padapter);
1801                 return _SUCCESS;
1802         }
1803
1804 authclnt_fail:
1805
1806         //pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE);
1807
1808         return _FAIL;
1809
1810 }
1811
1812 unsigned int OnAssocReq(_adapter *padapter, union recv_frame *precv_frame)
1813 {
1814 #ifdef CONFIG_AP_MODE
1815         _irqL irqL;
1816         u16 capab_info, listen_interval;
1817         struct rtw_ieee802_11_elems elems;
1818         struct sta_info *pstat;
1819         unsigned char           reassoc, *p, *pos, *wpa_ie;
1820         unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
1821         int             i, ie_len, wpa_ie_len, left;
1822         unsigned char           supportRate[16];
1823         int                                     supportRateNum;
1824         unsigned short          status = _STATS_SUCCESSFUL_;
1825         unsigned short          frame_type, ie_offset=0;
1826         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1827         struct security_priv *psecuritypriv = &padapter->securitypriv;
1828         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
1829         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1830         WLAN_BSSID_EX   *cur = &(pmlmeinfo->network);
1831         struct sta_priv *pstapriv = &padapter->stapriv;
1832         u8 *pframe = precv_frame->u.hdr.rx_data;
1833         uint pkt_len = precv_frame->u.hdr.len;
1834 #ifdef CONFIG_P2P
1835         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
1836         u8 p2p_status_code = P2P_STATUS_SUCCESS;
1837         u8 *p2pie;
1838         u32 p2pielen = 0;
1839 #ifdef CONFIG_WFD
1840         u8      wfd_ie[ 128 ] = { 0x00 };
1841         u32     wfd_ielen = 0;
1842 #endif // CONFIG_WFD
1843 #endif //CONFIG_P2P
1844
1845 #ifdef CONFIG_CONCURRENT_MODE
1846         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
1847                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
1848         {
1849                 //don't process assoc request;
1850                 return _SUCCESS;
1851         }
1852 #endif //CONFIG_CONCURRENT_MODE
1853
1854         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
1855                 return _FAIL;
1856
1857         frame_type = GetFrameSubType(pframe);
1858         if (frame_type == WIFI_ASSOCREQ)
1859         {
1860                 reassoc = 0;
1861                 ie_offset = _ASOCREQ_IE_OFFSET_;
1862         }
1863         else // WIFI_REASSOCREQ
1864         {
1865                 reassoc = 1;
1866                 ie_offset = _REASOCREQ_IE_OFFSET_;
1867         }
1868
1869
1870         if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
1871                 DBG_871X("handle_assoc(reassoc=%d) - too short payload (len=%lu)"
1872                        "\n", reassoc, (unsigned long)pkt_len);
1873                 return _FAIL;
1874         }
1875
1876         pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
1877         if (pstat == (struct sta_info *)NULL)
1878         {
1879                 status = _RSON_CLS2_;
1880                 goto asoc_class2_error;
1881         }
1882
1883         capab_info = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN);
1884         //capab_info = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
1885         //listen_interval = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN+2));
1886         listen_interval = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN+2);
1887
1888         left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
1889         pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
1890
1891
1892         DBG_871X("%s\n", __FUNCTION__);
1893
1894         // check if this stat has been successfully authenticated/assocated
1895         if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS))
1896         {
1897                 if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS))
1898                 {
1899                         status = _RSON_CLS2_;
1900                         goto asoc_class2_error;
1901                 }
1902                 else
1903                 {
1904                         pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
1905                         pstat->state |= WIFI_FW_ASSOC_STATE;
1906                 }
1907         }
1908         else
1909         {
1910                 pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
1911                 pstat->state |= WIFI_FW_ASSOC_STATE;
1912         }
1913
1914
1915 #if 0// todo:tkip_countermeasures
1916         if (hapd->tkip_countermeasures) {
1917                 resp = WLAN_REASON_MICHAEL_MIC_FAILURE;
1918                 goto fail;
1919         }
1920 #endif
1921
1922         pstat->capability = capab_info;
1923
1924 #if 0//todo:
1925         //check listen_interval
1926         if (listen_interval > hapd->conf->max_listen_interval) {
1927                 hostapd_logger(hapd, mgmt->sa, HOSTAPD_MODULE_IEEE80211,
1928                                HOSTAPD_LEVEL_DEBUG,
1929                                "Too large Listen Interval (%d)",
1930                                listen_interval);
1931                 resp = WLAN_STATUS_ASSOC_DENIED_LISTEN_INT_TOO_LARGE;
1932                 goto fail;
1933         }
1934
1935         pstat->listen_interval = listen_interval;
1936 #endif
1937
1938         //now parse all ieee802_11 ie to point to elems
1939         if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
1940             !elems.ssid) {
1941                 DBG_871X("STA " MAC_FMT " sent invalid association request\n",
1942                        MAC_ARG(pstat->hwaddr));
1943                 status = _STATS_FAILURE_;
1944                 goto OnAssocReqFail;
1945         }
1946
1947
1948         // now we should check all the fields...
1949         // checking SSID
1950         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
1951                 pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1952         if (p == NULL)
1953         {
1954                 status = _STATS_FAILURE_;
1955         }
1956
1957         if (ie_len == 0) // broadcast ssid, however it is not allowed in assocreq
1958                 status = _STATS_FAILURE_;
1959         else
1960         {
1961                 // check if ssid match
1962                 if (!_rtw_memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
1963                         status = _STATS_FAILURE_;
1964
1965                 if (ie_len != cur->Ssid.SsidLength)
1966                         status = _STATS_FAILURE_;
1967         }
1968
1969         if(_STATS_SUCCESSFUL_ != status)
1970                 goto OnAssocReqFail;
1971
1972         // check if the supported rate is ok
1973         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1974         if (p == NULL) {
1975                 DBG_871X("Rx a sta assoc-req which supported rate is empty!\n");
1976                 // use our own rate set as statoin used
1977                 //_rtw_memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN);
1978                 //supportRateNum = AP_BSSRATE_LEN;
1979
1980                 status = _STATS_FAILURE_;
1981                 goto OnAssocReqFail;
1982         }
1983         else {
1984                 _rtw_memcpy(supportRate, p+2, ie_len);
1985                 supportRateNum = ie_len;
1986
1987                 p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_ , &ie_len,
1988                                 pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1989                 if (p !=  NULL) {
1990
1991                         if(supportRateNum<=sizeof(supportRate))
1992                         {
1993                                 _rtw_memcpy(supportRate+supportRateNum, p+2, ie_len);
1994                                 supportRateNum += ie_len;
1995                         }
1996                 }
1997         }
1998
1999         //todo: mask supportRate between AP & STA -> move to update raid
2000         //get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0);
2001
2002         //update station supportRate
2003         pstat->bssratelen = supportRateNum;
2004         _rtw_memcpy(pstat->bssrateset, supportRate, supportRateNum);
2005         UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
2006
2007         //check RSN/WPA/WPS
2008         pstat->dot8021xalg = 0;
2009         pstat->wpa_psk = 0;
2010         pstat->wpa_group_cipher = 0;
2011         pstat->wpa2_group_cipher = 0;
2012         pstat->wpa_pairwise_cipher = 0;
2013         pstat->wpa2_pairwise_cipher = 0;
2014         _rtw_memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
2015         if((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
2016
2017                 int group_cipher=0, pairwise_cipher=0;
2018
2019                 wpa_ie = elems.rsn_ie;
2020                 wpa_ie_len = elems.rsn_ie_len;
2021
2022                 if(rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS)
2023                 {
2024                         pstat->dot8021xalg = 1;//psk,  todo:802.1x
2025                         pstat->wpa_psk |= BIT(1);
2026
2027                         pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
2028                         pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
2029
2030                         if(!pstat->wpa2_group_cipher)
2031                                 status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
2032
2033                         if(!pstat->wpa2_pairwise_cipher)
2034                                 status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
2035                 }
2036                 else
2037                 {
2038                         status = WLAN_STATUS_INVALID_IE;
2039                 }
2040
2041         } else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
2042
2043                 int group_cipher=0, pairwise_cipher=0;
2044
2045                 wpa_ie = elems.wpa_ie;
2046                 wpa_ie_len = elems.wpa_ie_len;
2047
2048                 if(rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS)
2049                 {
2050                         pstat->dot8021xalg = 1;//psk,  todo:802.1x
2051                         pstat->wpa_psk |= BIT(0);
2052
2053                         pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
2054                         pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
2055
2056                         if(!pstat->wpa_group_cipher)
2057                                 status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
2058
2059                         if(!pstat->wpa_pairwise_cipher)
2060                                 status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
2061
2062                 }
2063                 else
2064                 {
2065                         status = WLAN_STATUS_INVALID_IE;
2066                 }
2067
2068         } else {
2069                 wpa_ie = NULL;
2070                 wpa_ie_len = 0;
2071         }
2072
2073         if(_STATS_SUCCESSFUL_ != status)
2074                 goto OnAssocReqFail;
2075
2076         pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
2077         //if (hapd->conf->wps_state && wpa_ie == NULL) { //todo: to check ap if supporting WPS
2078         if(wpa_ie == NULL) {
2079                 if (elems.wps_ie) {
2080                         DBG_871X("STA included WPS IE in "
2081                                    "(Re)Association Request - assume WPS is "
2082                                    "used\n");
2083                         pstat->flags |= WLAN_STA_WPS;
2084                         //wpabuf_free(sta->wps_ie);
2085                         //sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4,
2086                         //                              elems.wps_ie_len - 4);
2087                 } else {
2088                         DBG_871X("STA did not include WPA/RSN IE "
2089                                    "in (Re)Association Request - possible WPS "
2090                                    "use\n");
2091                         pstat->flags |= WLAN_STA_MAYBE_WPS;
2092                 }
2093
2094
2095                 // AP support WPA/RSN, and sta is going to do WPS, but AP is not ready
2096                 // that the selected registrar of AP is _FLASE
2097                 if((psecuritypriv->wpa_psk >0)
2098                         && (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS)))
2099                 {
2100                         if(pmlmepriv->wps_beacon_ie)
2101                         {
2102                                 u8 selected_registrar = 0;
2103
2104                                 rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR , &selected_registrar, NULL);
2105
2106                                 if(!selected_registrar)
2107                                 {
2108                                         DBG_871X("selected_registrar is _FALSE , or AP is not ready to do WPS\n");
2109
2110                                         status = _STATS_UNABLE_HANDLE_STA_;
2111
2112                                         goto OnAssocReqFail;
2113                                 }
2114                         }
2115                 }
2116
2117         }
2118         else
2119         {
2120                 int copy_len;
2121
2122                 if(psecuritypriv->wpa_psk == 0)
2123                 {
2124                         DBG_871X("STA " MAC_FMT ": WPA/RSN IE in association "
2125                         "request, but AP don't support WPA/RSN\n", MAC_ARG(pstat->hwaddr));
2126
2127                         status = WLAN_STATUS_INVALID_IE;
2128
2129                         goto OnAssocReqFail;
2130
2131                 }
2132
2133                 if (elems.wps_ie) {
2134                         DBG_871X("STA included WPS IE in "
2135                                    "(Re)Association Request - WPS is "
2136                                    "used\n");
2137                         pstat->flags |= WLAN_STA_WPS;
2138                         copy_len=0;
2139                 }
2140                 else
2141                 {
2142                         copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)):(wpa_ie_len+2);
2143                 }
2144
2145
2146                 if(copy_len>0)
2147                         _rtw_memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
2148
2149         }
2150
2151
2152         // check if there is WMM IE & support WWM-PS
2153         pstat->flags &= ~WLAN_STA_WME;
2154         pstat->qos_option = 0;
2155         pstat->qos_info = 0;
2156         pstat->has_legacy_ac = _TRUE;
2157         pstat->uapsd_vo = 0;
2158         pstat->uapsd_vi = 0;
2159         pstat->uapsd_be = 0;
2160         pstat->uapsd_bk = 0;
2161         if (pmlmepriv->qospriv.qos_option)
2162         {
2163                 p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
2164                 for (;;)
2165                 {
2166                         p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
2167                         if (p != NULL) {
2168                                 if (_rtw_memcmp(p+2, WMM_IE, 6)) {
2169
2170                                         pstat->flags |= WLAN_STA_WME;
2171
2172                                         pstat->qos_option = 1;
2173                                         pstat->qos_info = *(p+8);
2174
2175                                         pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
2176
2177                                         if((pstat->qos_info&0xf) !=0xf)
2178                                                 pstat->has_legacy_ac = _TRUE;
2179                                         else
2180                                                 pstat->has_legacy_ac = _FALSE;
2181
2182                                         if(pstat->qos_info&0xf)
2183                                         {
2184                                                 if(pstat->qos_info&BIT(0))
2185                                                         pstat->uapsd_vo = BIT(0)|BIT(1);
2186                                                 else
2187                                                         pstat->uapsd_vo = 0;
2188
2189                                                 if(pstat->qos_info&BIT(1))
2190                                                         pstat->uapsd_vi = BIT(0)|BIT(1);
2191                                                 else
2192                                                         pstat->uapsd_vi = 0;
2193
2194                                                 if(pstat->qos_info&BIT(2))
2195                                                         pstat->uapsd_bk = BIT(0)|BIT(1);
2196                                                 else
2197                                                         pstat->uapsd_bk = 0;
2198
2199                                                 if(pstat->qos_info&BIT(3))
2200                                                         pstat->uapsd_be = BIT(0)|BIT(1);
2201                                                 else
2202                                                         pstat->uapsd_be = 0;
2203
2204                                         }
2205
2206                                         break;
2207                                 }
2208                         }
2209                         else {
2210                                 break;
2211                         }
2212                         p = p + ie_len + 2;
2213                 }
2214         }
2215
2216
2217 #ifdef CONFIG_80211N_HT
2218         /* save HT capabilities in the sta object */
2219         _rtw_memset(&pstat->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));
2220         if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap))
2221         {
2222                 pstat->flags |= WLAN_STA_HT;
2223
2224                 pstat->flags |= WLAN_STA_WME;
2225
2226                 _rtw_memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap));
2227
2228         } else
2229                 pstat->flags &= ~WLAN_STA_HT;
2230
2231
2232         if((pmlmepriv->htpriv.ht_option == _FALSE) && (pstat->flags&WLAN_STA_HT))
2233         {
2234                 status = _STATS_FAILURE_;
2235                 goto OnAssocReqFail;
2236         }
2237
2238
2239         if ((pstat->flags & WLAN_STA_HT) &&
2240                     ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
2241                       (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP)))
2242         {
2243                 DBG_871X("HT: " MAC_FMT " tried to "
2244                                    "use TKIP with HT association\n", MAC_ARG(pstat->hwaddr));
2245
2246                 //status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY;
2247                 //goto OnAssocReqFail;
2248         }
2249 #endif /* CONFIG_80211N_HT */
2250
2251 #ifdef CONFIG_80211AC_VHT
2252         _rtw_memset(&pstat->vhtpriv, 0, sizeof(struct vht_priv));
2253         if (elems.vht_capabilities && elems.vht_capabilities_len == 12) {
2254                 pstat->flags |= WLAN_STA_VHT;
2255
2256                 _rtw_memcpy(pstat->vhtpriv.vht_cap, elems.vht_capabilities, 12);
2257
2258                 if (elems.vht_op_mode_notify && elems.vht_op_mode_notify_len == 1) {
2259                         _rtw_memcpy(&pstat->vhtpriv.vht_op_mode_notify, elems.vht_op_mode_notify, 1);
2260                 }
2261                 else // for Frame without Operating Mode notify ie; default: 80M
2262                 {
2263                         pstat->vhtpriv.vht_op_mode_notify = CHANNEL_WIDTH_80;
2264                 }
2265         }
2266         else {
2267                 pstat->flags &= ~WLAN_STA_VHT;
2268         }
2269
2270         if((pmlmepriv->vhtpriv.vht_option == _FALSE) && (pstat->flags&WLAN_STA_VHT))
2271         {
2272                 status = _STATS_FAILURE_;
2273                 goto OnAssocReqFail;
2274         }
2275 #endif /* CONFIG_80211AC_VHT */
2276
2277        //
2278        //if (hapd->iface->current_mode->mode == HOSTAPD_MODE_IEEE80211G)//?
2279         pstat->flags |= WLAN_STA_NONERP;
2280         for (i = 0; i < pstat->bssratelen; i++) {
2281                 if ((pstat->bssrateset[i] & 0x7f) > 22) {
2282                         pstat->flags &= ~WLAN_STA_NONERP;
2283                         break;
2284                 }
2285         }
2286
2287         if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
2288                 pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
2289         else
2290                 pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
2291
2292
2293
2294         if (status != _STATS_SUCCESSFUL_)
2295                 goto OnAssocReqFail;
2296
2297 #ifdef CONFIG_P2P
2298         pstat->is_p2p_device = _FALSE;
2299         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2300         {
2301                 if( (p2pie=rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , NULL, &p2pielen)))
2302                 {
2303                         pstat->is_p2p_device = _TRUE;
2304                         if((p2p_status_code=(u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat))>0)
2305                         {
2306                                 pstat->p2p_status_code = p2p_status_code;
2307                                 status = _STATS_CAP_FAIL_;
2308                                 goto OnAssocReqFail;
2309                         }
2310                 }
2311 #ifdef CONFIG_WFD
2312                 if(rtw_get_wfd_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , wfd_ie, &wfd_ielen ))
2313                 {
2314                         u8      attr_content[ 10 ] = { 0x00 };
2315                         u32     attr_contentlen = 0;
2316
2317                         DBG_8192C( "[%s] WFD IE Found!!\n", __FUNCTION__ );
2318                         rtw_get_wfd_attr_content( wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
2319                         if ( attr_contentlen )
2320                         {
2321                                 pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16( attr_content + 2 );
2322                                 DBG_8192C( "[%s] Peer PORT NUM = %d\n", __FUNCTION__, pwdinfo->wfd_info->peer_rtsp_ctrlport );
2323                         }
2324                 }
2325 #endif
2326         }
2327         pstat->p2p_status_code = p2p_status_code;
2328 #endif //CONFIG_P2P
2329
2330         //TODO: identify_proprietary_vendor_ie();
2331         // Realtek proprietary IE
2332         // identify if this is Broadcom sta
2333         // identify if this is ralink sta
2334         // Customer proprietary IE
2335
2336
2337
2338         /* get a unique AID */
2339         if (pstat->aid > 0) {
2340                 DBG_871X("  old AID %d\n", pstat->aid);
2341         } else {
2342                 for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
2343                         if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
2344                                 break;
2345
2346                 //if (pstat->aid > NUM_STA) {
2347                 if (pstat->aid > pstapriv->max_num_sta) {
2348
2349                         pstat->aid = 0;
2350
2351                         DBG_871X("  no room for more AIDs\n");
2352
2353                         status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
2354
2355                         goto OnAssocReqFail;
2356
2357
2358                 } else {
2359                         pstapriv->sta_aid[pstat->aid - 1] = pstat;
2360                         DBG_871X("allocate new AID = (%d)\n", pstat->aid);
2361                 }
2362         }
2363
2364
2365         pstat->state &= (~WIFI_FW_ASSOC_STATE);
2366         pstat->state |= WIFI_FW_ASSOC_SUCCESS;
2367
2368         _enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
2369         if (!rtw_is_list_empty(&pstat->auth_list))
2370         {
2371                 rtw_list_delete(&pstat->auth_list);
2372                 pstapriv->auth_list_cnt--;
2373         }
2374         _exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
2375
2376         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2377         if (rtw_is_list_empty(&pstat->asoc_list))
2378         {
2379                 pstat->expire_to = pstapriv->expire_to;
2380                 rtw_list_insert_tail(&pstat->asoc_list, &pstapriv->asoc_list);
2381                 pstapriv->asoc_list_cnt++;
2382         }
2383         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2384
2385         // now the station is qualified to join our BSS...
2386         if(pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_==status))
2387         {
2388 #ifdef CONFIG_NATIVEAP_MLME
2389                 //.1 bss_cap_update & sta_info_update
2390                 bss_cap_update_on_sta_join(padapter, pstat);
2391                 sta_info_update(padapter, pstat);
2392
2393                 //.2 issue assoc rsp before notify station join event.
2394                 if (frame_type == WIFI_ASSOCREQ)
2395                         issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
2396                 else
2397                         issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
2398
2399 #ifdef CONFIG_IOCTL_CFG80211
2400                 _enter_critical_bh(&pstat->lock, &irqL);
2401                 if(pstat->passoc_req)
2402                 {
2403                         rtw_mfree(pstat->passoc_req, pstat->assoc_req_len);
2404                         pstat->passoc_req = NULL;
2405                         pstat->assoc_req_len = 0;
2406                 }
2407
2408                 pstat->passoc_req =  rtw_zmalloc(pkt_len);
2409                 if(pstat->passoc_req)
2410                 {
2411                         _rtw_memcpy(pstat->passoc_req, pframe, pkt_len);
2412                         pstat->assoc_req_len = pkt_len;
2413                 }
2414                 _exit_critical_bh(&pstat->lock, &irqL);
2415 #endif //CONFIG_IOCTL_CFG80211
2416
2417                 //.3-(1) report sta add event
2418                 report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
2419
2420 #endif //CONFIG_NATIVEAP_MLME
2421         }
2422
2423         return _SUCCESS;
2424
2425 asoc_class2_error:
2426
2427 #ifdef CONFIG_NATIVEAP_MLME
2428         issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
2429 #endif
2430
2431         return _FAIL;
2432
2433 OnAssocReqFail:
2434
2435
2436 #ifdef CONFIG_NATIVEAP_MLME
2437         pstat->aid = 0;
2438         if (frame_type == WIFI_ASSOCREQ)
2439                 issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
2440         else
2441                 issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
2442 #endif
2443
2444
2445 #endif /* CONFIG_AP_MODE */
2446
2447         return _FAIL;
2448
2449 }
2450
2451 unsigned int OnAssocRsp(_adapter *padapter, union recv_frame *precv_frame)
2452 {
2453         uint i;
2454         int res;
2455         unsigned short  status;
2456         PNDIS_802_11_VARIABLE_IEs       pIE;
2457         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2458         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2459         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2460         //WLAN_BSSID_EX                 *cur_network = &(pmlmeinfo->network);
2461         u8 *pframe = precv_frame->u.hdr.rx_data;
2462         uint pkt_len = precv_frame->u.hdr.len;
2463         PNDIS_802_11_VARIABLE_IEs       pWapiIE = NULL;
2464
2465         DBG_871X("%s\n", __FUNCTION__);
2466
2467         //check A1 matches or not
2468         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
2469                 return _SUCCESS;
2470
2471         if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
2472                 return _SUCCESS;
2473
2474         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
2475                 return _SUCCESS;
2476
2477         _cancel_timer_ex(&pmlmeext->link_timer);
2478
2479         //status
2480         if ((status = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 2))) > 0)
2481         {
2482                 DBG_871X("assoc reject, status code: %d\n", status);
2483                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
2484                 res = -4;
2485                 goto report_assoc_result;
2486         }
2487
2488         //get capabilities
2489         pmlmeinfo->capability = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2490
2491         //set slot time
2492         pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10))? 9: 20;
2493
2494         //AID
2495         res = pmlmeinfo->aid = (int)(le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
2496
2497         //following are moved to join event callback function
2498         //to handle HT, WMM, rate adaptive, update MAC reg
2499         //for not to handle the synchronous IO in the tasklet
2500         for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;)
2501         {
2502                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + i);
2503
2504                 switch (pIE->ElementID)
2505                 {
2506                         case _VENDOR_SPECIFIC_IE_:
2507                                 if (_rtw_memcmp(pIE->data, WMM_PARA_OUI, 6))    //WMM
2508                                 {
2509                                         WMM_param_handler(padapter, pIE);
2510                                 }
2511 #if defined(CONFIG_P2P) && defined(CONFIG_WFD)
2512                                 else if ( _rtw_memcmp(pIE->data, WFD_OUI, 4))           //WFD
2513                                 {
2514                                         DBG_871X( "[%s] Found WFD IE\n", __FUNCTION__ );
2515                                         WFD_info_handler( padapter, pIE );
2516                                 }
2517 #endif
2518                                 break;
2519
2520 #ifdef CONFIG_WAPI_SUPPORT
2521                         case _WAPI_IE_:
2522                                 pWapiIE = pIE;
2523                                 break;
2524 #endif
2525
2526                         case _HT_CAPABILITY_IE_:        //HT caps
2527                                 HT_caps_handler(padapter, pIE);
2528                                 break;
2529
2530                         case _HT_EXTRA_INFO_IE_:        //HT info
2531                                 HT_info_handler(padapter, pIE);
2532                                 break;
2533
2534 #ifdef CONFIG_80211AC_VHT
2535                         case EID_VHTCapability:
2536                                 VHT_caps_handler(padapter, pIE);
2537                                 break;
2538
2539                         case EID_VHTOperation:
2540                                 VHT_operation_handler(padapter, pIE);
2541                                 break;
2542 #endif
2543
2544                         case _ERPINFO_IE_:
2545                                 ERP_IE_handler(padapter, pIE);
2546
2547                         default:
2548                                 break;
2549                 }
2550
2551                 i += (pIE->Length + 2);
2552         }
2553
2554 #ifdef CONFIG_WAPI_SUPPORT
2555         rtw_wapi_on_assoc_ok(padapter, pIE);
2556 #endif
2557
2558         pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
2559         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
2560
2561         //Update Basic Rate Table for spec, 2010-12-28 , by thomas
2562         UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);
2563
2564 report_assoc_result:
2565         if (res > 0) {
2566                 rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
2567         } else {
2568                 rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
2569         }
2570
2571         report_join_res(padapter, res);
2572
2573         return _SUCCESS;
2574 }
2575
2576 unsigned int OnDeAuth(_adapter *padapter, union recv_frame *precv_frame)
2577 {
2578         unsigned short  reason;
2579         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2580         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2581         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2582         u8 *pframe = precv_frame->u.hdr.rx_data;
2583 #ifdef CONFIG_P2P
2584         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
2585 #endif //CONFIG_P2P
2586
2587         //check A3
2588         if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
2589                 return _SUCCESS;
2590
2591 #ifdef CONFIG_P2P
2592         if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
2593         {
2594                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
2595                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
2596         }
2597 #endif //CONFIG_P2P
2598
2599         reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2600
2601         DBG_871X("%s Reason code(%d)\n", __FUNCTION__,reason);
2602
2603         rtw_lock_rx_suspend_timeout(8000);
2604
2605 #ifdef CONFIG_AP_MODE
2606         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
2607         {
2608                 _irqL irqL;
2609                 struct sta_info *psta;
2610                 struct sta_priv *pstapriv = &padapter->stapriv;
2611
2612                 //_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2613                 //rtw_free_stainfo(padapter, psta);
2614                 //_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2615
2616                 DBG_871X_LEVEL(_drv_always_, "ap recv deauth reason code(%d) sta:%pM\n",
2617                                 reason, GetAddr2Ptr(pframe));
2618
2619                 psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2620                 if(psta)
2621                 {
2622                         u8 updated = _FALSE;
2623
2624                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2625                         if(rtw_is_list_empty(&psta->asoc_list)==_FALSE)
2626                         {
2627                                 rtw_list_delete(&psta->asoc_list);
2628                                 pstapriv->asoc_list_cnt--;
2629                                 updated = ap_free_sta(padapter, psta, _FALSE, reason);
2630
2631                         }
2632                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2633
2634                         associated_clients_update(padapter, updated);
2635                 }
2636
2637
2638                 return _SUCCESS;
2639         }
2640         else
2641 #endif
2642         {
2643                 int     ignore_received_deauth = 0;
2644
2645                 //      Commented by Albert 20130604
2646                 //      Before sending the auth frame to start the STA/GC mode connection with AP/GO,
2647                 //      we will send the deauth first.
2648                 //      However, the Win8.1 with BRCM Wi-Fi will send the deauth with reason code 6 to us after receieving our deauth.
2649                 //      Added the following code to avoid this case.
2650                 if ( ( pmlmeinfo->state & WIFI_FW_AUTH_STATE ) ||
2651                         ( pmlmeinfo->state & WIFI_FW_ASSOC_STATE ) )
2652                 {
2653                         if ( reason == WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA )
2654                         {
2655                                 ignore_received_deauth = 1;
2656                         } else if (WLAN_REASON_PREV_AUTH_NOT_VALID == reason) {
2657                                 // TODO: 802.11r
2658                                 ignore_received_deauth = 1;
2659                         }
2660                 }
2661
2662                 DBG_871X_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM, ignore = %d\n",
2663                                 reason, GetAddr3Ptr(pframe), ignore_received_deauth);
2664
2665                 if ( 0 == ignore_received_deauth )
2666                 {
2667                         receive_disconnect(padapter, GetAddr3Ptr(pframe) ,reason);
2668                 }
2669         }
2670         pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE;
2671         return _SUCCESS;
2672
2673 }
2674
2675 unsigned int OnDisassoc(_adapter *padapter, union recv_frame *precv_frame)
2676 {
2677         unsigned short  reason;
2678         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2679         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2680         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2681         u8 *pframe = precv_frame->u.hdr.rx_data;
2682 #ifdef CONFIG_P2P
2683         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
2684 #endif //CONFIG_P2P
2685
2686         //check A3
2687         if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
2688                 return _SUCCESS;
2689
2690 #ifdef CONFIG_P2P
2691         if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
2692         {
2693                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
2694                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
2695         }
2696 #endif //CONFIG_P2P
2697
2698         reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2699
2700         DBG_871X("%s Reason code(%d)\n", __FUNCTION__,reason);
2701
2702         rtw_lock_rx_suspend_timeout(8000);
2703
2704 #ifdef CONFIG_AP_MODE
2705         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
2706         {
2707                 _irqL irqL;
2708                 struct sta_info *psta;
2709                 struct sta_priv *pstapriv = &padapter->stapriv;
2710
2711                 //_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2712                 //rtw_free_stainfo(padapter, psta);
2713                 //_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
2714
2715                 DBG_871X_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
2716                                 reason, GetAddr2Ptr(pframe));
2717
2718                 psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2719                 if(psta)
2720                 {
2721                         u8 updated = _FALSE;
2722
2723                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2724                         if(rtw_is_list_empty(&psta->asoc_list)==_FALSE)
2725                         {
2726                                 rtw_list_delete(&psta->asoc_list);
2727                                 pstapriv->asoc_list_cnt--;
2728                                 updated = ap_free_sta(padapter, psta, _FALSE, reason);
2729
2730                         }
2731                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2732
2733                         associated_clients_update(padapter, updated);
2734                 }
2735
2736                 return _SUCCESS;
2737         }
2738         else
2739 #endif
2740         {
2741                 DBG_871X_LEVEL(_drv_always_, "sta recv disassoc reason code(%d) sta:%pM\n",
2742                                 reason, GetAddr3Ptr(pframe));
2743
2744                 receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
2745         }
2746         pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE;
2747         return _SUCCESS;
2748
2749 }
2750
2751 unsigned int OnAtim(_adapter *padapter, union recv_frame *precv_frame)
2752 {
2753         DBG_871X("%s\n", __FUNCTION__);
2754         return _SUCCESS;
2755 }
2756
2757 unsigned int on_action_spct_ch_switch(_adapter *padapter, struct sta_info *psta, u8 *ies, uint ies_len)
2758 {
2759         unsigned int ret = _FAIL;
2760         struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
2761         struct mlme_ext_info    *pmlmeinfo = &(mlmeext->mlmext_info);
2762
2763         if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
2764                 ret = _SUCCESS;
2765                 goto exit;
2766         }
2767
2768         if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
2769
2770                 int ch_switch_mode = -1, ch = -1, ch_switch_cnt = -1;
2771                 int ch_offset = -1;
2772                 u8 bwmode;
2773                 struct ieee80211_info_element *ie;
2774
2775                 DBG_871X(FUNC_NDEV_FMT" from "MAC_FMT"\n",
2776                         FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(psta->hwaddr));
2777
2778                 for_each_ie(ie, ies, ies_len) {
2779                         if (ie->id == WLAN_EID_CHANNEL_SWITCH) {
2780                                 ch_switch_mode = ie->data[0];
2781                                 ch = ie->data[1];
2782                                 ch_switch_cnt = ie->data[2];
2783                                 DBG_871X("ch_switch_mode:%d, ch:%d, ch_switch_cnt:%d\n",
2784                                         ch_switch_mode, ch, ch_switch_cnt);
2785                         }
2786                         else if (ie->id == WLAN_EID_SECONDARY_CHANNEL_OFFSET) {
2787                                 ch_offset = secondary_ch_offset_to_hal_ch_offset(ie->data[0]);
2788                                 DBG_871X("ch_offset:%d\n", ch_offset);
2789                         }
2790                 }
2791
2792                 if (ch == -1)
2793                         return _SUCCESS;
2794
2795                 if (ch_offset == -1)
2796                         bwmode = mlmeext->cur_bwmode;
2797                 else
2798                         bwmode = (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) ?
2799                                 CHANNEL_WIDTH_20 : CHANNEL_WIDTH_40;
2800
2801                 ch_offset = (ch_offset == -1) ? mlmeext->cur_ch_offset : ch_offset;
2802
2803                 /* todo:
2804                  * 1. the decision of channel switching
2805                  * 2. things after channel switching
2806                  */
2807
2808                 ret = rtw_set_ch_cmd(padapter, ch, bwmode, ch_offset, _TRUE);
2809         }
2810
2811 exit:
2812         return ret;
2813 }
2814
2815 unsigned int on_action_spct(_adapter *padapter, union recv_frame *precv_frame)
2816 {
2817         unsigned int ret = _FAIL;
2818         struct sta_info *psta = NULL;
2819         struct sta_priv *pstapriv = &padapter->stapriv;
2820         u8 *pframe = precv_frame->u.hdr.rx_data;
2821         uint frame_len = precv_frame->u.hdr.len;
2822         u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
2823         u8 category;
2824         u8 action;
2825
2826         DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
2827
2828         psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2829
2830         if (!psta)
2831                 goto exit;
2832
2833         category = frame_body[0];
2834         if(category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
2835                 goto exit;
2836
2837         action = frame_body[1];
2838         switch (action) {
2839         case RTW_WLAN_ACTION_SPCT_MSR_REQ:
2840         case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
2841         case RTW_WLAN_ACTION_SPCT_TPC_REQ:
2842         case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
2843                 break;
2844         case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
2845                 #ifdef CONFIG_SPCT_CH_SWITCH
2846                 ret = on_action_spct_ch_switch(padapter, psta, &frame_body[2],
2847                         frame_len-(frame_body-pframe)-2);
2848                 #endif
2849                 break;
2850         default:
2851                 break;
2852         }
2853
2854 exit:
2855         return ret;
2856 }
2857
2858 unsigned int OnAction_qos(_adapter *padapter, union recv_frame *precv_frame)
2859 {
2860         return _SUCCESS;
2861 }
2862
2863 unsigned int OnAction_dls(_adapter *padapter, union recv_frame *precv_frame)
2864 {
2865         return _SUCCESS;
2866 }
2867
2868 unsigned int OnAction_back(_adapter *padapter, union recv_frame *precv_frame)
2869 {
2870         u8 *addr;
2871         struct sta_info *psta=NULL;
2872         struct recv_reorder_ctrl *preorder_ctrl;
2873         unsigned char           *frame_body;
2874         unsigned char           category, action;
2875         unsigned short  tid, status, reason_code = 0;
2876         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2877         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2878         u8 *pframe = precv_frame->u.hdr.rx_data;
2879         struct sta_priv *pstapriv = &padapter->stapriv;
2880 #ifdef CONFIG_80211N_HT
2881
2882         DBG_871X("%s\n", __FUNCTION__);
2883
2884         //check RA matches or not
2885         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))//for if1, sta/ap mode
2886                 return _SUCCESS;
2887
2888 /*
2889         //check A1 matches or not
2890         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
2891                 return _SUCCESS;
2892 */
2893
2894         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
2895                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
2896                         return _SUCCESS;
2897
2898         addr = GetAddr2Ptr(pframe);
2899         psta = rtw_get_stainfo(pstapriv, addr);
2900
2901         if(psta==NULL)
2902                 return _SUCCESS;
2903
2904         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
2905
2906         category = frame_body[0];
2907         if (category == RTW_WLAN_CATEGORY_BACK)// representing Block Ack
2908         {
2909 #ifdef CONFIG_TDLS
2910                 if((psta->tdls_sta_state & TDLS_LINKED_STATE) &&
2911                         (psta->htpriv.ht_option==_TRUE) &&
2912                         (psta->htpriv.ampdu_enable==_TRUE))
2913                 {
2914                         DBG_871X("Recv [%s] from direc link\n", __FUNCTION__);
2915                 }
2916                 else
2917 #endif //CONFIG_TDLS
2918                 if (!pmlmeinfo->HT_enable)
2919                 {
2920                         return _SUCCESS;
2921                 }
2922
2923                 action = frame_body[1];
2924                 DBG_871X("%s, action=%d\n", __FUNCTION__, action);
2925                 switch (action)
2926                 {
2927                         case RTW_WLAN_ACTION_ADDBA_REQ: //ADDBA request
2928
2929                                 _rtw_memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
2930                                 //process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), GetAddr3Ptr(pframe));
2931                                 process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), addr);
2932
2933                                 if(pmlmeinfo->bAcceptAddbaReq == _TRUE)
2934                                 {
2935                                         issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0);
2936                                 }
2937                                 else
2938                                 {
2939                                         issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);//reject ADDBA Req
2940                                 }
2941
2942                                 break;
2943
2944                         case RTW_WLAN_ACTION_ADDBA_RESP: //ADDBA response
2945
2946                                 //status = frame_body[3] | (frame_body[4] << 8); //endian issue
2947                                 status = RTW_GET_LE16(&frame_body[3]);
2948                                 tid = ((frame_body[5] >> 2) & 0x7);
2949
2950                                 if (status == 0)
2951                                 {       //successful
2952                                         DBG_871X("agg_enable for TID=%d\n", tid);
2953                                         psta->htpriv.agg_enable_bitmap |= 1 << tid;
2954                                         psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
2955                                 }
2956                                 else
2957                                 {
2958                                         psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
2959                                 }
2960
2961                                 if(psta->state & WIFI_STA_ALIVE_CHK_STATE)
2962                                 {
2963                                         DBG_871X("%s alive check - rx ADDBA response\n", __func__);
2964                                         psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
2965                                         psta->expire_to = pstapriv->expire_to;
2966                                         psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
2967                                 }
2968
2969                                 //DBG_871X("marc: ADDBA RSP: %x\n", pmlmeinfo->agg_enable_bitmap);
2970                                 break;
2971
2972                         case RTW_WLAN_ACTION_DELBA: //DELBA
2973                                 if ((frame_body[3] & BIT(3)) == 0)
2974                                 {
2975                                         psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
2976                                         psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
2977
2978                                         //reason_code = frame_body[4] | (frame_body[5] << 8);
2979                                         reason_code = RTW_GET_LE16(&frame_body[4]);
2980                                 }
2981                                 else if((frame_body[3] & BIT(3)) == BIT(3))
2982                                 {
2983                                         tid = (frame_body[3] >> 4) & 0x0F;
2984
2985                                         preorder_ctrl =  &psta->recvreorder_ctrl[tid];
2986                                         preorder_ctrl->enable = _FALSE;
2987                                         preorder_ctrl->indicate_seq = 0xffff;
2988                                         #ifdef DBG_RX_SEQ
2989                                         DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
2990                                                 preorder_ctrl->indicate_seq);
2991                                         #endif
2992                                 }
2993
2994                                 DBG_871X("%s(): DELBA: %x(%x)\n", __FUNCTION__,pmlmeinfo->agg_enable_bitmap, reason_code);
2995                                 //todo: how to notify the host while receiving DELETE BA
2996                                 break;
2997
2998                         default:
2999                                 break;
3000                 }
3001         }
3002 #endif //CONFIG_80211N_HT
3003         return _SUCCESS;
3004 }
3005
3006 #ifdef CONFIG_P2P
3007
3008 static int get_reg_classes_full_count(struct p2p_channels channel_list) {
3009         int cnt = 0;
3010         int i;
3011
3012         for (i = 0; i < channel_list.reg_classes; i++) {
3013                 cnt += channel_list.reg_class[i].channels;
3014         }
3015
3016         return cnt;
3017 }
3018
3019 static void get_channel_cnt_24g_5gl_5gh(  struct mlme_ext_priv *pmlmeext, u8* p24g_cnt, u8* p5gl_cnt, u8* p5gh_cnt )
3020 {
3021         int     i = 0;
3022
3023         *p24g_cnt = 0;
3024         *p5gl_cnt = 0;
3025         *p5gh_cnt = 0;
3026
3027         for( i = 0; i < pmlmeext->max_chan_nums; i++ )
3028         {
3029                 if ( pmlmeext->channel_set[ i ].ChannelNum <= 14 )
3030                 {
3031                         (*p24g_cnt)++;
3032                 }
3033                 else if ( ( pmlmeext->channel_set[ i ].ChannelNum > 14 ) && ( pmlmeext->channel_set[ i ].ChannelNum <= 48 ) )
3034                 {
3035                         //      Just include the channel 36, 40, 44, 48 channels for 5G low
3036                         (*p5gl_cnt)++;
3037                 }
3038                 else if ( ( pmlmeext->channel_set[ i ].ChannelNum >= 149 ) && ( pmlmeext->channel_set[ i ].ChannelNum <= 161 ) )
3039                 {
3040                         //      Just include the channel 149, 153, 157, 161 channels for 5G high
3041                         (*p5gh_cnt)++;
3042                 }
3043         }
3044 }
3045
3046 void issue_p2p_GO_request(_adapter *padapter, u8* raddr)
3047 {
3048
3049         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3050         u8                      action = P2P_PUB_ACTION_ACTION;
3051         u32                     p2poui = cpu_to_be32(P2POUI);
3052         u8                      oui_subtype = P2P_GO_NEGO_REQ;
3053         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
3054         u8                      wpsielen = 0, p2pielen = 0, i;
3055         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
3056         u16                     len_channellist_attr = 0;
3057 #ifdef CONFIG_WFD
3058         u32                                     wfdielen = 0;
3059 #endif //CONFIG_WFD
3060
3061         struct xmit_frame                       *pmgntframe;
3062         struct pkt_attrib                       *pattrib;
3063         unsigned char                                   *pframe;
3064         struct rtw_ieee80211_hdr        *pwlanhdr;
3065         unsigned short                          *fctrl;
3066         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3067         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3068         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3069         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
3070
3071
3072         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
3073         {
3074                 return;
3075         }
3076
3077         DBG_871X( "[%s] In\n", __FUNCTION__ );
3078         //update attribute
3079         pattrib = &pmgntframe->attrib;
3080         update_mgntframe_attrib(padapter, pattrib);
3081
3082         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3083
3084         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3085         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3086
3087         fctrl = &(pwlanhdr->frame_ctl);
3088         *(fctrl) = 0;
3089
3090         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
3091         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
3092         _rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
3093
3094         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3095         pmlmeext->mgnt_seq++;
3096         SetFrameSubType(pframe, WIFI_ACTION);
3097
3098         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
3099         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3100
3101         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
3102         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
3103         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
3104         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3105         pwdinfo->negotiation_dialog_token = 1;  //      Initialize the dialog value
3106         pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &(pattrib->pktlen));
3107
3108
3109
3110         //      WPS Section
3111         wpsielen = 0;
3112         //      WPS OUI
3113         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
3114         wpsielen += 4;
3115
3116         //      WPS version
3117         //      Type:
3118         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
3119         wpsielen += 2;
3120
3121         //      Length:
3122         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
3123         wpsielen += 2;
3124
3125         //      Value:
3126         wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
3127
3128         //      Device Password ID
3129         //      Type:
3130         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
3131         wpsielen += 2;
3132
3133         //      Length:
3134         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
3135         wpsielen += 2;
3136
3137         //      Value:
3138
3139         if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN )
3140         {
3141                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_USER_SPEC );
3142         }
3143         else if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN )
3144         {
3145                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );
3146         }
3147         else if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC )
3148         {
3149                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_PBC );
3150         }
3151
3152         wpsielen += 2;
3153
3154         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
3155
3156
3157         //      P2P IE Section.
3158
3159         //      P2P OUI
3160         p2pielen = 0;
3161         p2pie[ p2pielen++ ] = 0x50;
3162         p2pie[ p2pielen++ ] = 0x6F;
3163         p2pie[ p2pielen++ ] = 0x9A;
3164         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
3165
3166         //      Commented by Albert 20110306
3167         //      According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes
3168         //      1. P2P Capability
3169         //      2. Group Owner Intent
3170         //      3. Configuration Timeout
3171         //      4. Listen Channel
3172         //      5. Extended Listen Timing
3173         //      6. Intended P2P Interface Address
3174         //      7. Channel List
3175         //      8. P2P Device Info
3176         //      9. Operating Channel
3177
3178
3179         //      P2P Capability
3180         //      Type:
3181         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
3182
3183         //      Length:
3184         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3185         p2pielen += 2;
3186
3187         //      Value:
3188         //      Device Capability Bitmap, 1 byte
3189         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
3190
3191         //      Group Capability Bitmap, 1 byte
3192         if ( pwdinfo->persistent_supported )
3193         {
3194                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
3195         }
3196         else
3197         {
3198                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
3199         }
3200
3201
3202         //      Group Owner Intent
3203         //      Type:
3204         p2pie[ p2pielen++ ] = P2P_ATTR_GO_INTENT;
3205
3206         //      Length:
3207         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3208         p2pielen += 2;
3209
3210         //      Value:
3211         //      Todo the tie breaker bit.
3212         p2pie[ p2pielen++ ] = ( ( pwdinfo->intent << 1 ) &  0xFE );
3213
3214         //      Configuration Timeout
3215         //      Type:
3216         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
3217
3218         //      Length:
3219         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3220         p2pielen += 2;
3221
3222         //      Value:
3223         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
3224         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
3225
3226
3227         //      Listen Channel
3228         //      Type:
3229         p2pie[ p2pielen++ ] = P2P_ATTR_LISTEN_CH;
3230
3231         //      Length:
3232         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
3233         p2pielen += 2;
3234
3235         //      Value:
3236         //      Country String
3237         p2pie[ p2pielen++ ] = 'X';
3238         p2pie[ p2pielen++ ] = 'X';
3239
3240         //      The third byte should be set to 0x04.
3241         //      Described in the "Operating Channel Attribute" section.
3242         p2pie[ p2pielen++ ] = 0x04;
3243
3244         //      Operating Class
3245         p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
3246
3247         //      Channel Number
3248         p2pie[ p2pielen++ ] = pwdinfo->listen_channel;  //      listening channel number
3249
3250
3251         //      Extended Listen Timing ATTR
3252         //      Type:
3253         p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
3254
3255         //      Length:
3256         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
3257         p2pielen += 2;
3258
3259         //      Value:
3260         //      Availability Period
3261         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
3262         p2pielen += 2;
3263
3264         //      Availability Interval
3265         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
3266         p2pielen += 2;
3267
3268
3269         //      Intended P2P Interface Address
3270         //      Type:
3271         p2pie[ p2pielen++ ] = P2P_ATTR_INTENTED_IF_ADDR;
3272
3273         //      Length:
3274         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
3275         p2pielen += 2;
3276
3277         //      Value:
3278         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
3279         p2pielen += ETH_ALEN;
3280
3281
3282         //      Channel List
3283         //      Type:
3284         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
3285
3286         // Length:
3287         // Country String(3)
3288         // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
3289         // + number of channels in all classes
3290         len_channellist_attr = 3
3291            + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
3292            + get_reg_classes_full_count(pmlmeext->channel_list);
3293
3294 #ifdef CONFIG_CONCURRENT_MODE
3295         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3296         {
3297                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
3298         }
3299         else
3300         {
3301                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3302         }
3303 #else
3304
3305         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3306
3307 #endif
3308         p2pielen += 2;
3309
3310         //      Value:
3311         //      Country String
3312         p2pie[ p2pielen++ ] = 'X';
3313         p2pie[ p2pielen++ ] = 'X';
3314
3315         //      The third byte should be set to 0x04.
3316         //      Described in the "Operating Channel Attribute" section.
3317         p2pie[ p2pielen++ ] = 0x04;
3318
3319         //      Channel Entry List
3320
3321 #ifdef CONFIG_CONCURRENT_MODE
3322         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3323         {
3324                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
3325                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3326
3327                 //      Operating Class
3328                 if ( pbuddy_mlmeext->cur_channel > 14 )
3329                 {
3330                         if ( pbuddy_mlmeext->cur_channel >= 149 )
3331                         {
3332                                 p2pie[ p2pielen++ ] = 0x7c;
3333                         }
3334                         else
3335                         {
3336                                 p2pie[ p2pielen++ ] = 0x73;
3337                         }
3338                 }
3339                 else
3340                 {
3341                         p2pie[ p2pielen++ ] = 0x51;
3342                 }
3343
3344                 //      Number of Channels
3345                 //      Just support 1 channel and this channel is AP's channel
3346                 p2pie[ p2pielen++ ] = 1;
3347
3348                 //      Channel List
3349                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
3350         }
3351         else
3352         {
3353                 int i,j;
3354                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3355                         //      Operating Class
3356                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3357
3358                         //      Number of Channels
3359                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3360
3361                         //      Channel List
3362                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3363                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3364                         }
3365                 }
3366         }
3367 #else // CONFIG_CONCURRENT_MODE
3368         {
3369                 int i,j;
3370                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3371                         //      Operating Class
3372                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3373
3374                         //      Number of Channels
3375                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3376
3377                         //      Channel List
3378                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3379                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3380                         }
3381                 }
3382         }
3383 #endif // CONFIG_CONCURRENT_MODE
3384
3385         //      Device Info
3386         //      Type:
3387         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
3388
3389         //      Length:
3390         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
3391         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
3392         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
3393         p2pielen += 2;
3394
3395         //      Value:
3396         //      P2P Device Address
3397         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
3398         p2pielen += ETH_ALEN;
3399
3400         //      Config Method
3401         //      This field should be big endian. Noted by P2P specification.
3402
3403         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
3404
3405         p2pielen += 2;
3406
3407         //      Primary Device Type
3408         //      Category ID
3409         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
3410         p2pielen += 2;
3411
3412         //      OUI
3413         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
3414         p2pielen += 4;
3415
3416         //      Sub Category ID
3417         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
3418         p2pielen += 2;
3419
3420         //      Number of Secondary Device Types
3421         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
3422
3423         //      Device Name
3424         //      Type:
3425         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
3426         p2pielen += 2;
3427
3428         //      Length:
3429         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
3430         p2pielen += 2;
3431
3432         //      Value:
3433         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len );
3434         p2pielen += pwdinfo->device_name_len;
3435
3436
3437         //      Operating Channel
3438         //      Type:
3439         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
3440
3441         //      Length:
3442         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
3443         p2pielen += 2;
3444
3445         //      Value:
3446         //      Country String
3447         p2pie[ p2pielen++ ] = 'X';
3448         p2pie[ p2pielen++ ] = 'X';
3449
3450         //      The third byte should be set to 0x04.
3451         //      Described in the "Operating Channel Attribute" section.
3452         p2pie[ p2pielen++ ] = 0x04;
3453
3454         //      Operating Class
3455         if ( pwdinfo->operating_channel <= 14 )
3456         {
3457                 //      Operating Class
3458                 p2pie[ p2pielen++ ] = 0x51;
3459         }
3460         else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
3461         {
3462                 //      Operating Class
3463                 p2pie[ p2pielen++ ] = 0x73;
3464         }
3465         else
3466         {
3467                 //      Operating Class
3468                 p2pie[ p2pielen++ ] = 0x7c;
3469         }
3470
3471         //      Channel Number
3472         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
3473
3474         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
3475
3476 #ifdef CONFIG_WFD
3477         wfdielen = build_nego_req_wfd_ie(pwdinfo, pframe);
3478         pframe += wfdielen;
3479         pattrib->pktlen += wfdielen;
3480 #endif //CONFIG_WFD
3481
3482         pattrib->last_txcmdsz = pattrib->pktlen;
3483
3484         dump_mgntframe(padapter, pmgntframe);
3485
3486         return;
3487
3488 }
3489
3490
3491 void issue_p2p_GO_response(_adapter *padapter, u8* raddr, u8* frame_body,uint len, u8 result)
3492 {
3493
3494         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3495         u8                      action = P2P_PUB_ACTION_ACTION;
3496         u32                     p2poui = cpu_to_be32(P2POUI);
3497         u8                      oui_subtype = P2P_GO_NEGO_RESP;
3498         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
3499         u8                      p2pielen = 0, i;
3500         uint                    wpsielen = 0;
3501         u16                     wps_devicepassword_id = 0x0000;
3502         uint                    wps_devicepassword_id_len = 0;
3503         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh;
3504         u16                     len_channellist_attr = 0;
3505
3506         struct xmit_frame                       *pmgntframe;
3507         struct pkt_attrib                       *pattrib;
3508         unsigned char                                   *pframe;
3509         struct rtw_ieee80211_hdr        *pwlanhdr;
3510         unsigned short                          *fctrl;
3511         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3512         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3513         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3514         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
3515
3516 #ifdef CONFIG_WFD
3517         u32                                     wfdielen = 0;
3518 #endif //CONFIG_WFD
3519
3520         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
3521         {
3522                 return;
3523         }
3524
3525         DBG_871X( "[%s] In, result = %d\n", __FUNCTION__,  result );
3526         //update attribute
3527         pattrib = &pmgntframe->attrib;
3528         update_mgntframe_attrib(padapter, pattrib);
3529
3530         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3531
3532         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3533         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3534
3535         fctrl = &(pwlanhdr->frame_ctl);
3536         *(fctrl) = 0;
3537
3538         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
3539         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
3540         _rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
3541
3542         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3543         pmlmeext->mgnt_seq++;
3544         SetFrameSubType(pframe, WIFI_ACTION);
3545
3546         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
3547         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3548
3549         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
3550         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
3551         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
3552         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3553         pwdinfo->negotiation_dialog_token = frame_body[7];      //      The Dialog Token of provisioning discovery request frame.
3554         pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
3555
3556         //      Commented by Albert 20110328
3557         //      Try to get the device password ID from the WPS IE of group negotiation request frame
3558         //      WiFi Direct test plan 5.1.15
3559         rtw_get_wps_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
3560         rtw_get_wps_attr_content( wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len);
3561         wps_devicepassword_id = be16_to_cpu( wps_devicepassword_id );
3562
3563         _rtw_memset( wpsie, 0x00, 255 );
3564         wpsielen = 0;
3565
3566         //      WPS Section
3567         wpsielen = 0;
3568         //      WPS OUI
3569         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
3570         wpsielen += 4;
3571
3572         //      WPS version
3573         //      Type:
3574         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
3575         wpsielen += 2;
3576
3577         //      Length:
3578         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
3579         wpsielen += 2;
3580
3581         //      Value:
3582         wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
3583
3584         //      Device Password ID
3585         //      Type:
3586         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
3587         wpsielen += 2;
3588
3589         //      Length:
3590         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
3591         wpsielen += 2;
3592
3593         //      Value:
3594         if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
3595         {
3596                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );
3597         }
3598         else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
3599         {
3600                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_USER_SPEC );
3601         }
3602         else
3603         {
3604                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_PBC );
3605         }
3606         wpsielen += 2;
3607
3608         //      Commented by Kurt 20120113
3609         //      If some device wants to do p2p handshake without sending prov_disc_req
3610         //      We have to get peer_req_cm from here.
3611         if(_rtw_memcmp( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3) )
3612         {
3613                 if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
3614                 {
3615                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3 );
3616                 }
3617                 else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
3618                 {
3619                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3 );
3620                 }
3621                 else
3622                 {
3623                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3 );
3624                 }
3625         }
3626
3627         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
3628
3629
3630         //      P2P IE Section.
3631
3632         //      P2P OUI
3633         p2pielen = 0;
3634         p2pie[ p2pielen++ ] = 0x50;
3635         p2pie[ p2pielen++ ] = 0x6F;
3636         p2pie[ p2pielen++ ] = 0x9A;
3637         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
3638
3639         //      Commented by Albert 20100908
3640         //      According to the P2P Specification, the group negoitation response frame should contain 9 P2P attributes
3641         //      1. Status
3642         //      2. P2P Capability
3643         //      3. Group Owner Intent
3644         //      4. Configuration Timeout
3645         //      5. Operating Channel
3646         //      6. Intended P2P Interface Address
3647         //      7. Channel List
3648         //      8. Device Info
3649         //      9. Group ID     ( Only GO )
3650
3651
3652         //      ToDo:
3653
3654         //      P2P Status
3655         //      Type:
3656         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
3657
3658         //      Length:
3659         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3660         p2pielen += 2;
3661
3662         //      Value:
3663         p2pie[ p2pielen++ ] = result;
3664
3665         //      P2P Capability
3666         //      Type:
3667         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
3668
3669         //      Length:
3670         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3671         p2pielen += 2;
3672
3673         //      Value:
3674         //      Device Capability Bitmap, 1 byte
3675
3676         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
3677         {
3678                 //      Commented by Albert 2011/03/08
3679                 //      According to the P2P specification
3680                 //      if the sending device will be client, the P2P Capability should be reserved of group negotation response frame
3681                 p2pie[ p2pielen++ ] = 0;
3682         }
3683         else
3684         {
3685                 //      Be group owner or meet the error case
3686                 p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
3687         }
3688
3689         //      Group Capability Bitmap, 1 byte
3690         if ( pwdinfo->persistent_supported )
3691         {
3692                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
3693         }
3694         else
3695         {
3696                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
3697         }
3698
3699         //      Group Owner Intent
3700         //      Type:
3701         p2pie[ p2pielen++ ] = P2P_ATTR_GO_INTENT;
3702
3703         //      Length:
3704         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3705         p2pielen += 2;
3706
3707         //      Value:
3708         if ( pwdinfo->peer_intent & 0x01 )
3709         {
3710                 //      Peer's tie breaker bit is 1, our tie breaker bit should be 0
3711                 p2pie[ p2pielen++ ] = ( pwdinfo->intent << 1 );
3712         }
3713         else
3714         {
3715                 //      Peer's tie breaker bit is 0, our tie breaker bit should be 1
3716                 p2pie[ p2pielen++ ] = ( ( pwdinfo->intent << 1 ) | BIT(0) );
3717         }
3718
3719
3720         //      Configuration Timeout
3721         //      Type:
3722         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
3723
3724         //      Length:
3725         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3726         p2pielen += 2;
3727
3728         //      Value:
3729         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
3730         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
3731
3732         //      Operating Channel
3733         //      Type:
3734         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
3735
3736         //      Length:
3737         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
3738         p2pielen += 2;
3739
3740         //      Value:
3741         //      Country String
3742         p2pie[ p2pielen++ ] = 'X';
3743         p2pie[ p2pielen++ ] = 'X';
3744
3745         //      The third byte should be set to 0x04.
3746         //      Described in the "Operating Channel Attribute" section.
3747         p2pie[ p2pielen++ ] = 0x04;
3748
3749         //      Operating Class
3750         if ( pwdinfo->operating_channel <= 14 )
3751         {
3752                 //      Operating Class
3753                 p2pie[ p2pielen++ ] = 0x51;
3754         }
3755         else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
3756         {
3757                 //      Operating Class
3758                 p2pie[ p2pielen++ ] = 0x73;
3759         }
3760         else
3761         {
3762                 //      Operating Class
3763                 p2pie[ p2pielen++ ] = 0x7c;
3764         }
3765
3766         //      Channel Number
3767         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
3768
3769         //      Intended P2P Interface Address
3770         //      Type:
3771         p2pie[ p2pielen++ ] = P2P_ATTR_INTENTED_IF_ADDR;
3772
3773         //      Length:
3774         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
3775         p2pielen += 2;
3776
3777         //      Value:
3778         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
3779         p2pielen += ETH_ALEN;
3780
3781         //      Channel List
3782         //      Type:
3783         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
3784
3785         // Country String(3)
3786         // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
3787         // + number of channels in all classes
3788         len_channellist_attr = 3
3789            + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
3790            + get_reg_classes_full_count(pmlmeext->channel_list);
3791
3792 #ifdef CONFIG_CONCURRENT_MODE
3793         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3794         {
3795                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
3796         }
3797         else
3798         {
3799                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3800         }
3801 #else
3802
3803         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3804
3805  #endif
3806         p2pielen += 2;
3807
3808         //      Value:
3809         //      Country String
3810         p2pie[ p2pielen++ ] = 'X';
3811         p2pie[ p2pielen++ ] = 'X';
3812
3813         //      The third byte should be set to 0x04.
3814         //      Described in the "Operating Channel Attribute" section.
3815         p2pie[ p2pielen++ ] = 0x04;
3816
3817         //      Channel Entry List
3818
3819 #ifdef CONFIG_CONCURRENT_MODE
3820         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3821         {
3822                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
3823                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3824
3825                 //      Operating Class
3826                 if ( pbuddy_mlmeext->cur_channel > 14 )
3827                 {
3828                         if ( pbuddy_mlmeext->cur_channel >= 149 )
3829                         {
3830                                 p2pie[ p2pielen++ ] = 0x7c;
3831                         }
3832                         else
3833                         {
3834                                 p2pie[ p2pielen++ ] = 0x73;
3835                         }
3836                 }
3837                 else
3838                 {
3839                         p2pie[ p2pielen++ ] = 0x51;
3840                 }
3841
3842                 //      Number of Channels
3843                 //      Just support 1 channel and this channel is AP's channel
3844                 p2pie[ p2pielen++ ] = 1;
3845
3846                 //      Channel List
3847                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
3848         }
3849         else
3850         {
3851                 int i, j;
3852                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3853                         //      Operating Class
3854                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3855
3856                         //      Number of Channels
3857                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3858
3859                         //      Channel List
3860                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3861                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3862                         }
3863                 }
3864         }
3865 #else // CONFIG_CONCURRENT_MODE
3866         {
3867                 int i, j;
3868                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3869                         //      Operating Class
3870                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3871
3872                         //      Number of Channels
3873                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3874
3875                         //      Channel List
3876                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3877                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3878                         }
3879                 }
3880         }
3881 #endif // CONFIG_CONCURRENT_MODE
3882
3883
3884         //      Device Info
3885         //      Type:
3886         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
3887
3888         //      Length:
3889         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
3890         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
3891         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
3892         p2pielen += 2;
3893
3894         //      Value:
3895         //      P2P Device Address
3896         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
3897         p2pielen += ETH_ALEN;
3898
3899         //      Config Method
3900         //      This field should be big endian. Noted by P2P specification.
3901
3902         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
3903
3904         p2pielen += 2;
3905
3906         //      Primary Device Type
3907         //      Category ID
3908         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
3909         p2pielen += 2;
3910
3911         //      OUI
3912         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
3913         p2pielen += 4;
3914
3915         //      Sub Category ID
3916         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
3917         p2pielen += 2;
3918
3919         //      Number of Secondary Device Types
3920         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
3921
3922         //      Device Name
3923         //      Type:
3924         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
3925         p2pielen += 2;
3926
3927         //      Length:
3928         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
3929         p2pielen += 2;
3930
3931         //      Value:
3932         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len );
3933         p2pielen += pwdinfo->device_name_len;
3934
3935         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
3936         {
3937                 //      Group ID Attribute
3938                 //      Type:
3939                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
3940
3941                 //      Length:
3942                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + pwdinfo->nego_ssidlen );
3943                 p2pielen += 2;
3944
3945                 //      Value:
3946                 //      p2P Device Address
3947                 _rtw_memcpy( p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN );
3948                 p2pielen += ETH_ALEN;
3949
3950                 //      SSID
3951                 _rtw_memcpy( p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
3952                 p2pielen += pwdinfo->nego_ssidlen;
3953
3954         }
3955
3956         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
3957
3958 #ifdef CONFIG_WFD
3959         wfdielen = build_nego_resp_wfd_ie(pwdinfo, pframe);
3960         pframe += wfdielen;
3961         pattrib->pktlen += wfdielen;
3962 #endif //CONFIG_WFD
3963
3964         pattrib->last_txcmdsz = pattrib->pktlen;
3965
3966         dump_mgntframe(padapter, pmgntframe);
3967
3968         return;
3969
3970 }
3971
3972 void issue_p2p_GO_confirm(_adapter *padapter, u8* raddr, u8 result)
3973 {
3974
3975         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3976         u8                      action = P2P_PUB_ACTION_ACTION;
3977         u32                     p2poui = cpu_to_be32(P2POUI);
3978         u8                      oui_subtype = P2P_GO_NEGO_CONF;
3979         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
3980         u8                      wpsielen = 0, p2pielen = 0;
3981
3982         struct xmit_frame                       *pmgntframe;
3983         struct pkt_attrib                       *pattrib;
3984         unsigned char                                   *pframe;
3985         struct rtw_ieee80211_hdr        *pwlanhdr;
3986         unsigned short                          *fctrl;
3987         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3988         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3989         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3990         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
3991 #ifdef CONFIG_WFD
3992         u32                                     wfdielen = 0;
3993 #endif //CONFIG_WFD
3994
3995         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
3996         {
3997                 return;
3998         }
3999
4000         DBG_871X( "[%s] In\n", __FUNCTION__ );
4001         //update attribute
4002         pattrib = &pmgntframe->attrib;
4003         update_mgntframe_attrib(padapter, pattrib);
4004
4005         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4006
4007         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4008         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4009
4010         fctrl = &(pwlanhdr->frame_ctl);
4011         *(fctrl) = 0;
4012
4013         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
4014         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
4015         _rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
4016
4017         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4018         pmlmeext->mgnt_seq++;
4019         SetFrameSubType(pframe, WIFI_ACTION);
4020
4021         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4022         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4023
4024         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4025         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4026         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4027         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4028         pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
4029
4030
4031
4032         //      P2P IE Section.
4033
4034         //      P2P OUI
4035         p2pielen = 0;
4036         p2pie[ p2pielen++ ] = 0x50;
4037         p2pie[ p2pielen++ ] = 0x6F;
4038         p2pie[ p2pielen++ ] = 0x9A;
4039         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
4040
4041         //      Commented by Albert 20110306
4042         //      According to the P2P Specification, the group negoitation request frame should contain 5 P2P attributes
4043         //      1. Status
4044         //      2. P2P Capability
4045         //      3. Operating Channel
4046         //      4. Channel List
4047         //      5. Group ID     ( if this WiFi is GO )
4048
4049         //      P2P Status
4050         //      Type:
4051         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
4052
4053         //      Length:
4054         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
4055         p2pielen += 2;
4056
4057         //      Value:
4058         p2pie[ p2pielen++ ] = result;
4059
4060         //      P2P Capability
4061         //      Type:
4062         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
4063
4064         //      Length:
4065         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
4066         p2pielen += 2;
4067
4068         //      Value:
4069         //      Device Capability Bitmap, 1 byte
4070         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
4071
4072         //      Group Capability Bitmap, 1 byte
4073         if ( pwdinfo->persistent_supported )
4074         {
4075                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
4076         }
4077         else
4078         {
4079                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
4080         }
4081
4082
4083         //      Operating Channel
4084         //      Type:
4085         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4086
4087         //      Length:
4088         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4089         p2pielen += 2;
4090
4091         //      Value:
4092         //      Country String
4093         p2pie[ p2pielen++ ] = 'X';
4094         p2pie[ p2pielen++ ] = 'X';
4095
4096         //      The third byte should be set to 0x04.
4097         //      Described in the "Operating Channel Attribute" section.
4098         p2pie[ p2pielen++ ] = 0x04;
4099
4100
4101         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
4102         {
4103                 if ( pwdinfo->peer_operating_ch <= 14 )
4104                 {
4105                         //      Operating Class
4106                         p2pie[ p2pielen++ ] = 0x51;
4107                 }
4108                 else if ( ( pwdinfo->peer_operating_ch >= 36 ) && ( pwdinfo->peer_operating_ch <= 48 ) )
4109                 {
4110                         //      Operating Class
4111                         p2pie[ p2pielen++ ] = 0x73;
4112                 }
4113                 else
4114                 {
4115                         //      Operating Class
4116                         p2pie[ p2pielen++ ] = 0x7c;
4117                 }
4118
4119                 p2pie[ p2pielen++ ] = pwdinfo->peer_operating_ch;
4120         }
4121         else
4122         {
4123                 if ( pwdinfo->operating_channel <= 14 )
4124                 {
4125                         //      Operating Class
4126                         p2pie[ p2pielen++ ] = 0x51;
4127                 }
4128                 else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
4129                 {
4130                         //      Operating Class
4131                         p2pie[ p2pielen++ ] = 0x73;
4132                 }
4133                 else
4134                 {
4135                         //      Operating Class
4136                         p2pie[ p2pielen++ ] = 0x7c;
4137                 }
4138
4139                 //      Channel Number
4140                 p2pie[ p2pielen++ ] = pwdinfo->operating_channel;               //      Use the listen channel as the operating channel
4141         }
4142
4143
4144         //      Channel List
4145         //      Type:
4146         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
4147
4148         *(u16*) ( p2pie + p2pielen ) = 6;
4149         p2pielen += 2;
4150
4151         //      Country String
4152         p2pie[ p2pielen++ ] = 'X';
4153         p2pie[ p2pielen++ ] = 'X';
4154
4155         //      The third byte should be set to 0x04.
4156         //      Described in the "Operating Channel Attribute" section.
4157         p2pie[ p2pielen++ ] = 0x04;
4158
4159         //      Value:
4160         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
4161         {
4162                 if ( pwdinfo->peer_operating_ch <= 14 )
4163                 {
4164                         //      Operating Class
4165                         p2pie[ p2pielen++ ] = 0x51;
4166                 }
4167                 else if ( ( pwdinfo->peer_operating_ch >= 36 ) && ( pwdinfo->peer_operating_ch <= 48 ) )
4168                 {
4169                         //      Operating Class
4170                         p2pie[ p2pielen++ ] = 0x73;
4171                 }
4172                 else
4173                 {
4174                         //      Operating Class
4175                         p2pie[ p2pielen++ ] = 0x7c;
4176                 }
4177                 p2pie[ p2pielen++ ] = 1;
4178                 p2pie[ p2pielen++ ] = pwdinfo->peer_operating_ch;
4179         }
4180         else
4181         {
4182                 if ( pwdinfo->operating_channel <= 14 )
4183                 {
4184                         //      Operating Class
4185                         p2pie[ p2pielen++ ] = 0x51;
4186                 }
4187                 else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
4188                 {
4189                         //      Operating Class
4190                         p2pie[ p2pielen++ ] = 0x73;
4191                 }
4192                 else
4193                 {
4194                         //      Operating Class
4195                         p2pie[ p2pielen++ ] = 0x7c;
4196                 }
4197
4198                 //      Channel Number
4199                 p2pie[ p2pielen++ ] = 1;
4200                 p2pie[ p2pielen++ ] = pwdinfo->operating_channel;               //      Use the listen channel as the operating channel
4201         }
4202
4203         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
4204         {
4205                 //      Group ID Attribute
4206                 //      Type:
4207                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
4208
4209                 //      Length:
4210                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + pwdinfo->nego_ssidlen );
4211                 p2pielen += 2;
4212
4213                 //      Value:
4214                 //      p2P Device Address
4215                 _rtw_memcpy( p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN );
4216                 p2pielen += ETH_ALEN;
4217
4218                 //      SSID
4219                 _rtw_memcpy( p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
4220                 p2pielen += pwdinfo->nego_ssidlen;
4221         }
4222
4223         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
4224
4225 #ifdef CONFIG_WFD
4226         wfdielen = build_nego_confirm_wfd_ie(pwdinfo, pframe);
4227         pframe += wfdielen;
4228         pattrib->pktlen += wfdielen;
4229 #endif //CONFIG_WFD
4230
4231         pattrib->last_txcmdsz = pattrib->pktlen;
4232
4233         dump_mgntframe(padapter, pmgntframe);
4234
4235         return;
4236
4237 }
4238
4239 void issue_p2p_invitation_request(_adapter *padapter, u8* raddr )
4240 {
4241
4242         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
4243         u8                      action = P2P_PUB_ACTION_ACTION;
4244         u32                     p2poui = cpu_to_be32(P2POUI);
4245         u8                      oui_subtype = P2P_INVIT_REQ;
4246         u8                      p2pie[ 255 ] = { 0x00 };
4247         u8                      p2pielen = 0, i;
4248         u8                      dialogToken = 3;
4249         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
4250         u16                     len_channellist_attr = 0;
4251 #ifdef CONFIG_WFD
4252         u32                                     wfdielen = 0;
4253 #endif //CONFIG_WFD
4254 #ifdef CONFIG_CONCURRENT_MODE
4255         _adapter                                *pbuddy_adapter = padapter->pbuddy_adapter;
4256         struct wifidirect_info  *pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
4257         struct mlme_priv                *pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
4258         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4259 #endif
4260
4261         struct xmit_frame                       *pmgntframe;
4262         struct pkt_attrib                       *pattrib;
4263         unsigned char                                   *pframe;
4264         struct rtw_ieee80211_hdr        *pwlanhdr;
4265         unsigned short                          *fctrl;
4266         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
4267         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4268         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4269         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
4270
4271
4272         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4273         {
4274                 return;
4275         }
4276
4277         //update attribute
4278         pattrib = &pmgntframe->attrib;
4279         update_mgntframe_attrib(padapter, pattrib);
4280
4281         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4282
4283         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4284         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4285
4286         fctrl = &(pwlanhdr->frame_ctl);
4287         *(fctrl) = 0;
4288
4289         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
4290         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
4291         _rtw_memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
4292
4293         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4294         pmlmeext->mgnt_seq++;
4295         SetFrameSubType(pframe, WIFI_ACTION);
4296
4297         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4298         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4299
4300         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4301         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4302         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4303         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4304         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
4305
4306         //      P2P IE Section.
4307
4308         //      P2P OUI
4309         p2pielen = 0;
4310         p2pie[ p2pielen++ ] = 0x50;
4311         p2pie[ p2pielen++ ] = 0x6F;
4312         p2pie[ p2pielen++ ] = 0x9A;
4313         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
4314
4315         //      Commented by Albert 20101011
4316         //      According to the P2P Specification, the P2P Invitation request frame should contain 7 P2P attributes
4317         //      1. Configuration Timeout
4318         //      2. Invitation Flags
4319         //      3. Operating Channel    ( Only GO )
4320         //      4. P2P Group BSSID      ( Should be included if I am the GO )
4321         //      5. Channel List
4322         //      6. P2P Group ID
4323         //      7. P2P Device Info
4324
4325         //      Configuration Timeout
4326         //      Type:
4327         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
4328
4329         //      Length:
4330         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
4331         p2pielen += 2;
4332
4333         //      Value:
4334         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
4335         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
4336
4337         //      Invitation Flags
4338         //      Type:
4339         p2pie[ p2pielen++ ] = P2P_ATTR_INVITATION_FLAGS;
4340
4341         //      Length:
4342         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
4343         p2pielen += 2;
4344
4345         //      Value:
4346         p2pie[ p2pielen++ ] = P2P_INVITATION_FLAGS_PERSISTENT;
4347
4348
4349         //      Operating Channel
4350         //      Type:
4351         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4352
4353         //      Length:
4354         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4355         p2pielen += 2;
4356
4357         //      Value:
4358         //      Country String
4359         p2pie[ p2pielen++ ] = 'X';
4360         p2pie[ p2pielen++ ] = 'X';
4361
4362         //      The third byte should be set to 0x04.
4363         //      Described in the "Operating Channel Attribute" section.
4364         p2pie[ p2pielen++ ] = 0x04;
4365
4366         //      Operating Class
4367         if ( pwdinfo->invitereq_info.operating_ch <= 14 )
4368                 p2pie[ p2pielen++ ] = 0x51;
4369         else if ( ( pwdinfo->invitereq_info.operating_ch >= 36 ) && ( pwdinfo->invitereq_info.operating_ch <= 48 ) )
4370                 p2pie[ p2pielen++ ] = 0x73;
4371         else
4372                 p2pie[ p2pielen++ ] = 0x7c;
4373
4374         //      Channel Number
4375         p2pie[ p2pielen++ ] = pwdinfo->invitereq_info.operating_ch;     //      operating channel number
4376
4377         if ( _rtw_memcmp( myid( &padapter->eeprompriv ), pwdinfo->invitereq_info.go_bssid, ETH_ALEN ) )
4378         {
4379                 //      P2P Group BSSID
4380                 //      Type:
4381                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_BSSID;
4382
4383                 //      Length:
4384                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
4385                 p2pielen += 2;
4386
4387                 //      Value:
4388                 //      P2P Device Address for GO
4389                 _rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN );
4390                 p2pielen += ETH_ALEN;
4391         }
4392
4393         //      Channel List
4394         //      Type:
4395         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
4396
4397
4398         //      Length:
4399         // Country String(3)
4400         // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
4401         // + number of channels in all classes
4402         len_channellist_attr = 3
4403            + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
4404            + get_reg_classes_full_count(pmlmeext->channel_list);
4405
4406 #ifdef CONFIG_CONCURRENT_MODE
4407         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4408         {
4409                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
4410         }
4411         else
4412         {
4413                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4414         }
4415 #else
4416
4417         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4418
4419  #endif
4420         p2pielen += 2;
4421
4422         //      Value:
4423         //      Country String
4424         p2pie[ p2pielen++ ] = 'X';
4425         p2pie[ p2pielen++ ] = 'X';
4426
4427         //      The third byte should be set to 0x04.
4428         //      Described in the "Operating Channel Attribute" section.
4429         p2pie[ p2pielen++ ] = 0x04;
4430
4431         //      Channel Entry List
4432 #ifdef CONFIG_CONCURRENT_MODE
4433         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4434         {
4435                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
4436                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4437
4438                 //      Operating Class
4439                 if ( pbuddy_mlmeext->cur_channel > 14 )
4440                 {
4441                         if ( pbuddy_mlmeext->cur_channel >= 149 )
4442                         {
4443                                 p2pie[ p2pielen++ ] = 0x7c;
4444                         }
4445                         else
4446                         {
4447                                 p2pie[ p2pielen++ ] = 0x73;
4448                         }
4449                 }
4450                 else
4451                 {
4452                         p2pie[ p2pielen++ ] = 0x51;
4453                 }
4454
4455                 //      Number of Channels
4456                 //      Just support 1 channel and this channel is AP's channel
4457                 p2pie[ p2pielen++ ] = 1;
4458
4459                 //      Channel List
4460                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
4461         }
4462         else
4463         {
4464                 int i, j;
4465                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4466                         //      Operating Class
4467                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4468
4469                         //      Number of Channels
4470                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4471
4472                         //      Channel List
4473                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4474                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4475                         }
4476                 }
4477         }
4478 #else // CONFIG_CONCURRENT_MODE
4479         {
4480                 int i, j;
4481                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4482                         //      Operating Class
4483                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4484
4485                         //      Number of Channels
4486                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4487
4488                         //      Channel List
4489                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4490                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4491                         }
4492                 }
4493         }
4494 #endif // CONFIG_CONCURRENT_MODE
4495
4496
4497         //      P2P Group ID
4498         //      Type:
4499         p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
4500
4501         //      Length:
4502         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 6 + pwdinfo->invitereq_info.ssidlen );
4503         p2pielen += 2;
4504
4505         //      Value:
4506         //      P2P Device Address for GO
4507         _rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN );
4508         p2pielen += ETH_ALEN;
4509
4510         //      SSID
4511         _rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid, pwdinfo->invitereq_info.ssidlen );
4512         p2pielen += pwdinfo->invitereq_info.ssidlen;
4513
4514
4515         //      Device Info
4516         //      Type:
4517         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
4518
4519         //      Length:
4520         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
4521         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
4522         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
4523         p2pielen += 2;
4524
4525         //      Value:
4526         //      P2P Device Address
4527         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
4528         p2pielen += ETH_ALEN;
4529
4530         //      Config Method
4531         //      This field should be big endian. Noted by P2P specification.
4532         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
4533         p2pielen += 2;
4534
4535         //      Primary Device Type
4536         //      Category ID
4537         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
4538         p2pielen += 2;
4539
4540         //      OUI
4541         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
4542         p2pielen += 4;
4543
4544         //      Sub Category ID
4545         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
4546         p2pielen += 2;
4547
4548         //      Number of Secondary Device Types
4549         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
4550
4551         //      Device Name
4552         //      Type:
4553         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
4554         p2pielen += 2;
4555
4556         //      Length:
4557         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
4558         p2pielen += 2;
4559
4560         //      Value:
4561         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
4562         p2pielen += pwdinfo->device_name_len;
4563
4564         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
4565
4566 #ifdef CONFIG_WFD
4567         wfdielen = build_invitation_req_wfd_ie(pwdinfo, pframe);
4568         pframe += wfdielen;
4569         pattrib->pktlen += wfdielen;
4570 #endif //CONFIG_WFD
4571
4572         pattrib->last_txcmdsz = pattrib->pktlen;
4573
4574         dump_mgntframe(padapter, pmgntframe);
4575
4576         return;
4577
4578 }
4579
4580 void issue_p2p_invitation_response(_adapter *padapter, u8* raddr, u8 dialogToken, u8 status_code)
4581 {
4582
4583         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
4584         u8                      action = P2P_PUB_ACTION_ACTION;
4585         u32                     p2poui = cpu_to_be32(P2POUI);
4586         u8                      oui_subtype = P2P_INVIT_RESP;
4587         u8                      p2pie[ 255 ] = { 0x00 };
4588         u8                      p2pielen = 0, i;
4589         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
4590         u16                     len_channellist_attr = 0;
4591 #ifdef CONFIG_CONCURRENT_MODE
4592         _adapter                                *pbuddy_adapter = padapter->pbuddy_adapter;
4593         struct wifidirect_info  *pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
4594         struct mlme_priv                *pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
4595         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4596 #endif
4597 #ifdef CONFIG_WFD
4598         u32                                     wfdielen = 0;
4599 #endif //CONFIG_WFD
4600
4601         struct xmit_frame                       *pmgntframe;
4602         struct pkt_attrib                       *pattrib;
4603         unsigned char                                   *pframe;
4604         struct rtw_ieee80211_hdr        *pwlanhdr;
4605         unsigned short                          *fctrl;
4606         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
4607         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4608         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4609         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
4610
4611
4612         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4613         {
4614                 return;
4615         }
4616
4617         //update attribute
4618         pattrib = &pmgntframe->attrib;
4619         update_mgntframe_attrib(padapter, pattrib);
4620
4621         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4622
4623         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4624         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4625
4626         fctrl = &(pwlanhdr->frame_ctl);
4627         *(fctrl) = 0;
4628
4629         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
4630         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
4631         _rtw_memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
4632
4633         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4634         pmlmeext->mgnt_seq++;
4635         SetFrameSubType(pframe, WIFI_ACTION);
4636
4637         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4638         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4639
4640         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4641         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4642         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4643         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4644         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
4645
4646         //      P2P IE Section.
4647
4648         //      P2P OUI
4649         p2pielen = 0;
4650         p2pie[ p2pielen++ ] = 0x50;
4651         p2pie[ p2pielen++ ] = 0x6F;
4652         p2pie[ p2pielen++ ] = 0x9A;
4653         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
4654
4655         //      Commented by Albert 20101005
4656         //      According to the P2P Specification, the P2P Invitation response frame should contain 5 P2P attributes
4657         //      1. Status
4658         //      2. Configuration Timeout
4659         //      3. Operating Channel    ( Only GO )
4660         //      4. P2P Group BSSID      ( Only GO )
4661         //      5. Channel List
4662
4663         //      P2P Status
4664         //      Type:
4665         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
4666
4667         //      Length:
4668         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
4669         p2pielen += 2;
4670
4671         //      Value:
4672         //      When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE.
4673         //      Sent the event receiving the P2P Invitation Req frame to DMP UI.
4674         //      DMP had to compare the MAC address to find out the profile.
4675         //      So, the WiFi driver will send the P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB.
4676         //      If the UI found the corresponding profile, the WiFi driver sends the P2P Invitation Req
4677         //      to NB to rebuild the persistent group.
4678         p2pie[ p2pielen++ ] = status_code;
4679
4680         //      Configuration Timeout
4681         //      Type:
4682         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
4683
4684         //      Length:
4685         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
4686         p2pielen += 2;
4687
4688         //      Value:
4689         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P GO
4690         p2pie[ p2pielen++ ] = 200;      //      2 seconds needed to be the P2P Client
4691
4692         if( status_code == P2P_STATUS_SUCCESS )
4693         {
4694                 if( rtw_p2p_chk_role( pwdinfo, P2P_ROLE_GO ) )
4695                 {
4696                         //      The P2P Invitation request frame asks this Wi-Fi device to be the P2P GO
4697                         //      In this case, the P2P Invitation response frame should carry the two more P2P attributes.
4698                         //      First one is operating channel attribute.
4699                         //      Second one is P2P Group BSSID attribute.
4700
4701                         //      Operating Channel
4702                         //      Type:
4703                         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4704
4705                         //      Length:
4706                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4707                         p2pielen += 2;
4708
4709                         //      Value:
4710                         //      Country String
4711                         p2pie[ p2pielen++ ] = 'X';
4712                         p2pie[ p2pielen++ ] = 'X';
4713
4714                         //      The third byte should be set to 0x04.
4715                         //      Described in the "Operating Channel Attribute" section.
4716                         p2pie[ p2pielen++ ] = 0x04;
4717
4718                         //      Operating Class
4719                         p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
4720
4721                         //      Channel Number
4722                         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
4723
4724
4725                         //      P2P Group BSSID
4726                         //      Type:
4727                         p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_BSSID;
4728
4729                         //      Length:
4730                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
4731                         p2pielen += 2;
4732
4733                         //      Value:
4734                         //      P2P Device Address for GO
4735                         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
4736                         p2pielen += ETH_ALEN;
4737
4738                 }
4739
4740                 //      Channel List
4741                 //      Type:
4742                 p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
4743
4744                 //      Length:
4745                 // Country String(3)
4746                 // + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
4747                 // + number of channels in all classes
4748                 len_channellist_attr = 3
4749                         + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
4750                         + get_reg_classes_full_count(pmlmeext->channel_list);
4751
4752 #ifdef CONFIG_CONCURRENT_MODE
4753                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4754                 {
4755                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
4756                 }
4757                 else
4758                 {
4759                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4760                 }
4761 #else
4762
4763                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4764
4765 #endif
4766                 p2pielen += 2;
4767
4768                 //      Value:
4769                 //      Country String
4770                 p2pie[ p2pielen++ ] = 'X';
4771                 p2pie[ p2pielen++ ] = 'X';
4772
4773                 //      The third byte should be set to 0x04.
4774                 //      Described in the "Operating Channel Attribute" section.
4775                 p2pie[ p2pielen++ ] = 0x04;
4776
4777                 //      Channel Entry List
4778 #ifdef CONFIG_CONCURRENT_MODE
4779                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4780                 {
4781                         _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
4782                         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4783
4784                         //      Operating Class
4785                         if ( pbuddy_mlmeext->cur_channel > 14 )
4786                         {
4787                                 if ( pbuddy_mlmeext->cur_channel >= 149 )
4788                                 {
4789                                         p2pie[ p2pielen++ ] = 0x7c;
4790                                 }
4791                                 else
4792                                 {
4793                                         p2pie[ p2pielen++ ] = 0x73;
4794                                 }
4795                         }
4796                         else
4797                         {
4798                                 p2pie[ p2pielen++ ] = 0x51;
4799                         }
4800
4801                         //      Number of Channels
4802                         //      Just support 1 channel and this channel is AP's channel
4803                         p2pie[ p2pielen++ ] = 1;
4804
4805                         //      Channel List
4806                         p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
4807                 }
4808                 else
4809                 {
4810                         int i, j;
4811                         for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4812                                 //      Operating Class
4813                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4814
4815                                 //      Number of Channels
4816                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4817
4818                                 //      Channel List
4819                                 for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4820                                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4821                                 }
4822                         }
4823                 }
4824 #else // CONFIG_CONCURRENT_MODE
4825                 {
4826                         int i, j;
4827                         for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4828                                 //      Operating Class
4829                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4830
4831                                 //      Number of Channels
4832                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4833
4834                                 //      Channel List
4835                                 for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4836                                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4837                                 }
4838                         }
4839                 }
4840 #endif // CONFIG_CONCURRENT_MODE
4841         }
4842
4843         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
4844
4845 #ifdef CONFIG_WFD
4846         wfdielen = build_invitation_resp_wfd_ie(pwdinfo, pframe);
4847         pframe += wfdielen;
4848         pattrib->pktlen += wfdielen;
4849 #endif //CONFIG_WFD
4850
4851         pattrib->last_txcmdsz = pattrib->pktlen;
4852
4853         dump_mgntframe(padapter, pmgntframe);
4854
4855         return;
4856
4857 }
4858
4859 void issue_p2p_provision_request(_adapter *padapter, u8* pssid, u8 ussidlen, u8* pdev_raddr )
4860 {
4861         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
4862         u8                      action = P2P_PUB_ACTION_ACTION;
4863         u8                      dialogToken = 1;
4864         u32                     p2poui = cpu_to_be32(P2POUI);
4865         u8                      oui_subtype = P2P_PROVISION_DISC_REQ;
4866         u8                      wpsie[ 100 ] = { 0x00 };
4867         u8                      wpsielen = 0;
4868         u32                     p2pielen = 0;
4869 #ifdef CONFIG_WFD
4870         u32                                     wfdielen = 0;
4871 #endif //CONFIG_WFD
4872
4873         struct xmit_frame                       *pmgntframe;
4874         struct pkt_attrib                       *pattrib;
4875         unsigned char                                   *pframe;
4876         struct rtw_ieee80211_hdr        *pwlanhdr;
4877         unsigned short                          *fctrl;
4878         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
4879         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4880         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4881         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
4882
4883
4884         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4885         {
4886                 return;
4887         }
4888
4889         DBG_871X( "[%s] In\n", __FUNCTION__ );
4890         //update attribute
4891         pattrib = &pmgntframe->attrib;
4892         update_mgntframe_attrib(padapter, pattrib);
4893
4894         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4895
4896         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4897         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4898
4899         fctrl = &(pwlanhdr->frame_ctl);
4900         *(fctrl) = 0;
4901
4902         _rtw_memcpy(pwlanhdr->addr1, pdev_raddr, ETH_ALEN);
4903         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
4904         _rtw_memcpy(pwlanhdr->addr3, pdev_raddr, ETH_ALEN);
4905
4906         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4907         pmlmeext->mgnt_seq++;
4908         SetFrameSubType(pframe, WIFI_ACTION);
4909
4910         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4911         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4912
4913         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4914         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4915         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4916         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4917         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
4918
4919         p2pielen = build_prov_disc_request_p2p_ie( pwdinfo, pframe, pssid, ussidlen, pdev_raddr );
4920
4921         pframe += p2pielen;
4922         pattrib->pktlen += p2pielen;
4923
4924         wpsielen = 0;
4925         //      WPS OUI
4926         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
4927         wpsielen += 4;
4928
4929         //      WPS version
4930         //      Type:
4931         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
4932         wpsielen += 2;
4933
4934         //      Length:
4935         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
4936         wpsielen += 2;
4937
4938         //      Value:
4939         wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
4940
4941         //      Config Method
4942         //      Type:
4943         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
4944         wpsielen += 2;
4945
4946         //      Length:
4947         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
4948         wpsielen += 2;
4949
4950         //      Value:
4951         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->tx_prov_disc_info.wps_config_method_request );
4952         wpsielen += 2;
4953
4954         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
4955
4956
4957 #ifdef CONFIG_WFD
4958         wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe);
4959         pframe += wfdielen;
4960         pattrib->pktlen += wfdielen;
4961 #endif //CONFIG_WFD
4962
4963         pattrib->last_txcmdsz = pattrib->pktlen;
4964
4965         dump_mgntframe(padapter, pmgntframe);
4966
4967         return;
4968
4969 }
4970
4971
4972 u8 is_matched_in_profilelist( u8* peermacaddr, struct profile_info* profileinfo )
4973 {
4974         u8 i, match_result = 0;
4975
4976         DBG_871X( "[%s] peermac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
4977                     peermacaddr[0], peermacaddr[1],peermacaddr[2],peermacaddr[3],peermacaddr[4],peermacaddr[5]);
4978
4979         for( i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++ )
4980         {
4981                DBG_871X( "[%s] profileinfo_mac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
4982                             profileinfo->peermac[0], profileinfo->peermac[1],profileinfo->peermac[2],profileinfo->peermac[3],profileinfo->peermac[4],profileinfo->peermac[5]);
4983                 if ( _rtw_memcmp( peermacaddr, profileinfo->peermac, ETH_ALEN ) )
4984                 {
4985                         match_result = 1;
4986                         DBG_871X( "[%s] Match!\n", __FUNCTION__ );
4987                         break;
4988                 }
4989         }
4990
4991         return (match_result );
4992 }
4993
4994 void issue_probersp_p2p(_adapter *padapter, unsigned char *da)
4995 {
4996         struct xmit_frame                       *pmgntframe;
4997         struct pkt_attrib                       *pattrib;
4998         unsigned char                                   *pframe;
4999         struct rtw_ieee80211_hdr        *pwlanhdr;
5000         unsigned short                          *fctrl;
5001         unsigned char                                   *mac;
5002         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
5003         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5004         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5005         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
5006         //WLAN_BSSID_EX                 *cur_network = &(pmlmeinfo->network);
5007         u16                                     beacon_interval = 100;
5008         u16                                     capInfo = 0;
5009         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
5010         u8                                      wpsie[255] = { 0x00 };
5011         u32                                     wpsielen = 0, p2pielen = 0;
5012 #ifdef CONFIG_WFD
5013         u32                                     wfdielen = 0;
5014 #endif //CONFIG_WFD
5015 #ifdef CONFIG_INTEL_WIDI
5016         u8 zero_array_check[L2SDTA_SERVICE_VE_LEN] = { 0x00 };
5017 #endif //CONFIG_INTEL_WIDI
5018
5019         //DBG_871X("%s\n", __FUNCTION__);
5020
5021         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
5022         {
5023                 return;
5024         }
5025
5026         //update attribute
5027         pattrib = &pmgntframe->attrib;
5028         update_mgntframe_attrib(padapter, pattrib);
5029
5030         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5031
5032         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5033         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5034
5035         mac = myid(&(padapter->eeprompriv));
5036
5037         fctrl = &(pwlanhdr->frame_ctl);
5038         *(fctrl) = 0;
5039         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
5040         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
5041
5042         //      Use the device address for BSSID field.
5043         _rtw_memcpy(pwlanhdr->addr3, mac, ETH_ALEN);
5044
5045         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5046         pmlmeext->mgnt_seq++;
5047         SetFrameSubType(fctrl, WIFI_PROBERSP);
5048
5049         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
5050         pattrib->pktlen = pattrib->hdrlen;
5051         pframe += pattrib->hdrlen;
5052
5053         //timestamp will be inserted by hardware
5054         pframe += 8;
5055         pattrib->pktlen += 8;
5056
5057         // beacon interval: 2 bytes
5058         _rtw_memcpy(pframe, (unsigned char *) &beacon_interval, 2);
5059         pframe += 2;
5060         pattrib->pktlen += 2;
5061
5062         //      capability info: 2 bytes
5063         //      ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of WiFi Direct Spec)
5064         capInfo |= cap_ShortPremble;
5065         capInfo |= cap_ShortSlot;
5066
5067         _rtw_memcpy(pframe, (unsigned char *) &capInfo, 2);
5068         pframe += 2;
5069         pattrib->pktlen += 2;
5070
5071
5072         // SSID
5073         pframe = rtw_set_ie(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid, &pattrib->pktlen);
5074
5075         // supported rates...
5076         //      Use the OFDM rate in the P2P probe response frame. ( 6(B), 9(B), 12, 18, 24, 36, 48, 54 )
5077         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
5078
5079         // DS parameter set
5080         pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&pwdinfo->listen_channel, &pattrib->pktlen);
5081
5082 #ifdef CONFIG_IOCTL_CFG80211
5083         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
5084         {
5085                 if( pmlmepriv->wps_probe_resp_ie != NULL && pmlmepriv->p2p_probe_resp_ie != NULL )
5086                 {
5087                         //WPS IE
5088                         _rtw_memcpy(pframe, pmlmepriv->wps_probe_resp_ie, pmlmepriv->wps_probe_resp_ie_len);
5089                         pattrib->pktlen += pmlmepriv->wps_probe_resp_ie_len;
5090                         pframe += pmlmepriv->wps_probe_resp_ie_len;
5091
5092                         //P2P IE
5093                         _rtw_memcpy(pframe, pmlmepriv->p2p_probe_resp_ie, pmlmepriv->p2p_probe_resp_ie_len);
5094                         pattrib->pktlen += pmlmepriv->p2p_probe_resp_ie_len;
5095                         pframe += pmlmepriv->p2p_probe_resp_ie_len;
5096                 }
5097         }
5098         else
5099 #endif //CONFIG_IOCTL_CFG80211
5100         {
5101
5102                 //      Todo: WPS IE
5103                 //      Noted by Albert 20100907
5104                 //      According to the WPS specification, all the WPS attribute is presented by Big Endian.
5105
5106                 wpsielen = 0;
5107                 //      WPS OUI
5108                 *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
5109                 wpsielen += 4;
5110
5111                 //      WPS version
5112                 //      Type:
5113                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
5114                 wpsielen += 2;
5115
5116                 //      Length:
5117                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5118                 wpsielen += 2;
5119
5120                 //      Value:
5121                 wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
5122
5123 #ifdef CONFIG_INTEL_WIDI
5124                 //      Commented by Kurt
5125                 //      Appended WiDi info. only if we did issued_probereq_widi(), and then we saved ven. ext. in pmlmepriv->sa_ext.
5126                 if(  _rtw_memcmp(pmlmepriv->sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN) == _FALSE
5127                         || pmlmepriv->num_p2p_sdt != 0 )
5128                 {
5129                         //Sec dev type
5130                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SEC_DEV_TYPE_LIST );
5131                         wpsielen += 2;
5132
5133                         //      Length:
5134                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
5135                         wpsielen += 2;
5136
5137                         //      Value:
5138                         //      Category ID
5139                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_DISPLAYS );
5140                         wpsielen += 2;
5141
5142                         //      OUI
5143                         *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( INTEL_DEV_TYPE_OUI );
5144                         wpsielen += 4;
5145
5146                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_WIDI_CONSUMER_SINK );
5147                         wpsielen += 2;
5148
5149                         if(  _rtw_memcmp(pmlmepriv->sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN) == _FALSE )
5150                         {
5151                                 //      Vendor Extension
5152                                 _rtw_memcpy( wpsie + wpsielen, pmlmepriv->sa_ext, L2SDTA_SERVICE_VE_LEN );
5153                                 wpsielen += L2SDTA_SERVICE_VE_LEN;
5154                         }
5155                 }
5156 #endif //CONFIG_INTEL_WIDI
5157
5158                 //      WiFi Simple Config State
5159                 //      Type:
5160                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SIMPLE_CONF_STATE );
5161                 wpsielen += 2;
5162
5163                 //      Length:
5164                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5165                 wpsielen += 2;
5166
5167                 //      Value:
5168                 wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG;   //      Not Configured.
5169
5170                 //      Response Type
5171                 //      Type:
5172                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_RESP_TYPE );
5173                 wpsielen += 2;
5174
5175                 //      Length:
5176                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5177                 wpsielen += 2;
5178
5179                 //      Value:
5180                 wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
5181
5182                 //      UUID-E
5183                 //      Type:
5184                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_UUID_E );
5185                 wpsielen += 2;
5186
5187                 //      Length:
5188                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0010 );
5189                 wpsielen += 2;
5190
5191                 //      Value:
5192                 if (pwdinfo->external_uuid == 0) {
5193                         _rtw_memset( wpsie + wpsielen, 0x0, 16 );
5194                         _rtw_memcpy( wpsie + wpsielen, myid( &padapter->eeprompriv ), ETH_ALEN );
5195                 } else {
5196                         _rtw_memcpy( wpsie + wpsielen, pwdinfo->uuid, 0x10 );
5197                 }
5198                 wpsielen += 0x10;
5199
5200                 //      Manufacturer
5201                 //      Type:
5202                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MANUFACTURER );
5203                 wpsielen += 2;
5204
5205                 //      Length:
5206                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0007 );
5207                 wpsielen += 2;
5208
5209                 //      Value:
5210                 _rtw_memcpy( wpsie + wpsielen, "Realtek", 7 );
5211                 wpsielen += 7;
5212
5213                 //      Model Name
5214                 //      Type:
5215                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MODEL_NAME );
5216                 wpsielen += 2;
5217
5218                 //      Length:
5219                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0006 );
5220                 wpsielen += 2;
5221
5222                 //      Value:
5223                 _rtw_memcpy( wpsie + wpsielen, "8192CU", 6 );
5224                 wpsielen += 6;
5225
5226                 //      Model Number
5227                 //      Type:
5228                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MODEL_NUMBER );
5229                 wpsielen += 2;
5230
5231                 //      Length:
5232                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5233                 wpsielen += 2;
5234
5235                 //      Value:
5236                 wpsie[ wpsielen++ ] = 0x31;             //      character 1
5237
5238                 //      Serial Number
5239                 //      Type:
5240                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SERIAL_NUMBER );
5241                 wpsielen += 2;
5242
5243                 //      Length:
5244                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( ETH_ALEN );
5245                 wpsielen += 2;
5246
5247                 //      Value:
5248                 _rtw_memcpy( wpsie + wpsielen, "123456" , ETH_ALEN );
5249                 wpsielen += ETH_ALEN;
5250
5251                 //      Primary Device Type
5252                 //      Type:
5253                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
5254                 wpsielen += 2;
5255
5256                 //      Length:
5257                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
5258                 wpsielen += 2;
5259
5260                 //      Value:
5261                 //      Category ID
5262                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
5263                 wpsielen += 2;
5264
5265                 //      OUI
5266                 *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( WPSOUI );
5267                 wpsielen += 4;
5268
5269                 //      Sub Category ID
5270                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
5271                 wpsielen += 2;
5272
5273                 //      Device Name
5274                 //      Type:
5275                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
5276                 wpsielen += 2;
5277
5278                 //      Length:
5279                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->device_name_len );
5280                 wpsielen += 2;
5281
5282                 //      Value:
5283                 _rtw_memcpy( wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len );
5284                 wpsielen += pwdinfo->device_name_len;
5285
5286                 //      Config Method
5287                 //      Type:
5288                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
5289                 wpsielen += 2;
5290
5291                 //      Length:
5292                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
5293                 wpsielen += 2;
5294
5295                 //      Value:
5296                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
5297                 wpsielen += 2;
5298
5299
5300                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
5301
5302
5303                 p2pielen = build_probe_resp_p2p_ie(pwdinfo, pframe);
5304                 pframe += p2pielen;
5305                 pattrib->pktlen += p2pielen;
5306         }
5307
5308 #ifdef CONFIG_WFD
5309 #ifdef CONFIG_IOCTL_CFG80211
5310         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
5311 #endif //CONFIG_IOCTL_CFG80211
5312         {
5313                 wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
5314                 pframe += wfdielen;
5315                 pattrib->pktlen += wfdielen;
5316         }
5317 #ifdef CONFIG_IOCTL_CFG80211
5318         else if (pmlmepriv->wfd_probe_resp_ie != NULL && pmlmepriv->wfd_probe_resp_ie_len>0)
5319         {
5320                 //WFD IE
5321                 _rtw_memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, pmlmepriv->wfd_probe_resp_ie_len);
5322                 pattrib->pktlen += pmlmepriv->wfd_probe_resp_ie_len;
5323                 pframe += pmlmepriv->wfd_probe_resp_ie_len;
5324         }
5325 #endif //CONFIG_IOCTL_CFG80211
5326 #endif //CONFIG_WFD
5327
5328         pattrib->last_txcmdsz = pattrib->pktlen;
5329
5330
5331         dump_mgntframe(padapter, pmgntframe);
5332
5333         return;
5334
5335 }
5336
5337 int _issue_probereq_p2p(_adapter *padapter, u8 *da, int wait_ack)
5338 {
5339         int ret = _FAIL;
5340         struct xmit_frame               *pmgntframe;
5341         struct pkt_attrib               *pattrib;
5342         unsigned char                   *pframe;
5343         struct rtw_ieee80211_hdr        *pwlanhdr;
5344         unsigned short          *fctrl;
5345         unsigned char                   *mac;
5346         unsigned char                   bssrate[NumRates];
5347         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
5348         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5349         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5350         int     bssrate_len = 0;
5351         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
5352         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
5353         u8                                      wpsie[255] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
5354         u16                                     wpsielen = 0, p2pielen = 0;
5355 #ifdef CONFIG_WFD
5356         u32                                     wfdielen = 0;
5357 #endif //CONFIG_WFD
5358
5359         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
5360
5361
5362         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
5363         {
5364                 goto exit;
5365         }
5366
5367         //update attribute
5368         pattrib = &pmgntframe->attrib;
5369         update_mgntframe_attrib(padapter, pattrib);
5370
5371
5372         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5373
5374         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5375         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5376
5377         mac = myid(&(padapter->eeprompriv));
5378
5379         fctrl = &(pwlanhdr->frame_ctl);
5380         *(fctrl) = 0;
5381
5382         if (da) {
5383                 _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
5384                 _rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN);
5385         } else {
5386                 if ( ( pwdinfo->p2p_info.scan_op_ch_only ) || ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) )
5387                 {
5388                         //      This two flags will be set when this is only the P2P client mode.
5389                         _rtw_memcpy(pwlanhdr->addr1, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
5390                         _rtw_memcpy(pwlanhdr->addr3, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
5391                 }
5392                 else
5393                 {
5394                         //      broadcast probe request frame
5395                         _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
5396                         _rtw_memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
5397                 }
5398         }
5399         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
5400
5401         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
5402         pmlmeext->mgnt_seq++;
5403         SetFrameSubType(pframe, WIFI_PROBEREQ);
5404
5405         pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
5406         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
5407
5408         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
5409         {
5410                 pframe = rtw_set_ie(pframe, _SSID_IE_, pwdinfo->tx_prov_disc_info.ssid.SsidLength, pwdinfo->tx_prov_disc_info.ssid.Ssid, &(pattrib->pktlen));
5411         }
5412         else
5413         {
5414                 pframe = rtw_set_ie(pframe, _SSID_IE_, P2P_WILDCARD_SSID_LEN, pwdinfo->p2p_wildcard_ssid, &(pattrib->pktlen));
5415         }
5416         //      Use the OFDM rate in the P2P probe request frame. ( 6(B), 9(B), 12(B), 24(B), 36, 48, 54 )
5417         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
5418
5419 #ifdef CONFIG_IOCTL_CFG80211
5420         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
5421         {
5422                 if( pmlmepriv->wps_probe_req_ie != NULL && pmlmepriv->p2p_probe_req_ie != NULL )
5423                 {
5424                         //WPS IE
5425                         _rtw_memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
5426                         pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
5427                         pframe += pmlmepriv->wps_probe_req_ie_len;
5428
5429                         //P2P IE
5430                         _rtw_memcpy(pframe, pmlmepriv->p2p_probe_req_ie, pmlmepriv->p2p_probe_req_ie_len);
5431                         pattrib->pktlen += pmlmepriv->p2p_probe_req_ie_len;
5432                         pframe += pmlmepriv->p2p_probe_req_ie_len;
5433                 }
5434         }
5435         else
5436 #endif //CONFIG_IOCTL_CFG80211
5437         {
5438
5439                 //      WPS IE
5440                 //      Noted by Albert 20110221
5441                 //      According to the WPS specification, all the WPS attribute is presented by Big Endian.
5442
5443                 wpsielen = 0;
5444                 //      WPS OUI
5445                 *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
5446                 wpsielen += 4;
5447
5448                 //      WPS version
5449                 //      Type:
5450                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
5451                 wpsielen += 2;
5452
5453                 //      Length:
5454                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
5455                 wpsielen += 2;
5456
5457                 //      Value:
5458                 wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
5459
5460                 if( pmlmepriv->wps_probe_req_ie == NULL )
5461                 {
5462                         //      UUID-E
5463                         //      Type:
5464                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_UUID_E );
5465                         wpsielen += 2;
5466
5467                         //      Length:
5468                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0010 );
5469                         wpsielen += 2;
5470
5471                         //      Value:
5472                         if (pwdinfo->external_uuid == 0) {
5473                                 _rtw_memset( wpsie + wpsielen, 0x0, 16 );
5474                                 _rtw_memcpy( wpsie + wpsielen, myid( &padapter->eeprompriv ), ETH_ALEN );
5475                         } else {
5476                                 _rtw_memcpy( wpsie + wpsielen, pwdinfo->uuid, 0x10 );
5477                         }
5478                         wpsielen += 0x10;
5479
5480                         //      Config Method
5481                         //      Type:
5482                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
5483                         wpsielen += 2;
5484
5485                         //      Length:
5486                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
5487                         wpsielen += 2;
5488
5489                         //      Value:
5490                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
5491                         wpsielen += 2;
5492                 }
5493
5494                 //      Device Name
5495                 //      Type:
5496                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
5497                 wpsielen += 2;
5498
5499                 //      Length:
5500                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->device_name_len );
5501                 wpsielen += 2;
5502
5503                 //      Value:
5504                 _rtw_memcpy( wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len );
5505                 wpsielen += pwdinfo->device_name_len;
5506
5507                 //      Primary Device Type
5508                 //      Type:
5509                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
5510                 wpsielen += 2;
5511
5512                 //      Length:
5513                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
5514                 wpsielen += 2;
5515
5516                 //      Value:
5517                 //      Category ID
5518                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_RTK_WIDI );
5519                 wpsielen += 2;
5520
5521                 //      OUI
5522                 *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( WPSOUI );
5523                 wpsielen += 4;
5524
5525                 //      Sub Category ID
5526                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_RTK_DMP );
5527                 wpsielen += 2;
5528
5529                 //      Device Password ID
5530                 //      Type:
5531                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
5532                 wpsielen += 2;
5533
5534                 //      Length:
5535                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
5536                 wpsielen += 2;
5537
5538                 //      Value:
5539                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );  //      Registrar-specified
5540                 wpsielen += 2;
5541
5542                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
5543
5544                 //      P2P OUI
5545                 p2pielen = 0;
5546                 p2pie[ p2pielen++ ] = 0x50;
5547                 p2pie[ p2pielen++ ] = 0x6F;
5548                 p2pie[ p2pielen++ ] = 0x9A;
5549                 p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
5550
5551                 //      Commented by Albert 20110221
5552                 //      According to the P2P Specification, the probe request frame should contain 5 P2P attributes
5553                 //      1. P2P Capability
5554                 //      2. P2P Device ID if this probe request wants to find the specific P2P device
5555                 //      3. Listen Channel
5556                 //      4. Extended Listen Timing
5557                 //      5. Operating Channel if this WiFi is working as the group owner now
5558
5559                 //      P2P Capability
5560                 //      Type:
5561                 p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
5562
5563                 //      Length:
5564                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
5565                 p2pielen += 2;
5566
5567                 //      Value:
5568                 //      Device Capability Bitmap, 1 byte
5569                 p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
5570
5571                 //      Group Capability Bitmap, 1 byte
5572                 if ( pwdinfo->persistent_supported )
5573                         p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
5574                 else
5575                         p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
5576
5577                 //      Listen Channel
5578                 //      Type:
5579                 p2pie[ p2pielen++ ] = P2P_ATTR_LISTEN_CH;
5580
5581                 //      Length:
5582                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
5583                 p2pielen += 2;
5584
5585                 //      Value:
5586                 //      Country String
5587                 p2pie[ p2pielen++ ] = 'X';
5588                 p2pie[ p2pielen++ ] = 'X';
5589
5590                 //      The third byte should be set to 0x04.
5591                 //      Described in the "Operating Channel Attribute" section.
5592                 p2pie[ p2pielen++ ] = 0x04;
5593
5594                 //      Operating Class
5595                 p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
5596
5597                 //      Channel Number
5598                 p2pie[ p2pielen++ ] = pwdinfo->listen_channel;  //      listen channel
5599
5600
5601                 //      Extended Listen Timing
5602                 //      Type:
5603                 p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
5604
5605                 //      Length:
5606                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
5607                 p2pielen += 2;
5608
5609                 //      Value:
5610                 //      Availability Period
5611                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
5612                 p2pielen += 2;
5613
5614                 //      Availability Interval
5615                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
5616                 p2pielen += 2;
5617
5618                 if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
5619                 {
5620                         //      Operating Channel (if this WiFi is working as the group owner now)
5621                         //      Type:
5622                         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
5623
5624                         //      Length:
5625                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
5626                         p2pielen += 2;
5627
5628                         //      Value:
5629                         //      Country String
5630                         p2pie[ p2pielen++ ] = 'X';
5631                         p2pie[ p2pielen++ ] = 'X';
5632
5633                         //      The third byte should be set to 0x04.
5634                         //      Described in the "Operating Channel Attribute" section.
5635                         p2pie[ p2pielen++ ] = 0x04;
5636
5637                         //      Operating Class
5638                         p2pie[ p2pielen++ ] = 0x51;     //      Copy from SD7
5639
5640                         //      Channel Number
5641                         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       //      operating channel number
5642
5643                 }
5644
5645                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
5646
5647         }
5648
5649 #ifdef CONFIG_WFD
5650 #ifdef CONFIG_IOCTL_CFG80211
5651         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
5652 #endif
5653         {
5654                 wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe);
5655                 pframe += wfdielen;
5656                 pattrib->pktlen += wfdielen;
5657         }
5658 #ifdef CONFIG_IOCTL_CFG80211
5659         else if (pmlmepriv->wfd_probe_req_ie != NULL && pmlmepriv->wfd_probe_req_ie_len>0)
5660         {
5661                 //WFD IE
5662                 _rtw_memcpy(pframe, pmlmepriv->wfd_probe_req_ie, pmlmepriv->wfd_probe_req_ie_len);
5663                 pattrib->pktlen += pmlmepriv->wfd_probe_req_ie_len;
5664                 pframe += pmlmepriv->wfd_probe_req_ie_len;
5665         }
5666 #endif //CONFIG_IOCTL_CFG80211
5667 #endif //CONFIG_WFD
5668
5669         pattrib->last_txcmdsz = pattrib->pktlen;
5670
5671         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
5672
5673         if (wait_ack) {
5674                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
5675         } else {
5676                 dump_mgntframe(padapter, pmgntframe);
5677                 ret = _SUCCESS;
5678         }
5679
5680 exit:
5681         return ret;
5682 }
5683
5684 inline void issue_probereq_p2p(_adapter *adapter, u8 *da)
5685 {
5686         _issue_probereq_p2p(adapter, da, _FALSE);
5687 }
5688
5689 int issue_probereq_p2p_ex(_adapter *adapter, u8 *da, int try_cnt, int wait_ms)
5690 {
5691         int ret;
5692         int i = 0;
5693         u32 start = rtw_get_current_time();
5694
5695         do
5696         {
5697                 ret = _issue_probereq_p2p(adapter, da, wait_ms>0?_TRUE:_FALSE);
5698
5699                 i++;
5700
5701                 if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
5702                         break;
5703
5704                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
5705                         rtw_msleep_os(wait_ms);
5706
5707         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
5708
5709         if (ret != _FAIL) {
5710                 ret = _SUCCESS;
5711                 #ifndef DBG_XMIT_ACK
5712                 goto exit;
5713                 #endif
5714         }
5715
5716         if (try_cnt && wait_ms) {
5717                 if (da)
5718                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
5719                                 FUNC_ADPT_ARG(adapter), MAC_ARG(da), rtw_get_oper_ch(adapter),
5720                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
5721                 else
5722                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
5723                                 FUNC_ADPT_ARG(adapter), rtw_get_oper_ch(adapter),
5724                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
5725         }
5726 exit:
5727         return ret;
5728 }
5729
5730 #endif //CONFIG_P2P
5731
5732 s32 rtw_action_public_decache(union recv_frame *recv_frame, s32 token)
5733 {
5734         _adapter *adapter = recv_frame->u.hdr.adapter;
5735         struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
5736         u8 *frame = recv_frame->u.hdr.rx_data;
5737         u16 seq_ctrl = ( (recv_frame->u.hdr.attrib.seq_num&0xffff) << 4) |
5738                 (recv_frame->u.hdr.attrib.frag_num & 0xf);
5739
5740         if (GetRetry(frame)) {
5741                 if (token >= 0) {
5742                         if ((seq_ctrl == mlmeext->action_public_rxseq)
5743                                 && (token == mlmeext->action_public_dialog_token))
5744                         {
5745                                 DBG_871X(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x, token:%d\n",
5746                                         FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
5747                                 return _FAIL;
5748                         }
5749                 } else {
5750                         if (seq_ctrl == mlmeext->action_public_rxseq) {
5751                                 DBG_871X(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x\n",
5752                                         FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq);
5753                                 return _FAIL;
5754                         }
5755                 }
5756         }
5757
5758         mlmeext->action_public_rxseq = seq_ctrl;
5759
5760         if (token >= 0)
5761                 mlmeext->action_public_dialog_token = token;
5762
5763         return _SUCCESS;
5764 }
5765
5766 unsigned int on_action_public_p2p(union recv_frame *precv_frame)
5767 {
5768         _adapter *padapter = precv_frame->u.hdr.adapter;
5769         u8 *pframe = precv_frame->u.hdr.rx_data;
5770         uint len = precv_frame->u.hdr.len;
5771         u8 *frame_body;
5772         u8 dialogToken=0;
5773 #ifdef CONFIG_P2P
5774         u8 *p2p_ie;
5775         u32     p2p_ielen, wps_ielen;
5776         struct  wifidirect_info *pwdinfo = &( padapter->wdinfo );
5777         u8      result = P2P_STATUS_SUCCESS;
5778         u8      empty_addr[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
5779         u8 *merged_p2pie = NULL;
5780         u32 merged_p2p_ielen= 0;
5781 #endif //CONFIG_P2P
5782
5783         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
5784
5785         dialogToken = frame_body[7];
5786
5787         if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
5788                 return _FAIL;
5789
5790 #ifdef CONFIG_P2P
5791         _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
5792 #ifdef CONFIG_IOCTL_CFG80211
5793         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211)
5794         {
5795                 rtw_cfg80211_rx_p2p_action_public(padapter, pframe, len);
5796         }
5797         else
5798 #endif //CONFIG_IOCTL_CFG80211
5799         {
5800                 //      Do nothing if the driver doesn't enable the P2P function.
5801                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
5802                         return _SUCCESS;
5803
5804                 len -= sizeof(struct rtw_ieee80211_hdr_3addr);
5805
5806                 switch( frame_body[ 6 ] )//OUI Subtype
5807                 {
5808                         case P2P_GO_NEGO_REQ:
5809                         {
5810                                 DBG_871X( "[%s] Got GO Nego Req Frame\n", __FUNCTION__);
5811                                 _rtw_memset( &pwdinfo->groupid_info, 0x00, sizeof( struct group_id_info ) );
5812
5813                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
5814                                 {
5815                                         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
5816                                 }
5817
5818                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
5819                                 {
5820                                         //      Commented by Albert 20110526
5821                                         //      In this case, this means the previous nego fail doesn't be reset yet.
5822                                         _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
5823                                         //      Restore the previous p2p state
5824                                         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
5825                                         DBG_871X( "[%s] Restore the previous p2p state to %d\n", __FUNCTION__, rtw_p2p_state(pwdinfo) );
5826                                 }
5827 #ifdef CONFIG_CONCURRENT_MODE
5828                                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
5829                                 {
5830                                         _cancel_timer_ex( &pwdinfo->ap_p2p_switch_timer );
5831                                 }
5832 #endif // CONFIG_CONCURRENT_MODE
5833
5834                                 //      Commented by Kurt 20110902
5835                                 //Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered.
5836                                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
5837                                         rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
5838
5839                                 //      Commented by Kurt 20120113
5840                                 //      Get peer_dev_addr here if peer doesn't issue prov_disc frame.
5841                                 if( _rtw_memcmp(pwdinfo->rx_prov_disc_info.peerDevAddr, empty_addr, ETH_ALEN) )
5842                                         _rtw_memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
5843
5844                                 result = process_p2p_group_negotation_req( pwdinfo, frame_body, len );
5845                                 issue_p2p_GO_response( padapter, GetAddr2Ptr(pframe), frame_body, len, result );
5846 #ifdef CONFIG_INTEL_WIDI
5847                                 if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) {
5848                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
5849                                         _cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
5850                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
5851                                 }
5852 #endif //CONFIG_INTEL_WIDI
5853
5854                                 //      Commented by Albert 20110718
5855                                 //      No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer.
5856 #ifdef CONFIG_CONCURRENT_MODE
5857                                 //      Commented by Albert 20120107
5858                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
5859 #else // CONFIG_CONCURRENT_MODE
5860                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
5861 #endif // CONFIG_CONCURRENT_MODE
5862                                 break;
5863                         }
5864                         case P2P_GO_NEGO_RESP:
5865                         {
5866                                 DBG_871X( "[%s] Got GO Nego Resp Frame\n", __FUNCTION__);
5867
5868                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
5869                                 {
5870                                         //      Commented by Albert 20110425
5871                                         //      The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function.
5872                                         _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
5873                                         pwdinfo->nego_req_info.benable = _FALSE;
5874                                         result = process_p2p_group_negotation_resp( pwdinfo, frame_body, len);
5875                                         issue_p2p_GO_confirm( pwdinfo->padapter, GetAddr2Ptr(pframe), result);
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
5891                                         //      Reset the dialog token for group negotiation frames.
5892                                         pwdinfo->negotiation_dialog_token = 1;
5893
5894                                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
5895                                         {
5896                                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
5897                                         }
5898                                 }
5899                                 else
5900                                 {
5901                                         DBG_871X( "[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __FUNCTION__);
5902                                 }
5903
5904                                 break;
5905                         }
5906                         case P2P_GO_NEGO_CONF:
5907                         {
5908                                 DBG_871X( "[%s] Got GO Nego Confirm Frame\n", __FUNCTION__);
5909                                 result = process_p2p_group_negotation_confirm( pwdinfo, frame_body, len);
5910                                 if ( P2P_STATUS_SUCCESS == result )
5911                                 {
5912                                         if ( rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT )
5913                                         {
5914                                                 pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
5915                                                 #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
5916                                                 pwdinfo->p2p_info.operation_ch[ 1 ] = 1;        //Check whether GO is operating in channel 1;
5917                                                 pwdinfo->p2p_info.operation_ch[ 2 ] = 6;        //Check whether GO is operating in channel 6;
5918                                                 pwdinfo->p2p_info.operation_ch[ 3 ] = 11;       //Check whether GO is operating in channel 11;
5919                                                 #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
5920                                                 pwdinfo->p2p_info.scan_op_ch_only = 1;
5921                                                 _set_timer( &pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH );
5922                                         }
5923                                 }
5924                                 break;
5925                         }
5926                         case P2P_INVIT_REQ:
5927                         {
5928                                 //      Added by Albert 2010/10/05
5929                                 //      Received the P2P Invite Request frame.
5930
5931                                 DBG_871X( "[%s] Got invite request frame!\n", __FUNCTION__ );
5932                                 if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
5933                                 {
5934                                         //      Parse the necessary information from the P2P Invitation Request frame.
5935                                         //      For example: The MAC address of sending this P2P Invitation Request frame.
5936                                         u32     attr_contentlen = 0;
5937                                         u8      status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
5938                                         struct group_id_info group_id;
5939                                         u8      invitation_flag = 0;
5940                                         int j=0;
5941
5942                                         merged_p2p_ielen = rtw_get_p2p_merged_ies_len(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_);
5943
5944                                         merged_p2pie = rtw_zmalloc(merged_p2p_ielen + 2);       // 2 is for EID and Length
5945                                         if (merged_p2pie == NULL)
5946                                         {
5947                                                 DBG_871X( "[%s] Malloc p2p ie fail\n", __FUNCTION__);
5948                                                 goto exit;
5949                                         }
5950                                         _rtw_memset(merged_p2pie, 0x00, merged_p2p_ielen);
5951
5952                                         merged_p2p_ielen = rtw_p2p_merge_ies(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, merged_p2pie);
5953
5954                                         rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen);
5955                                         if ( attr_contentlen )
5956                                         {
5957
5958                                                 rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen);
5959                                                 //      Commented by Albert 20120510
5960                                                 //      Copy to the pwdinfo->p2p_peer_interface_addr.
5961                                                 //      So that the WFD UI ( or Sigma ) can get the peer interface address by using the following command.
5962                                                 //      #> iwpriv wlan0 p2p_get peer_ifa
5963                                                 //      After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant.
5964
5965                                                 if ( attr_contentlen )
5966                                                 {
5967                                                         DBG_871X( "[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
5968                                                                         pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1],
5969                                                                         pwdinfo->p2p_peer_interface_addr[2], pwdinfo->p2p_peer_interface_addr[3],
5970                                                                         pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5] );
5971                                                 }
5972
5973                                                 if ( invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT )
5974                                                 {
5975                                                         //      Re-invoke the persistent group.
5976
5977                                                         _rtw_memset( &group_id, 0x00, sizeof( struct group_id_info ) );
5978                                                         rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_ID, ( u8* ) &group_id, &attr_contentlen);
5979                                                         if ( attr_contentlen )
5980                                                         {
5981                                                                 if ( _rtw_memcmp( group_id.go_device_addr, myid( &padapter->eeprompriv ), ETH_ALEN ) )
5982                                                                 {
5983                                                                         //      The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO.
5984                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO );
5985                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_GO );
5986                                                                         status_code = P2P_STATUS_SUCCESS;
5987                                                                 }
5988                                                                 else
5989                                                                 {
5990                                                                         //      The p2p device sending this p2p invitation request wants to be the persistent GO.
5991                                                                         if ( is_matched_in_profilelist( pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[ 0 ] ) )
5992                                                                         {
5993                                                                                 u8 operatingch_info[5] = { 0x00 };
5994                                                                                 if ( rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen) )
5995                                                                                 {
5996                                                                                         if( rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, (u32)operatingch_info[4] ) >= 0 )
5997                                                                                         {
5998                                                                                                 //      The operating channel is acceptable for this device.
5999                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[0]= operatingch_info[4];
6000                                                                                                 #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
6001                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[1]= 1;          //Check whether GO is operating in channel 1;
6002                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[2]= 6;          //Check whether GO is operating in channel 6;
6003                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[3]= 11;         //Check whether GO is operating in channel 11;
6004                                                                                                 #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
6005                                                                                                 pwdinfo->rx_invitereq_info.scan_op_ch_only = 1;
6006                                                                                                 _set_timer( &pwdinfo->reset_ch_sitesurvey, P2P_RESET_SCAN_CH );
6007                                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH );
6008                                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
6009                                                                                                 status_code = P2P_STATUS_SUCCESS;
6010                                                                                         }
6011                                                                                         else
6012                                                                                         {
6013                                                                                                 //      The operating channel isn't supported by this device.
6014                                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
6015                                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_DEVICE );
6016                                                                                                 status_code = P2P_STATUS_FAIL_NO_COMMON_CH;
6017                                                                                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
6018                                                                                         }
6019                                                                                 }
6020                                                                                 else
6021                                                                                 {
6022                                                                                         //      Commented by Albert 20121130
6023                                                                                         //      Intel will use the different P2P IE to store the operating channel information
6024                                                                                         //      Workaround for Intel WiDi 3.5
6025                                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH );
6026                                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
6027                                                                                         status_code = P2P_STATUS_SUCCESS;
6028                                                                                 }
6029                                                                         }
6030                                                                         else
6031                                                                         {
6032                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
6033                                                                                 #ifdef CONFIG_INTEL_WIDI
6034                                                                                 _rtw_memcpy( pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN );
6035                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
6036                                                                                 #endif //CONFIG_INTEL_WIDI
6037
6038                                                                                 status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
6039                                                                         }
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                                                 else
6049                                                 {
6050                                                         //      Received the invitation to join a P2P group.
6051
6052                                                         _rtw_memset( &group_id, 0x00, sizeof( struct group_id_info ) );
6053                                                         rtw_get_p2p_attr_content( merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_ID, ( u8* ) &group_id, &attr_contentlen);
6054                                                         if ( attr_contentlen )
6055                                                         {
6056                                                                 if ( _rtw_memcmp( group_id.go_device_addr, myid( &padapter->eeprompriv ), ETH_ALEN ) )
6057                                                                 {
6058                                                                         //      In this case, the GO can't be myself.
6059                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
6060                                                                         status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6061                                                                 }
6062                                                                 else
6063                                                                 {
6064                                                                         //      The p2p device sending this p2p invitation request wants to join an existing P2P group
6065                                                                         //      Commented by Albert 2012/06/28
6066                                                                         //      In this case, this Wi-Fi device should use the iwpriv command to get the peer device address.
6067                                                                         //      The peer device address should be the destination address for the provisioning discovery request.
6068                                                                         //      Then, this Wi-Fi device should use the iwpriv command to get the peer interface address.
6069                                                                         //      The peer interface address should be the address for WPS mac address
6070                                                                         _rtw_memcpy( pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN );
6071                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
6072                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN );
6073                                                                         status_code = P2P_STATUS_SUCCESS;
6074                                                                 }
6075                                                         }
6076                                                         else
6077                                                         {
6078                                                                 DBG_871X( "[%s] P2P Group ID Attribute NOT FOUND!\n", __FUNCTION__ );
6079                                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6080                                                         }
6081                                                 }
6082                                         }
6083                                         else
6084                                         {
6085                                                 DBG_871X( "[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __FUNCTION__ );
6086                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
6087                                         }
6088
6089                                         DBG_871X( "[%s] status_code = %d\n", __FUNCTION__, status_code );
6090
6091                                         pwdinfo->inviteresp_info.token = frame_body[ 7 ];
6092                                         issue_p2p_invitation_response( padapter, GetAddr2Ptr(pframe), pwdinfo->inviteresp_info.token, status_code );
6093                                         _set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
6094                                 }
6095 #ifdef CONFIG_INTEL_WIDI
6096                                 if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) {
6097                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
6098                                         _cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
6099                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
6100                                 }
6101 #endif //CONFIG_INTEL_WIDI
6102                                 break;
6103                         }
6104                         case P2P_INVIT_RESP:
6105                         {
6106                                 u8      attr_content = 0x00;
6107                                 u32     attr_contentlen = 0;
6108
6109                                 DBG_871X( "[%s] Got invite response frame!\n", __FUNCTION__ );
6110                                 _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
6111                                 if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
6112                                 {
6113                                         rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
6114
6115                                         if ( attr_contentlen == 1 )
6116                                         {
6117                                                 DBG_871X( "[%s] Status = %d\n", __FUNCTION__, attr_content );
6118                                                 pwdinfo->invitereq_info.benable = _FALSE;
6119
6120                                                 if ( attr_content == P2P_STATUS_SUCCESS )
6121                                                 {
6122                                                         if ( _rtw_memcmp( pwdinfo->invitereq_info.go_bssid, myid( &padapter->eeprompriv ), ETH_ALEN ))
6123                                                         {
6124                                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO );
6125                                                         }
6126                                                         else
6127                                                         {
6128                                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
6129                                                         }
6130                                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_OK );
6131                                                 }
6132                                                 else
6133                                                 {
6134                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
6135                                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
6136                                                 }
6137                                         }
6138                                         else
6139                                         {
6140                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
6141                                                 rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
6142                                         }
6143                                 }
6144                                 else
6145                                 {
6146                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
6147                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
6148                                 }
6149
6150                                 if ( rtw_p2p_chk_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL ) )
6151                                 {
6152                                         _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
6153                                 }
6154                                 break;
6155                         }
6156                         case P2P_DEVDISC_REQ:
6157
6158                                 process_p2p_devdisc_req(pwdinfo, pframe, len);
6159
6160                                 break;
6161
6162                         case P2P_DEVDISC_RESP:
6163
6164                                 process_p2p_devdisc_resp(pwdinfo, pframe, len);
6165
6166                                 break;
6167
6168                         case P2P_PROVISION_DISC_REQ:
6169                                 DBG_871X( "[%s] Got Provisioning Discovery Request Frame\n", __FUNCTION__ );
6170                                 process_p2p_provdisc_req(pwdinfo, pframe, len);
6171                                 _rtw_memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
6172
6173                                 //20110902 Kurt
6174                                 //Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered.
6175                                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
6176                                         rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
6177
6178                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ);
6179                                 _set_timer( &pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT );
6180 #ifdef CONFIG_INTEL_WIDI
6181                                 if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) {
6182                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
6183                                         _cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
6184                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
6185                                 }
6186 #endif //CONFIG_INTEL_WIDI
6187                                 break;
6188
6189                         case P2P_PROVISION_DISC_RESP:
6190                                 //      Commented by Albert 20110707
6191                                 //      Should we check the pwdinfo->tx_prov_disc_info.bsent flag here??
6192                                 DBG_871X( "[%s] Got Provisioning Discovery Response Frame\n", __FUNCTION__ );
6193                                 //      Commented by Albert 20110426
6194                                 //      The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function.
6195                                 _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
6196                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP);
6197                                 process_p2p_provdisc_resp(pwdinfo, pframe);
6198                                 _set_timer( &pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT );
6199                                 break;
6200
6201                 }
6202         }
6203
6204
6205 exit:
6206
6207         if(merged_p2pie)
6208         {
6209                 rtw_mfree(merged_p2pie, merged_p2p_ielen + 2);
6210         }
6211 #endif //CONFIG_P2P
6212         return _SUCCESS;
6213 }
6214
6215 unsigned int on_action_public_vendor(union recv_frame *precv_frame)
6216 {
6217         unsigned int ret = _FAIL;
6218         u8 *pframe = precv_frame->u.hdr.rx_data;
6219         uint frame_len = precv_frame->u.hdr.len;
6220         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6221
6222         if (_rtw_memcmp(frame_body + 2, P2P_OUI, 4) == _TRUE) {
6223                 ret = on_action_public_p2p(precv_frame);
6224         }
6225
6226         return ret;
6227 }
6228
6229 unsigned int on_action_public_default(union recv_frame *precv_frame, u8 action)
6230 {
6231         unsigned int ret = _FAIL;
6232         u8 *pframe = precv_frame->u.hdr.rx_data;
6233         uint frame_len = precv_frame->u.hdr.len;
6234         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6235         u8 token;
6236         _adapter *adapter = precv_frame->u.hdr.adapter;
6237         int cnt = 0;
6238         char msg[64];
6239
6240         token = frame_body[2];
6241
6242         if (rtw_action_public_decache(precv_frame, token) == _FAIL)
6243                 goto exit;
6244
6245         #ifdef CONFIG_IOCTL_CFG80211
6246         cnt += sprintf((msg+cnt), "%s(token:%u)", action_public_str(action), token);
6247         rtw_cfg80211_rx_action(adapter, pframe, frame_len, msg);
6248         #endif
6249
6250         ret = _SUCCESS;
6251
6252 exit:
6253         return ret;
6254 }
6255
6256 unsigned int on_action_public(_adapter *padapter, union recv_frame *precv_frame)
6257 {
6258         unsigned int ret = _FAIL;
6259         u8 *pframe = precv_frame->u.hdr.rx_data;
6260         uint frame_len = precv_frame->u.hdr.len;
6261         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6262         u8 category, action;
6263
6264         /* check RA matches or not */
6265         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
6266                 goto exit;
6267
6268         category = frame_body[0];
6269         if(category != RTW_WLAN_CATEGORY_PUBLIC)
6270                 goto exit;
6271
6272         action = frame_body[1];
6273         switch (action) {
6274         case ACT_PUBLIC_VENDOR:
6275                 ret = on_action_public_vendor(precv_frame);
6276                 break;
6277         default:
6278                 ret = on_action_public_default(precv_frame, action);
6279                 break;
6280         }
6281
6282 exit:
6283         return ret;
6284 }
6285
6286 unsigned int OnAction_ht(_adapter *padapter, union recv_frame *precv_frame)
6287 {
6288         u8 *pframe = precv_frame->u.hdr.rx_data;
6289         uint frame_len = precv_frame->u.hdr.len;
6290         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6291         u8 category, action;
6292
6293         /* check RA matches or not */
6294         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
6295                 goto exit;
6296
6297         category = frame_body[0];
6298         if(category != RTW_WLAN_CATEGORY_HT)
6299                 goto exit;
6300
6301         action = frame_body[1];
6302         switch (action) {
6303         case RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING:
6304 #ifdef CONFIG_BEAMFORMING
6305                 //DBG_871X("RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING\n");
6306                 beamforming_get_report_frame(padapter, precv_frame);
6307 #endif //CONFIG_BEAMFORMING
6308                 break;
6309         default:
6310                 break;
6311         }
6312
6313 exit:
6314
6315         return _SUCCESS;
6316 }
6317
6318 #ifdef CONFIG_IEEE80211W
6319 unsigned int OnAction_sa_query(_adapter *padapter, union recv_frame *precv_frame)
6320 {
6321         u8 *pframe = precv_frame->u.hdr.rx_data;
6322         struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
6323         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6324         unsigned short tid;
6325         //Baron
6326
6327         DBG_871X("OnAction_sa_query\n");
6328
6329         switch (pframe[WLAN_HDR_A3_LEN+1])
6330         {
6331                 case 0: //SA Query req
6332                         _rtw_memcpy(&tid, &pframe[WLAN_HDR_A3_LEN+2], sizeof(unsigned short));
6333                         DBG_871X("OnAction_sa_query request,action=%d, tid=%04x\n", pframe[WLAN_HDR_A3_LEN+1], tid);
6334                         issue_action_SA_Query(padapter, GetAddr2Ptr(pframe), 1, tid);
6335                         break;
6336
6337                 case 1: //SA Query rsp
6338                         _cancel_timer_ex(&pmlmeext->sa_query_timer);
6339                         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]);
6340                         break;
6341                 default:
6342                         break;
6343         }
6344         if(0)
6345         {
6346                 int pp;
6347                 printk("pattrib->pktlen = %d =>", pattrib->pkt_len);
6348                 for(pp=0;pp< pattrib->pkt_len; pp++)
6349                         printk(" %02x ", pframe[pp]);
6350                 printk("\n");
6351         }
6352
6353         return _SUCCESS;
6354 }
6355 #endif //CONFIG_IEEE80211W
6356
6357 unsigned int OnAction_wmm(_adapter *padapter, union recv_frame *precv_frame)
6358 {
6359         return _SUCCESS;
6360 }
6361
6362 unsigned int OnAction_vht(_adapter *padapter, union recv_frame *precv_frame)
6363 {
6364 #ifdef CONFIG_80211AC_VHT
6365         struct rx_pkt_attrib *prxattrib = &precv_frame->u.hdr.attrib;
6366         u8 *pframe = precv_frame->u.hdr.rx_data;
6367         uint frame_len = precv_frame->u.hdr.len;
6368         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
6369         u8 category, action;
6370         struct sta_info *psta = NULL;
6371
6372         /* check RA matches or not */
6373         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
6374                 goto exit;
6375
6376         category = frame_body[0];
6377         if(category != RTW_WLAN_CATEGORY_VHT)
6378                 goto exit;
6379
6380         action = frame_body[1];
6381         switch (action) {
6382         case RTW_WLAN_ACTION_VHT_COMPRESSED_BEAMFORMING:
6383 #ifdef CONFIG_BEAMFORMING
6384                 //DBG_871X("RTW_WLAN_ACTION_VHT_COMPRESSED_BEAMFORMING\n");
6385                 beamforming_get_report_frame(padapter, precv_frame);
6386 #endif //CONFIG_BEAMFORMING
6387                 break;
6388         case RTW_WLAN_ACTION_VHT_OPMODE_NOTIFICATION:
6389                 // CategoryCode(1) + ActionCode(1) + OpModeNotification(1)
6390                 //DBG_871X("RTW_WLAN_ACTION_VHT_OPMODE_NOTIFICATION\n");
6391                 psta = rtw_get_stainfo(&padapter->stapriv, prxattrib->ta);
6392                 if (psta)
6393                         rtw_process_vht_op_mode_notify(padapter, &frame_body[2], psta);
6394                 break;
6395         default:
6396                 break;
6397         }
6398
6399 exit:
6400 #endif //CONFIG_80211AC_VHT
6401
6402         return _SUCCESS;
6403 }
6404
6405 unsigned int OnAction_p2p(_adapter *padapter, union recv_frame *precv_frame)
6406 {
6407 #ifdef CONFIG_P2P
6408         u8 *frame_body;
6409         u8 category, OUI_Subtype, dialogToken=0;
6410         u8 *pframe = precv_frame->u.hdr.rx_data;
6411         uint len = precv_frame->u.hdr.len;
6412         struct  wifidirect_info *pwdinfo = &( padapter->wdinfo );
6413
6414         //check RA matches or not
6415         if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))//for if1, sta/ap mode
6416                 return _SUCCESS;
6417
6418         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
6419
6420         category = frame_body[0];
6421         if(category != RTW_WLAN_CATEGORY_P2P)
6422                 return _SUCCESS;
6423
6424         if ( cpu_to_be32( *( ( u32* ) ( frame_body + 1 ) ) ) != P2POUI )
6425                 return _SUCCESS;
6426
6427 #ifdef CONFIG_IOCTL_CFG80211
6428         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
6429         {
6430                 rtw_cfg80211_rx_action_p2p(padapter, pframe, len);
6431                 return _SUCCESS;
6432         }
6433         else
6434 #endif //CONFIG_IOCTL_CFG80211
6435         {
6436                 len -= sizeof(struct rtw_ieee80211_hdr_3addr);
6437                 OUI_Subtype = frame_body[5];
6438                 dialogToken = frame_body[6];
6439
6440                 switch(OUI_Subtype)
6441                 {
6442                         case P2P_NOTICE_OF_ABSENCE:
6443
6444                                 break;
6445
6446                         case P2P_PRESENCE_REQUEST:
6447
6448                                 process_p2p_presence_req(pwdinfo, pframe, len);
6449
6450                                 break;
6451
6452                         case P2P_PRESENCE_RESPONSE:
6453
6454                                 break;
6455
6456                         case P2P_GO_DISC_REQUEST:
6457
6458                                 break;
6459
6460                         default:
6461                                 break;
6462
6463                 }
6464         }
6465 #endif //CONFIG_P2P
6466
6467         return _SUCCESS;
6468
6469 }
6470
6471 unsigned int OnAction(_adapter *padapter, union recv_frame *precv_frame)
6472 {
6473         int i;
6474         unsigned char   category;
6475         struct action_handler *ptable;
6476         unsigned char   *frame_body;
6477         u8 *pframe = precv_frame->u.hdr.rx_data;
6478
6479         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
6480
6481         category = frame_body[0];
6482
6483         for(i = 0; i < sizeof(OnAction_tbl)/sizeof(struct action_handler); i++)
6484         {
6485                 ptable = &OnAction_tbl[i];
6486
6487                 if(category == ptable->num)
6488                         ptable->func(padapter, precv_frame);
6489
6490         }
6491
6492         return _SUCCESS;
6493
6494 }
6495
6496 unsigned int DoReserved(_adapter *padapter, union recv_frame *precv_frame)
6497 {
6498
6499         //DBG_871X("rcvd mgt frame(%x, %x)\n", (GetFrameSubType(pframe) >> 4), *(unsigned int *)GetAddr1Ptr(pframe));
6500         return _SUCCESS;
6501 }
6502
6503 struct xmit_frame *_alloc_mgtxmitframe(struct xmit_priv *pxmitpriv, bool once)
6504 {
6505         struct xmit_frame *pmgntframe;
6506         struct xmit_buf *pxmitbuf;
6507
6508         if (once)
6509                 pmgntframe = rtw_alloc_xmitframe_once(pxmitpriv);
6510         else
6511                 pmgntframe = rtw_alloc_xmitframe_ext(pxmitpriv);
6512
6513         if (pmgntframe == NULL) {
6514                 DBG_871X(FUNC_ADPT_FMT" alloc xmitframe fail, once:%d\n", FUNC_ADPT_ARG(pxmitpriv->adapter), once);
6515                 goto exit;
6516         }
6517
6518         if ((pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv)) == NULL) {
6519                 DBG_871X(FUNC_ADPT_FMT" alloc xmitbuf fail\n", FUNC_ADPT_ARG(pxmitpriv->adapter));
6520                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
6521                 pmgntframe = NULL;
6522                 goto exit;
6523         }
6524
6525         pmgntframe->frame_tag = MGNT_FRAMETAG;
6526         pmgntframe->pxmitbuf = pxmitbuf;
6527         pmgntframe->buf_addr = pxmitbuf->pbuf;
6528         pxmitbuf->priv_data = pmgntframe;
6529
6530 exit:
6531         return pmgntframe;
6532
6533 }
6534
6535 inline struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
6536 {
6537         return _alloc_mgtxmitframe(pxmitpriv, _FALSE);
6538 }
6539
6540 inline struct xmit_frame *alloc_mgtxmitframe_once(struct xmit_priv *pxmitpriv)
6541 {
6542         return _alloc_mgtxmitframe(pxmitpriv, _TRUE);
6543 }
6544
6545
6546 /****************************************************************************
6547
6548 Following are some TX fuctions for WiFi MLME
6549
6550 *****************************************************************************/
6551
6552 void update_mgnt_tx_rate(_adapter *padapter, u8 rate)
6553 {
6554         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6555
6556         pmlmeext->tx_rate = rate;
6557         //DBG_871X("%s(): rate = %x\n",__FUNCTION__, rate);
6558 }
6559
6560 void update_mgntframe_attrib(_adapter *padapter, struct pkt_attrib *pattrib)
6561 {
6562         u8      wireless_mode;
6563         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6564         struct xmit_priv                *pxmitpriv = &padapter->xmitpriv;
6565         //_rtw_memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib));
6566
6567         pattrib->hdrlen = 24;
6568         pattrib->nr_frags = 1;
6569         pattrib->priority = 7;
6570         pattrib->mac_id = 0;
6571         pattrib->qsel = QSLT_MGNT;
6572
6573         pattrib->pktlen = 0;
6574
6575         if (pmlmeext->tx_rate == IEEE80211_CCK_RATE_1MB)
6576                 wireless_mode = WIRELESS_11B;
6577         else
6578                 wireless_mode = WIRELESS_11G;
6579         pattrib->raid =  rtw_get_mgntframe_raid(padapter, wireless_mode);
6580         pattrib->rate = pmlmeext->tx_rate;
6581
6582         pattrib->encrypt = _NO_PRIVACY_;
6583         pattrib->bswenc = _FALSE;
6584
6585         pattrib->qos_en = _FALSE;
6586         pattrib->ht_en = _FALSE;
6587         pattrib->bwmode = CHANNEL_WIDTH_20;
6588         pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
6589         pattrib->sgi = _FALSE;
6590
6591         pattrib->seqnum = pmlmeext->mgnt_seq;
6592
6593         pattrib->retry_ctrl = _TRUE;
6594
6595         pattrib->mbssid = 0;
6596         pattrib->hw_ssn_sel = pxmitpriv->hw_ssn_seq_no;
6597
6598 }
6599
6600 void update_mgntframe_attrib_addr(_adapter *padapter, struct xmit_frame *pmgntframe)
6601 {
6602         u8      *pframe;
6603         struct pkt_attrib       *pattrib = &pmgntframe->attrib;
6604
6605         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6606
6607         _rtw_memcpy(pattrib->ra, GetAddr1Ptr(pframe), ETH_ALEN);
6608         _rtw_memcpy(pattrib->ta, GetAddr2Ptr(pframe), ETH_ALEN);
6609 }
6610
6611 void dump_mgntframe(_adapter *padapter, struct xmit_frame *pmgntframe)
6612 {
6613         if(padapter->bSurpriseRemoved == _TRUE ||
6614                 padapter->bDriverStopped == _TRUE)
6615         {
6616                 rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
6617                 rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
6618                 return;
6619         }
6620
6621         rtw_hal_mgnt_xmit(padapter, pmgntframe);
6622 }
6623
6624 s32 dump_mgntframe_and_wait(_adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
6625 {
6626         s32 ret = _FAIL;
6627         _irqL irqL;
6628         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
6629         struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
6630         struct submit_ctx sctx;
6631
6632         if(padapter->bSurpriseRemoved == _TRUE ||
6633                 padapter->bDriverStopped == _TRUE)
6634         {
6635                 rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
6636                 rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
6637                 return ret;
6638         }
6639
6640         rtw_sctx_init(&sctx, timeout_ms);
6641         pxmitbuf->sctx = &sctx;
6642
6643         ret = rtw_hal_mgnt_xmit(padapter, pmgntframe);
6644
6645         if (ret == _SUCCESS)
6646                 ret = rtw_sctx_wait(&sctx, __func__);
6647
6648         _enter_critical(&pxmitpriv->lock_sctx, &irqL);
6649         pxmitbuf->sctx = NULL;
6650         _exit_critical(&pxmitpriv->lock_sctx, &irqL);
6651
6652          return ret;
6653 }
6654
6655 s32 dump_mgntframe_and_wait_ack(_adapter *padapter, struct xmit_frame *pmgntframe)
6656 {
6657 #ifdef CONFIG_XMIT_ACK
6658         static u8 seq_no = 0;
6659         s32 ret = _FAIL;
6660         u32 timeout_ms = 500;//  500ms
6661         struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
6662         #ifdef CONFIG_CONCURRENT_MODE
6663         if (padapter->pbuddy_adapter && !padapter->isprimary)
6664                 pxmitpriv = &(padapter->pbuddy_adapter->xmitpriv);
6665         #endif
6666
6667         if(padapter->bSurpriseRemoved == _TRUE ||
6668                 padapter->bDriverStopped == _TRUE)
6669         {
6670                 rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
6671                 rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
6672                 return -1;
6673         }
6674
6675         _enter_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
6676         pxmitpriv->ack_tx = _TRUE;
6677         pxmitpriv->seq_no = seq_no++;
6678         pmgntframe->ack_report = 1;
6679         if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS) {
6680                 ret = rtw_ack_tx_wait(pxmitpriv, timeout_ms);
6681         }
6682
6683         pxmitpriv->ack_tx = _FALSE;
6684         _exit_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
6685
6686          return ret;
6687 #else //!CONFIG_XMIT_ACK
6688         dump_mgntframe(padapter, pmgntframe);
6689         rtw_msleep_os(50);
6690         return _SUCCESS;
6691 #endif //!CONFIG_XMIT_ACK
6692 }
6693
6694 int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
6695 {
6696         u8 *ssid_ie;
6697         sint ssid_len_ori;
6698         int len_diff = 0;
6699
6700         ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
6701
6702         //DBG_871X("%s hidden_ssid_mode:%u, ssid_ie:%p, ssid_len_ori:%d\n", __FUNCTION__, hidden_ssid_mode, ssid_ie, ssid_len_ori);
6703
6704         if(ssid_ie && ssid_len_ori>0)
6705         {
6706                 switch(hidden_ssid_mode)
6707                 {
6708                         case 1:
6709                         {
6710                                 u8 *next_ie = ssid_ie + 2 + ssid_len_ori;
6711                                 u32 remain_len = 0;
6712
6713                                 remain_len = ies_len -(next_ie-ies);
6714
6715                                 ssid_ie[1] = 0;
6716                                 _rtw_memcpy(ssid_ie+2, next_ie, remain_len);
6717                                 len_diff -= ssid_len_ori;
6718
6719                                 break;
6720                         }
6721                         case 2:
6722                                 _rtw_memset(&ssid_ie[2], 0, ssid_len_ori);
6723                                 break;
6724                         default:
6725                                 break;
6726                 }
6727         }
6728
6729         return len_diff;
6730 }
6731
6732 void issue_beacon(_adapter *padapter, int timeout_ms)
6733 {
6734         struct xmit_frame       *pmgntframe;
6735         struct pkt_attrib       *pattrib;
6736         unsigned char   *pframe;
6737         struct rtw_ieee80211_hdr *pwlanhdr;
6738         unsigned short *fctrl;
6739         unsigned int    rate_len;
6740         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
6741 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6742         _irqL irqL;
6743         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
6744 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6745         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6746         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6747         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
6748         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
6749 #ifdef CONFIG_P2P
6750         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
6751 #endif //CONFIG_P2P
6752
6753
6754         //DBG_871X("%s\n", __FUNCTION__);
6755
6756 #ifdef CONFIG_BCN_ICF
6757         if ((pmgntframe = rtw_alloc_bcnxmitframe(pxmitpriv)) == NULL)
6758 #else
6759         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
6760 #endif
6761         {
6762                 DBG_871X("%s, alloc mgnt frame fail\n", __FUNCTION__);
6763                 return;
6764         }
6765 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6766         _enter_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
6767 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6768
6769         //update attribute
6770         pattrib = &pmgntframe->attrib;
6771         update_mgntframe_attrib(padapter, pattrib);
6772         pattrib->qsel = QSLT_BEACON;
6773         #ifdef CONFIG_CONCURRENT_MODE
6774         if(padapter->iface_type == IFACE_PORT1)
6775                 pattrib->mbssid = 1;
6776         #endif
6777
6778         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
6779
6780         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6781         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
6782
6783
6784         fctrl = &(pwlanhdr->frame_ctl);
6785         *(fctrl) = 0;
6786
6787         _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
6788         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
6789         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
6790
6791         SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
6792         //pmlmeext->mgnt_seq++;
6793         SetFrameSubType(pframe, WIFI_BEACON);
6794
6795         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
6796         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
6797
6798         if( (pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
6799         {
6800                 //DBG_871X("ie len=%d\n", cur_network->IELength);
6801 #ifdef CONFIG_P2P
6802                 // for P2P : Primary Device Type & Device Name
6803                 u32 wpsielen=0, insert_len=0;
6804                 u8 *wpsie=NULL;
6805                 wpsie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wpsielen);
6806
6807                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wpsie && wpsielen>0)
6808                 {
6809                         uint wps_offset, remainder_ielen;
6810                         u8 *premainder_ie, *pframe_wscie;
6811
6812                         wps_offset = (uint)(wpsie - cur_network->IEs);
6813
6814                         premainder_ie = wpsie + wpsielen;
6815
6816                         remainder_ielen = cur_network->IELength - wps_offset - wpsielen;
6817
6818 #ifdef CONFIG_IOCTL_CFG80211
6819                         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
6820                         {
6821                                 if(pmlmepriv->wps_beacon_ie && pmlmepriv->wps_beacon_ie_len>0)
6822                                 {
6823                                         _rtw_memcpy(pframe, cur_network->IEs, wps_offset);
6824                                         pframe += wps_offset;
6825                                         pattrib->pktlen += wps_offset;
6826
6827                                         _rtw_memcpy(pframe, pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len);
6828                                         pframe += pmlmepriv->wps_beacon_ie_len;
6829                                         pattrib->pktlen += pmlmepriv->wps_beacon_ie_len;
6830
6831                                         //copy remainder_ie to pframe
6832                                         _rtw_memcpy(pframe, premainder_ie, remainder_ielen);
6833                                         pframe += remainder_ielen;
6834                                         pattrib->pktlen += remainder_ielen;
6835                                 }
6836                                 else
6837                                 {
6838                                         _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
6839                                         pframe += cur_network->IELength;
6840                                         pattrib->pktlen += cur_network->IELength;
6841                                 }
6842                         }
6843                         else
6844 #endif //CONFIG_IOCTL_CFG80211
6845                         {
6846                                 pframe_wscie = pframe + wps_offset;
6847                                 _rtw_memcpy(pframe, cur_network->IEs, wps_offset+wpsielen);
6848                                 pframe += (wps_offset + wpsielen);
6849                                 pattrib->pktlen += (wps_offset + wpsielen);
6850
6851                                 //now pframe is end of wsc ie, insert Primary Device Type & Device Name
6852                                 //      Primary Device Type
6853                                 //      Type:
6854                                 *(u16*) ( pframe + insert_len) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
6855                                 insert_len += 2;
6856
6857                                 //      Length:
6858                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( 0x0008 );
6859                                 insert_len += 2;
6860
6861                                 //      Value:
6862                                 //      Category ID
6863                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
6864                                 insert_len += 2;
6865
6866                                 //      OUI
6867                                 *(u32*) ( pframe + insert_len ) = cpu_to_be32( WPSOUI );
6868                                 insert_len += 4;
6869
6870                                 //      Sub Category ID
6871                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
6872                                 insert_len += 2;
6873
6874
6875                                 //      Device Name
6876                                 //      Type:
6877                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
6878                                 insert_len += 2;
6879
6880                                 //      Length:
6881                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( pwdinfo->device_name_len );
6882                                 insert_len += 2;
6883
6884                                 //      Value:
6885                                 _rtw_memcpy( pframe + insert_len, pwdinfo->device_name, pwdinfo->device_name_len );
6886                                 insert_len += pwdinfo->device_name_len;
6887
6888
6889                                 //update wsc ie length
6890                                 *(pframe_wscie+1) = (wpsielen -2) + insert_len;
6891
6892                                 //pframe move to end
6893                                 pframe+=insert_len;
6894                                 pattrib->pktlen += insert_len;
6895
6896                                 //copy remainder_ie to pframe
6897                                 _rtw_memcpy(pframe, premainder_ie, remainder_ielen);
6898                                 pframe += remainder_ielen;
6899                                 pattrib->pktlen += remainder_ielen;
6900                         }
6901                 }
6902                 else
6903 #endif //CONFIG_P2P
6904                 {
6905                         int len_diff;
6906                         _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
6907                         len_diff = update_hidden_ssid(
6908                                 pframe+_BEACON_IE_OFFSET_
6909                                 , cur_network->IELength-_BEACON_IE_OFFSET_
6910                                 , pmlmeinfo->hidden_ssid_mode
6911                         );
6912                         pframe += (cur_network->IELength+len_diff);
6913                         pattrib->pktlen += (cur_network->IELength+len_diff);
6914                 }
6915
6916                 {
6917                         u8 *wps_ie;
6918                         uint wps_ielen;
6919                         u8 sr = 0;
6920                         wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr+TXDESC_OFFSET+sizeof (struct rtw_ieee80211_hdr_3addr)+_BEACON_IE_OFFSET_,
6921                                 pattrib->pktlen-sizeof (struct rtw_ieee80211_hdr_3addr)-_BEACON_IE_OFFSET_, NULL, &wps_ielen);
6922                         if (wps_ie && wps_ielen>0) {
6923                                 rtw_get_wps_attr_content(wps_ie,  wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8*)(&sr), NULL);
6924                         }
6925                         if (sr != 0)
6926                                 set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
6927                         else
6928                                 _clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
6929                 }
6930
6931 #ifdef CONFIG_P2P
6932                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
6933                 {
6934                         u32 len;
6935 #ifdef CONFIG_IOCTL_CFG80211
6936                         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
6937                         {
6938                                 len = pmlmepriv->p2p_beacon_ie_len;
6939                                 if(pmlmepriv->p2p_beacon_ie && len>0)
6940                                         _rtw_memcpy(pframe, pmlmepriv->p2p_beacon_ie, len);
6941                         }
6942                         else
6943 #endif //CONFIG_IOCTL_CFG80211
6944                         {
6945                                 len = build_beacon_p2p_ie(pwdinfo, pframe);
6946                         }
6947
6948                         pframe += len;
6949                         pattrib->pktlen += len;
6950 #ifdef CONFIG_WFD
6951 #ifdef CONFIG_IOCTL_CFG80211
6952                         if(_TRUE == pwdinfo->wfd_info->wfd_enable)
6953 #endif //CONFIG_IOCTL_CFG80211
6954                         {
6955                         len = build_beacon_wfd_ie( pwdinfo, pframe );
6956                         }
6957 #ifdef CONFIG_IOCTL_CFG80211
6958                         else
6959                         {
6960                                 len = 0;
6961                                 if(pmlmepriv->wfd_beacon_ie && pmlmepriv->wfd_beacon_ie_len>0)
6962                                 {
6963                                         len = pmlmepriv->wfd_beacon_ie_len;
6964                                         _rtw_memcpy(pframe, pmlmepriv->wfd_beacon_ie, len);
6965                                 }
6966                         }
6967 #endif //CONFIG_IOCTL_CFG80211
6968                         pframe += len;
6969                         pattrib->pktlen += len;
6970 #endif //CONFIG_WFD
6971                 }
6972 #endif //CONFIG_P2P
6973
6974                 goto _issue_bcn;
6975
6976         }
6977
6978         //below for ad-hoc mode
6979
6980         //timestamp will be inserted by hardware
6981         pframe += 8;
6982         pattrib->pktlen += 8;
6983
6984         // beacon interval: 2 bytes
6985
6986         _rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
6987
6988         pframe += 2;
6989         pattrib->pktlen += 2;
6990
6991         // capability info: 2 bytes
6992
6993         _rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
6994
6995         pframe += 2;
6996         pattrib->pktlen += 2;
6997
6998         // SSID
6999         pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
7000
7001         // supported rates...
7002         rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
7003         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8)? 8: rate_len), cur_network->SupportedRates, &pattrib->pktlen);
7004
7005         // DS parameter set
7006         pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
7007
7008         //if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
7009         {
7010                 u8 erpinfo=0;
7011                 u32 ATIMWindow;
7012                 // IBSS Parameter Set...
7013                 //ATIMWindow = cur->Configuration.ATIMWindow;
7014                 ATIMWindow = 0;
7015                 pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
7016
7017                 //ERP IE
7018                 pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
7019         }
7020
7021
7022         // EXTERNDED SUPPORTED RATE
7023         if (rate_len > 8)
7024         {
7025                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
7026         }
7027
7028
7029         //todo:HT for adhoc
7030
7031 _issue_bcn:
7032
7033 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7034         pmlmepriv->update_bcn = _FALSE;
7035
7036         _exit_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
7037 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7038
7039         if ((pattrib->pktlen + TXDESC_SIZE) > 512)
7040         {
7041                 DBG_871X("beacon frame too large\n");
7042                 return;
7043         }
7044
7045         pattrib->last_txcmdsz = pattrib->pktlen;
7046
7047         //DBG_871X("issue bcn_sz=%d\n", pattrib->last_txcmdsz);
7048         if(timeout_ms > 0)
7049                 dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
7050         else
7051                 dump_mgntframe(padapter, pmgntframe);
7052
7053 }
7054
7055 void issue_probersp(_adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
7056 {
7057         struct xmit_frame                       *pmgntframe;
7058         struct pkt_attrib                       *pattrib;
7059         unsigned char                                   *pframe;
7060         struct rtw_ieee80211_hdr        *pwlanhdr;
7061         unsigned short                          *fctrl;
7062         unsigned char                                   *mac, *bssid;
7063         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
7064 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7065         u8 *pwps_ie;
7066         uint wps_ielen;
7067         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
7068 #endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7069         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7070         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7071         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
7072         unsigned int    rate_len;
7073 #ifdef CONFIG_P2P
7074         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
7075 #ifdef CONFIG_WFD
7076         u32                                     wfdielen = 0;
7077 #endif //CONFIG_WFD
7078 #endif //CONFIG_P2P
7079
7080         //DBG_871X("%s\n", __FUNCTION__);
7081
7082         if(da == NULL)
7083                 return;
7084
7085         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7086         {
7087                 DBG_871X("%s, alloc mgnt frame fail\n", __FUNCTION__);
7088                 return;
7089         }
7090
7091
7092         //update attribute
7093         pattrib = &pmgntframe->attrib;
7094         update_mgntframe_attrib(padapter, pattrib);
7095
7096         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7097
7098         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7099         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7100
7101         mac = myid(&(padapter->eeprompriv));
7102         bssid = cur_network->MacAddress;
7103
7104         fctrl = &(pwlanhdr->frame_ctl);
7105         *(fctrl) = 0;
7106         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
7107         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
7108         _rtw_memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
7109
7110         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7111         pmlmeext->mgnt_seq++;
7112         SetFrameSubType(fctrl, WIFI_PROBERSP);
7113
7114         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7115         pattrib->pktlen = pattrib->hdrlen;
7116         pframe += pattrib->hdrlen;
7117
7118
7119         if(cur_network->IELength>MAX_IE_SZ)
7120                 return;
7121
7122 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
7123         if( (pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
7124         {
7125                 pwps_ie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
7126
7127                 //inerset & update wps_probe_resp_ie
7128                 if((pmlmepriv->wps_probe_resp_ie!=NULL) && pwps_ie && (wps_ielen>0))
7129                 {
7130                         uint wps_offset, remainder_ielen;
7131                         u8 *premainder_ie;
7132
7133                         wps_offset = (uint)(pwps_ie - cur_network->IEs);
7134
7135                         premainder_ie = pwps_ie + wps_ielen;
7136
7137                         remainder_ielen = cur_network->IELength - wps_offset - wps_ielen;
7138
7139                         _rtw_memcpy(pframe, cur_network->IEs, wps_offset);
7140                         pframe += wps_offset;
7141                         pattrib->pktlen += wps_offset;
7142
7143                         wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];//to get ie data len
7144                         if((wps_offset+wps_ielen+2)<=MAX_IE_SZ)
7145                         {
7146                                 _rtw_memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen+2);
7147                                 pframe += wps_ielen+2;
7148                                 pattrib->pktlen += wps_ielen+2;
7149                         }
7150
7151                         if((wps_offset+wps_ielen+2+remainder_ielen)<=MAX_IE_SZ)
7152                         {
7153                                 _rtw_memcpy(pframe, premainder_ie, remainder_ielen);
7154                                 pframe += remainder_ielen;
7155                                 pattrib->pktlen += remainder_ielen;
7156                         }
7157                 }
7158                 else
7159                 {
7160                         _rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
7161                         pframe += cur_network->IELength;
7162                         pattrib->pktlen += cur_network->IELength;
7163                 }
7164
7165                 /* retrieve SSID IE from cur_network->Ssid */
7166                 {
7167                         u8 *ssid_ie;
7168                         sint ssid_ielen;
7169                         sint ssid_ielen_diff;
7170                         u8 buf[MAX_IE_SZ];
7171                         u8 *ies = pmgntframe->buf_addr+TXDESC_OFFSET+sizeof(struct rtw_ieee80211_hdr_3addr);
7172
7173                         ssid_ie = rtw_get_ie(ies+_FIXED_IE_LENGTH_, _SSID_IE_, &ssid_ielen,
7174                                 (pframe-ies)-_FIXED_IE_LENGTH_);
7175
7176                         ssid_ielen_diff = cur_network->Ssid.SsidLength - ssid_ielen;
7177
7178                         if (ssid_ie &&  cur_network->Ssid.SsidLength) {
7179                                 uint remainder_ielen;
7180                                 u8 *remainder_ie;
7181                                 remainder_ie = ssid_ie+2;
7182                                 remainder_ielen = (pframe-remainder_ie);
7183
7184                                 if (remainder_ielen > MAX_IE_SZ) {
7185                                         DBG_871X_LEVEL(_drv_warning_, FUNC_ADPT_FMT" remainder_ielen > MAX_IE_SZ\n", FUNC_ADPT_ARG(padapter));
7186                                         remainder_ielen = MAX_IE_SZ;
7187                                 }
7188
7189                                 _rtw_memcpy(buf, remainder_ie, remainder_ielen);
7190                                 _rtw_memcpy(remainder_ie+ssid_ielen_diff, buf, remainder_ielen);
7191                                 *(ssid_ie+1) = cur_network->Ssid.SsidLength;
7192                                 _rtw_memcpy(ssid_ie+2, cur_network->Ssid.Ssid, cur_network->Ssid.SsidLength);
7193
7194                                 pframe += ssid_ielen_diff;
7195                                 pattrib->pktlen += ssid_ielen_diff;
7196                         }
7197                 }
7198         }
7199         else
7200 #endif
7201         {
7202
7203                 //timestamp will be inserted by hardware
7204                 pframe += 8;
7205                 pattrib->pktlen += 8;
7206
7207                 // beacon interval: 2 bytes
7208
7209                 _rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
7210
7211                 pframe += 2;
7212                 pattrib->pktlen += 2;
7213
7214                 // capability info: 2 bytes
7215
7216                 _rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
7217
7218                 pframe += 2;
7219                 pattrib->pktlen += 2;
7220
7221                 //below for ad-hoc mode
7222
7223                 // SSID
7224                 pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
7225
7226                 // supported rates...
7227                 rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
7228                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8)? 8: rate_len), cur_network->SupportedRates, &pattrib->pktlen);
7229
7230                 // DS parameter set
7231                 pframe =rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
7232
7233                 if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
7234                 {
7235                         u8 erpinfo=0;
7236                         u32 ATIMWindow;
7237                         // IBSS Parameter Set...
7238                         //ATIMWindow = cur->Configuration.ATIMWindow;
7239                         ATIMWindow = 0;
7240                         pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
7241
7242                         //ERP IE
7243                         pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
7244                 }
7245
7246
7247                 // EXTERNDED SUPPORTED RATE
7248                 if (rate_len > 8)
7249                 {
7250                         pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
7251                 }
7252
7253
7254                 //todo:HT for adhoc
7255
7256         }
7257
7258 #ifdef CONFIG_P2P
7259         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)
7260                 /* IOT issue, When wifi_spec is not set, send probe_resp with P2P IE even if probe_req has no P2P IE */
7261                 && (is_valid_p2p_probereq || !padapter->registrypriv.wifi_spec))
7262         {
7263                 u32 len;
7264 #ifdef CONFIG_IOCTL_CFG80211
7265                 if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
7266                 {
7267                         //if pwdinfo->role == P2P_ROLE_DEVICE will call issue_probersp_p2p()
7268                         len = pmlmepriv->p2p_go_probe_resp_ie_len;
7269                         if(pmlmepriv->p2p_go_probe_resp_ie && len>0)
7270                                 _rtw_memcpy(pframe, pmlmepriv->p2p_go_probe_resp_ie, len);
7271                 }
7272                 else
7273 #endif //CONFIG_IOCTL_CFG80211
7274                 {
7275                         len = build_probe_resp_p2p_ie(pwdinfo, pframe);
7276                 }
7277
7278                 pframe += len;
7279                 pattrib->pktlen += len;
7280
7281 #ifdef CONFIG_WFD
7282 #ifdef CONFIG_IOCTL_CFG80211
7283                 if(_TRUE == pwdinfo->wfd_info->wfd_enable)
7284 #endif //CONFIG_IOCTL_CFG80211
7285                 {
7286                         len = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
7287                 }
7288 #ifdef CONFIG_IOCTL_CFG80211
7289                 else
7290                 {
7291                         len = 0;
7292                         if(pmlmepriv->wfd_probe_resp_ie && pmlmepriv->wfd_probe_resp_ie_len>0)
7293                         {
7294                                 len = pmlmepriv->wfd_probe_resp_ie_len;
7295                                 _rtw_memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, len);
7296                         }
7297                 }
7298 #endif //CONFIG_IOCTL_CFG80211
7299                 pframe += len;
7300                 pattrib->pktlen += len;
7301 #endif //CONFIG_WFD
7302
7303         }
7304 #endif //CONFIG_P2P
7305
7306
7307 #ifdef CONFIG_AUTO_AP_MODE
7308 {
7309         struct sta_info *psta;
7310         struct sta_priv *pstapriv = &padapter->stapriv;
7311
7312         DBG_871X("(%s)\n", __FUNCTION__);
7313
7314         //check rc station
7315         psta = rtw_get_stainfo(pstapriv, da);
7316         if (psta && psta->isrc && psta->pid>0)
7317         {
7318                 u8 RC_OUI[4]={0x00,0xE0,0x4C,0x0A};
7319                 u8 RC_INFO[14] = {0};
7320                 //EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2]
7321                 u16 cu_ch = (u16)cur_network->Configuration.DSConfig;
7322
7323                 DBG_871X("%s, reply rc(pid=0x%x) device "MAC_FMT" in ch=%d\n", __FUNCTION__,
7324                         psta->pid, MAC_ARG(psta->hwaddr), cu_ch);
7325
7326                 //append vendor specific ie
7327                 _rtw_memcpy(RC_INFO, RC_OUI, sizeof(RC_OUI));
7328                 _rtw_memcpy(&RC_INFO[4], mac, ETH_ALEN);
7329                 _rtw_memcpy(&RC_INFO[10], (u8*)&psta->pid, 2);
7330                 _rtw_memcpy(&RC_INFO[12], (u8*)&cu_ch, 2);
7331
7332                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, sizeof(RC_INFO), RC_INFO, &pattrib->pktlen);
7333         }
7334 }
7335 #endif //CONFIG_AUTO_AP_MODE
7336
7337
7338         pattrib->last_txcmdsz = pattrib->pktlen;
7339
7340
7341         dump_mgntframe(padapter, pmgntframe);
7342
7343         return;
7344
7345 }
7346
7347 int _issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da, u8 ch, bool append_wps, int wait_ack)
7348 {
7349         int ret = _FAIL;
7350         struct xmit_frame               *pmgntframe;
7351         struct pkt_attrib               *pattrib;
7352         unsigned char                   *pframe;
7353         struct rtw_ieee80211_hdr        *pwlanhdr;
7354         unsigned short          *fctrl;
7355         unsigned char                   *mac;
7356         unsigned char                   bssrate[NumRates];
7357         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
7358         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
7359         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7360         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7361         int     bssrate_len = 0;
7362         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
7363
7364         RT_TRACE(_module_rtl871x_mlme_c_,_drv_notice_,("+issue_probereq\n"));
7365
7366         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7367         {
7368                 goto exit;
7369         }
7370
7371         //update attribute
7372         pattrib = &pmgntframe->attrib;
7373         update_mgntframe_attrib(padapter, pattrib);
7374
7375
7376         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7377
7378         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7379         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7380
7381         mac = myid(&(padapter->eeprompriv));
7382
7383         fctrl = &(pwlanhdr->frame_ctl);
7384         *(fctrl) = 0;
7385
7386         if (da)
7387         {
7388                 //      unicast probe request frame
7389                 _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
7390                 _rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN);
7391         }
7392         else
7393         {
7394                 //      broadcast probe request frame
7395                 _rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
7396                 _rtw_memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
7397         }
7398
7399         _rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
7400
7401         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7402         pmlmeext->mgnt_seq++;
7403         SetFrameSubType(pframe, WIFI_PROBEREQ);
7404
7405         pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
7406         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
7407
7408         if(pssid)
7409                 pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &(pattrib->pktlen));
7410         else
7411                 pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &(pattrib->pktlen));
7412
7413         get_rate_set(padapter, bssrate, &bssrate_len);
7414
7415         if (bssrate_len > 8)
7416         {
7417                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
7418                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
7419         }
7420         else
7421         {
7422                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
7423         }
7424
7425         if (ch)
7426                 pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, &ch, &pattrib->pktlen);
7427
7428         if (append_wps) {
7429                 //add wps_ie for wps2.0
7430                 if(pmlmepriv->wps_probe_req_ie_len>0 && pmlmepriv->wps_probe_req_ie)
7431                 {
7432                         _rtw_memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
7433                         pframe += pmlmepriv->wps_probe_req_ie_len;
7434                         pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
7435                         //pmlmepriv->wps_probe_req_ie_len = 0 ;//reset to zero
7436                 }
7437         }
7438
7439         pattrib->last_txcmdsz = pattrib->pktlen;
7440
7441         RT_TRACE(_module_rtl871x_mlme_c_,_drv_notice_,("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
7442
7443         if (wait_ack) {
7444                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
7445         } else {
7446                 dump_mgntframe(padapter, pmgntframe);
7447                 ret = _SUCCESS;
7448         }
7449
7450 exit:
7451         return ret;
7452 }
7453
7454 inline void issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da)
7455 {
7456         _issue_probereq(padapter, pssid, da, 0, 1, _FALSE);
7457 }
7458
7459 int issue_probereq_ex(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da, u8 ch, bool append_wps,
7460         int try_cnt, int wait_ms)
7461 {
7462         int ret;
7463         int i = 0;
7464         u32 start = rtw_get_current_time();
7465
7466         do
7467         {
7468                 ret = _issue_probereq(padapter, pssid, da, ch, append_wps, wait_ms>0?_TRUE:_FALSE);
7469
7470                 i++;
7471
7472                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
7473                         break;
7474
7475                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
7476                         rtw_msleep_os(wait_ms);
7477
7478         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
7479
7480         if (ret != _FAIL) {
7481                 ret = _SUCCESS;
7482                 #ifndef DBG_XMIT_ACK
7483                 goto exit;
7484                 #endif
7485         }
7486
7487         if (try_cnt && wait_ms) {
7488                 if (da)
7489                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
7490                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
7491                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7492                 else
7493                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
7494                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
7495                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7496         }
7497 exit:
7498         return ret;
7499 }
7500
7501 // if psta == NULL, indiate we are station(client) now...
7502 void issue_auth(_adapter *padapter, struct sta_info *psta, unsigned short status)
7503 {
7504         struct xmit_frame                       *pmgntframe;
7505         struct pkt_attrib                       *pattrib;
7506         unsigned char                                   *pframe;
7507         struct rtw_ieee80211_hdr        *pwlanhdr;
7508         unsigned short                          *fctrl;
7509         unsigned int                                    val32;
7510         unsigned short                          val16;
7511         int use_shared_key = 0;
7512         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
7513         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7514         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7515
7516         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7517         {
7518                 return;
7519         }
7520
7521         //update attribute
7522         pattrib = &pmgntframe->attrib;
7523         update_mgntframe_attrib(padapter, pattrib);
7524
7525         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7526
7527         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7528         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7529
7530         fctrl = &(pwlanhdr->frame_ctl);
7531         *(fctrl) = 0;
7532
7533         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7534         pmlmeext->mgnt_seq++;
7535         SetFrameSubType(pframe, WIFI_AUTH);
7536
7537         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
7538         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7539
7540
7541         if(psta)// for AP mode
7542         {
7543 #ifdef CONFIG_NATIVEAP_MLME
7544
7545                 _rtw_memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);
7546                 _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
7547                 _rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
7548
7549
7550                 // setting auth algo number
7551                 val16 = (u16)psta->authalg;
7552
7553                 if(status != _STATS_SUCCESSFUL_)
7554                         val16 = 0;
7555
7556                 if (val16)      {
7557                         val16 = cpu_to_le16(val16);
7558                         use_shared_key = 1;
7559                 }
7560
7561                 pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7562
7563                 // setting auth seq number
7564                 val16 =(u16)psta->auth_seq;
7565                 val16 = cpu_to_le16(val16);
7566                 pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7567
7568                 // setting status code...
7569                 val16 = status;
7570                 val16 = cpu_to_le16(val16);
7571                 pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen));
7572
7573                 // added challenging text...
7574                 if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
7575                 {
7576                         pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, psta->chg_txt, &(pattrib->pktlen));
7577                 }
7578 #endif
7579         }
7580         else
7581         {
7582                 _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
7583                 _rtw_memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
7584                 _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
7585
7586                 // setting auth algo number
7587                 val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)? 1: 0;// 0:OPEN System, 1:Shared key
7588                 if (val16)      {
7589                         val16 = cpu_to_le16(val16);
7590                         use_shared_key = 1;
7591                 }
7592                 //DBG_871X("%s auth_algo= %s auth_seq=%d\n",__FUNCTION__,(pmlmeinfo->auth_algo==0)?"OPEN":"SHARED",pmlmeinfo->auth_seq);
7593
7594                 //setting IV for auth seq #3
7595                 if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
7596                 {
7597                         //DBG_871X("==> iv(%d),key_index(%d)\n",pmlmeinfo->iv,pmlmeinfo->key_index);
7598                         val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
7599                         val32 = cpu_to_le32(val32);
7600                         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&val32, &(pattrib->pktlen));
7601
7602                         pattrib->iv_len = 4;
7603                 }
7604
7605                 pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7606
7607                 // setting auth seq number
7608                 val16 = pmlmeinfo->auth_seq;
7609                 val16 = cpu_to_le16(val16);
7610                 pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
7611
7612
7613                 // setting status code...
7614                 val16 = status;
7615                 val16 = cpu_to_le16(val16);
7616                 pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen));
7617
7618                 // then checking to see if sending challenging text...
7619                 if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
7620                 {
7621                         pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen));
7622
7623                         SetPrivacy(fctrl);
7624
7625                         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7626
7627                         pattrib->encrypt = _WEP40_;
7628
7629                         pattrib->icv_len = 4;
7630
7631                         pattrib->pktlen += pattrib->icv_len;
7632
7633                 }
7634
7635         }
7636
7637         pattrib->last_txcmdsz = pattrib->pktlen;
7638
7639         rtw_wep_encrypt(padapter, (u8 *)pmgntframe);
7640         DBG_871X("%s\n", __FUNCTION__);
7641         dump_mgntframe(padapter, pmgntframe);
7642
7643         return;
7644 }
7645
7646
7647 void issue_asocrsp(_adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
7648 {
7649 #ifdef CONFIG_AP_MODE
7650         struct xmit_frame       *pmgntframe;
7651         struct rtw_ieee80211_hdr        *pwlanhdr;
7652         struct pkt_attrib *pattrib;
7653         unsigned char   *pbuf, *pframe;
7654         unsigned short val;
7655         unsigned short *fctrl;
7656         struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
7657         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
7658         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
7659         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7660         WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);
7661         u8 *ie = pnetwork->IEs;
7662 #ifdef CONFIG_P2P
7663         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
7664 #ifdef CONFIG_WFD
7665         u32                                     wfdielen = 0;
7666 #endif //CONFIG_WFD
7667
7668 #endif //CONFIG_P2P
7669
7670         DBG_871X("%s\n", __FUNCTION__);
7671
7672         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7673         {
7674                 return;
7675         }
7676
7677         //update attribute
7678         pattrib = &pmgntframe->attrib;
7679         update_mgntframe_attrib(padapter, pattrib);
7680
7681
7682         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7683
7684         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7685         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7686
7687         fctrl = &(pwlanhdr->frame_ctl);
7688         *(fctrl) = 0;
7689
7690         _rtw_memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
7691         _rtw_memcpy((void *)GetAddr2Ptr(pwlanhdr), myid(&(padapter->eeprompriv)), ETH_ALEN);
7692         _rtw_memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7693
7694
7695         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7696         pmlmeext->mgnt_seq++;
7697         if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
7698                 SetFrameSubType(pwlanhdr, pkt_type);
7699         else
7700                 return;
7701
7702         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7703         pattrib->pktlen += pattrib->hdrlen;
7704         pframe += pattrib->hdrlen;
7705
7706         //capability
7707         val = *(unsigned short *)rtw_get_capability_from_ie(ie);
7708
7709         pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_ , (unsigned char *)&val, &(pattrib->pktlen));
7710
7711         status = cpu_to_le16(status);
7712         pframe = rtw_set_fixed_ie(pframe , _STATUS_CODE_ , (unsigned char *)&status, &(pattrib->pktlen));
7713
7714         val = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
7715         pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_ , (unsigned char *)&val, &(pattrib->pktlen));
7716
7717         if (pstat->bssratelen <= 8)
7718         {
7719                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen));
7720         }
7721         else
7722         {
7723                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &(pattrib->pktlen));
7724                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (pstat->bssratelen-8), pstat->bssrateset+8, &(pattrib->pktlen));
7725         }
7726
7727 #ifdef CONFIG_80211N_HT
7728         if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option))
7729         {
7730                 uint ie_len=0;
7731
7732                 //FILL HT CAP INFO IE
7733                 //p = hostapd_eid_ht_capabilities_info(hapd, p);
7734                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
7735                 if(pbuf && ie_len>0)
7736                 {
7737                         _rtw_memcpy(pframe, pbuf, ie_len+2);
7738                         pframe += (ie_len+2);
7739                         pattrib->pktlen +=(ie_len+2);
7740                 }
7741
7742                 //FILL HT ADD INFO IE
7743                 //p = hostapd_eid_ht_operation(hapd, p);
7744                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
7745                 if(pbuf && ie_len>0)
7746                 {
7747                         _rtw_memcpy(pframe, pbuf, ie_len+2);
7748                         pframe += (ie_len+2);
7749                         pattrib->pktlen +=(ie_len+2);
7750                 }
7751
7752         }
7753 #endif
7754
7755 #ifdef CONFIG_80211AC_VHT
7756         if ((pstat->flags & WLAN_STA_VHT) && (pmlmepriv->vhtpriv.vht_option))
7757         {
7758                 u32 ie_len=0;
7759
7760                 //FILL VHT CAP IE
7761                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, EID_VHTCapability, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
7762                 if(pbuf && ie_len>0)
7763                 {
7764                         _rtw_memcpy(pframe, pbuf, ie_len+2);
7765                         pframe += (ie_len+2);
7766                         pattrib->pktlen +=(ie_len+2);
7767                 }
7768
7769                 //FILL VHT OPERATION IE
7770                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, EID_VHTOperation, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
7771                 if(pbuf && ie_len>0)
7772                 {
7773                         _rtw_memcpy(pframe, pbuf, ie_len+2);
7774                         pframe += (ie_len+2);
7775                         pattrib->pktlen +=(ie_len+2);
7776                 }
7777         }
7778 #endif //CONFIG_80211AC_VHT
7779
7780         //FILL WMM IE
7781         if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option))
7782         {
7783                 uint ie_len=0;
7784                 unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
7785
7786                 for (pbuf = ie + _BEACON_IE_OFFSET_; ;pbuf+= (ie_len + 2))
7787                 {
7788                         pbuf = rtw_get_ie(pbuf, _VENDOR_SPECIFIC_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
7789                         if(pbuf && _rtw_memcmp(pbuf+2, WMM_PARA_IE, 6))
7790                         {
7791                                 _rtw_memcpy(pframe, pbuf, ie_len+2);
7792                                 pframe += (ie_len+2);
7793                                 pattrib->pktlen +=(ie_len+2);
7794
7795                                 break;
7796                         }
7797
7798                         if ((pbuf == NULL) || (ie_len == 0))
7799                         {
7800                                 break;
7801                         }
7802                 }
7803
7804         }
7805
7806
7807         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
7808         {
7809                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
7810         }
7811
7812         //add WPS IE ie for wps 2.0
7813         if(pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len>0)
7814         {
7815                 _rtw_memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len);
7816
7817                 pframe += pmlmepriv->wps_assoc_resp_ie_len;
7818                 pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
7819         }
7820
7821 #ifdef CONFIG_P2P
7822         if( padapter->wdinfo.driver_interface == DRIVER_WEXT )
7823         {
7824                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && (pstat->is_p2p_device == _TRUE))
7825                 {
7826                         u32 len;
7827
7828                         len = build_assoc_resp_p2p_ie(pwdinfo, pframe, pstat->p2p_status_code);
7829
7830                         pframe += len;
7831                         pattrib->pktlen += len;
7832                 }
7833         }
7834 #ifdef CONFIG_WFD
7835         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)
7836 #ifdef CONFIG_IOCTL_CFG80211
7837                 && (_TRUE == pwdinfo->wfd_info->wfd_enable)
7838 #endif //CONFIG_IOCTL_CFG80211
7839         )
7840         {
7841                 wfdielen = build_assoc_resp_wfd_ie(pwdinfo, pframe);
7842                 pframe += wfdielen;
7843                 pattrib->pktlen += wfdielen;
7844         }
7845 #endif //CONFIG_WFD
7846 #endif //CONFIG_P2P
7847
7848         pattrib->last_txcmdsz = pattrib->pktlen;
7849
7850         dump_mgntframe(padapter, pmgntframe);
7851
7852 #endif
7853 }
7854
7855 void issue_assocreq(_adapter *padapter)
7856 {
7857         int ret = _FAIL;
7858         struct xmit_frame                               *pmgntframe;
7859         struct pkt_attrib                               *pattrib;
7860         unsigned char                                   *pframe, *p;
7861         struct rtw_ieee80211_hdr                        *pwlanhdr;
7862         unsigned short                          *fctrl;
7863         unsigned short                          val16;
7864         unsigned int                                    i, j, ie_len, index=0;
7865         unsigned char                                   rf_type, bssrate[NumRates], sta_bssrate[NumRates];
7866         PNDIS_802_11_VARIABLE_IEs       pIE;
7867         struct registry_priv    *pregpriv = &padapter->registrypriv;
7868         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
7869         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
7870         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7871         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7872         int     bssrate_len = 0, sta_bssrate_len = 0;
7873         u8      vs_ie_length = 0;
7874 #ifdef CONFIG_P2P
7875         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
7876         u8                                      p2pie[ 255 ] = { 0x00 };
7877         u16                                     p2pielen = 0;
7878 #ifdef CONFIG_WFD
7879         u32                                     wfdielen = 0;
7880 #endif //CONFIG_WFD
7881 #endif //CONFIG_P2P
7882
7883 #ifdef CONFIG_DFS
7884         u16     cap;
7885
7886         /* Dot H */
7887         u8 pow_cap_ele[2] = { 0x00 };
7888         u8 sup_ch[ 30 * 2 ] = {0x00 }, sup_ch_idx = 0, idx_5g = 2;      //For supported channel
7889 #endif //CONFIG_DFS
7890
7891         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7892                 goto exit;
7893
7894         //update attribute
7895         pattrib = &pmgntframe->attrib;
7896         update_mgntframe_attrib(padapter, pattrib);
7897
7898
7899         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7900
7901         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7902         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7903
7904         fctrl = &(pwlanhdr->frame_ctl);
7905         *(fctrl) = 0;
7906         _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7907         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
7908         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7909
7910         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7911         pmlmeext->mgnt_seq++;
7912         SetFrameSubType(pframe, WIFI_ASSOCREQ);
7913
7914         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
7915         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7916
7917         //caps
7918
7919 #ifdef CONFIG_DFS
7920         _rtw_memcpy(&cap, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
7921         cap |= cap_SpecMgmt;
7922         _rtw_memcpy(pframe, &cap, 2);
7923 #else
7924         _rtw_memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
7925 #endif //CONFIG_DFS
7926
7927         pframe += 2;
7928         pattrib->pktlen += 2;
7929
7930         //listen interval
7931         //todo: listen interval for power saving
7932         val16 = cpu_to_le16(3);
7933         _rtw_memcpy(pframe ,(unsigned char *)&val16, 2);
7934         pframe += 2;
7935         pattrib->pktlen += 2;
7936
7937         //SSID
7938         pframe = rtw_set_ie(pframe, _SSID_IE_,  pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &(pattrib->pktlen));
7939
7940 #ifdef CONFIG_DFS
7941         /* Dot H */
7942         if(pmlmeext->cur_channel > 14)
7943         {
7944                 pow_cap_ele[0] = 13;    // Minimum transmit power capability
7945                 pow_cap_ele[1] = 21;    // Maximum transmit power capability
7946                 pframe = rtw_set_ie(pframe, EID_PowerCap, 2, pow_cap_ele, &(pattrib->pktlen));
7947
7948                 //supported channels
7949                 do{
7950                         if( pmlmeext->channel_set[sup_ch_idx].ChannelNum <= 14 )
7951                         {
7952                                 sup_ch[0] = 1;  //First channel number
7953                                 sup_ch[1] = pmlmeext->channel_set[sup_ch_idx].ChannelNum;       //Number of channel
7954                         }
7955                         else
7956                         {
7957                                 sup_ch[idx_5g++] = pmlmeext->channel_set[sup_ch_idx].ChannelNum;
7958                                 sup_ch[idx_5g++] = 1;
7959                         }
7960                         sup_ch_idx++;
7961                 }
7962                 while( pmlmeext->channel_set[sup_ch_idx].ChannelNum != 0 );
7963                 pframe = rtw_set_ie(pframe, EID_SupportedChannels, idx_5g, sup_ch, &(pattrib->pktlen));
7964         }
7965 #endif //CONFIG_DFS
7966
7967         //supported rate & extended supported rate
7968
7969 #if 1   // Check if the AP's supported rates are also supported by STA.
7970         get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);
7971         //DBG_871X("sta_bssrate_len=%d\n", sta_bssrate_len);
7972
7973         if(pmlmeext->cur_channel == 14)// for JAPAN, channel 14 can only uses B Mode(CCK)
7974         {
7975                 sta_bssrate_len = 4;
7976         }
7977
7978
7979         //for (i = 0; i < sta_bssrate_len; i++) {
7980         //      DBG_871X("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]);
7981         //}
7982
7983         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
7984                 if (pmlmeinfo->network.SupportedRates[i] == 0) break;
7985                 DBG_871X("network.SupportedRates[%d]=%02X\n", i, pmlmeinfo->network.SupportedRates[i]);
7986         }
7987
7988
7989         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
7990                 if (pmlmeinfo->network.SupportedRates[i] == 0) break;
7991
7992
7993                 // Check if the AP's supported rates are also supported by STA.
7994                 for (j=0; j < sta_bssrate_len; j++) {
7995                          // Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP
7996                         if ( (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK)
7997                                         == (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)) {
7998                                 //DBG_871X("match i = %d, j=%d\n", i, j);
7999                                 break;
8000                         } else {
8001                                 //DBG_871X("not match: %02X != %02X\n", (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK), (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK));
8002                         }
8003                 }
8004
8005                 if (j == sta_bssrate_len) {
8006                         // the rate is not supported by STA
8007                         DBG_871X("%s(): the rate[%d]=%02X is not supported by STA!\n",__FUNCTION__, i, pmlmeinfo->network.SupportedRates[i]);
8008                 } else {
8009                         // the rate is supported by STA
8010                         bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
8011                 }
8012         }
8013
8014         bssrate_len = index;
8015         DBG_871X("bssrate_len = %d\n", bssrate_len);
8016
8017 #else   // Check if the AP's supported rates are also supported by STA.
8018 #if 0
8019         get_rate_set(padapter, bssrate, &bssrate_len);
8020 #else
8021         for (bssrate_len = 0; bssrate_len < NumRates; bssrate_len++) {
8022                 if (pmlmeinfo->network.SupportedRates[bssrate_len] == 0) break;
8023
8024                 if (pmlmeinfo->network.SupportedRates[bssrate_len] == 0x2C) // Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP
8025                         break;
8026
8027                 bssrate[bssrate_len] = pmlmeinfo->network.SupportedRates[bssrate_len];
8028         }
8029 #endif
8030 #endif  // Check if the AP's supported rates are also supported by STA.
8031
8032         if ((bssrate_len == 0) && (pmlmeinfo->network.SupportedRates[0] != 0)) {
8033                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
8034                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
8035                 goto exit; //don't connect to AP if no joint supported rate
8036         }
8037
8038
8039         if (bssrate_len > 8) {
8040                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
8041                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
8042         } else if (bssrate_len > 0) {
8043                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
8044         } else {
8045                 DBG_871X("%s: Connect to AP without 11b and 11g data rate!\n", __FUNCTION__);
8046         }
8047
8048         //vendor specific IE, such as WPA, WMM, WPS
8049         for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;)
8050         {
8051                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i);
8052
8053                 switch (pIE->ElementID)
8054                 {
8055                         case _VENDOR_SPECIFIC_IE_:
8056                                 if ((_rtw_memcmp(pIE->data, RTW_WPA_OUI, 4)) ||
8057                                                 (_rtw_memcmp(pIE->data, WMM_OUI, 4)) ||
8058                                                 (_rtw_memcmp(pIE->data, WPS_OUI, 4)))
8059                                 {
8060                                         vs_ie_length = pIE->Length;
8061                                         if((!padapter->registrypriv.wifi_spec) && (_rtw_memcmp(pIE->data, WPS_OUI, 4)))
8062                                         {
8063                                                 //Commented by Kurt 20110629
8064                                                 //In some older APs, WPS handshake
8065                                                 //would be fail if we append vender extensions informations to AP
8066
8067                                                 vs_ie_length = 14;
8068                                         }
8069
8070                                         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, vs_ie_length, pIE->data, &(pattrib->pktlen));
8071                                 }
8072                                 break;
8073
8074                         case EID_WPA2:
8075                                 pframe = rtw_set_ie(pframe, EID_WPA2, pIE->Length, pIE->data, &(pattrib->pktlen));
8076                                 break;
8077 #ifdef CONFIG_80211N_HT
8078                         case EID_HTCapability:
8079                                 if(padapter->mlmepriv.htpriv.ht_option==_TRUE) {
8080                                         if (!(is_ap_in_tkip(padapter)))
8081                                         {
8082                                                 _rtw_memcpy(&(pmlmeinfo->HT_caps), pIE->data, sizeof(struct HT_caps_element));
8083
8084                                                 pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = cpu_to_le16(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info);
8085
8086                                                 pframe = rtw_set_ie(pframe, EID_HTCapability, pIE->Length , (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
8087                                         }
8088                                 }
8089                                 break;
8090
8091                         case EID_EXTCapability:
8092                                 if(padapter->mlmepriv.htpriv.ht_option==_TRUE) {
8093                                         pframe = rtw_set_ie(pframe, EID_EXTCapability, pIE->Length, pIE->data, &(pattrib->pktlen));
8094                                 }
8095                                 break;
8096 #endif //CONFIG_80211N_HT
8097 #ifdef CONFIG_80211AC_VHT
8098                         case EID_VHTCapability:
8099                                 if (padapter->mlmepriv.vhtpriv.vht_option ==_TRUE) {
8100                                         pframe = rtw_set_ie(pframe, EID_VHTCapability, pIE->Length, pIE->data, &(pattrib->pktlen));
8101                                 }
8102                                 break;
8103
8104                         case EID_OpModeNotification:
8105                                 if (padapter->mlmepriv.vhtpriv.vht_option ==_TRUE) {
8106                                         pframe = rtw_set_ie(pframe, EID_OpModeNotification, pIE->Length, pIE->data, &(pattrib->pktlen));
8107                                 }
8108                                 break;
8109 #endif // CONFIG_80211AC_VHT
8110                         default:
8111                                 break;
8112                 }
8113
8114                 i += (pIE->Length + 2);
8115         }
8116
8117         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
8118         {
8119                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
8120         }
8121
8122
8123 #ifdef CONFIG_WAPI_SUPPORT
8124         rtw_build_assoc_req_wapi_ie(padapter, pframe, pattrib);
8125 #endif
8126
8127
8128 #ifdef CONFIG_P2P
8129
8130 #ifdef CONFIG_IOCTL_CFG80211
8131         if(adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211 )
8132         {
8133                 if(pmlmepriv->p2p_assoc_req_ie && pmlmepriv->p2p_assoc_req_ie_len>0)
8134                 {
8135                         _rtw_memcpy(pframe, pmlmepriv->p2p_assoc_req_ie, pmlmepriv->p2p_assoc_req_ie_len);
8136                         pframe += pmlmepriv->p2p_assoc_req_ie_len;
8137                         pattrib->pktlen += pmlmepriv->p2p_assoc_req_ie_len;
8138                 }
8139         }
8140         else
8141 #endif //CONFIG_IOCTL_CFG80211
8142         {
8143                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
8144                 {
8145                         //      Should add the P2P IE in the association request frame.
8146                         //      P2P OUI
8147
8148                         p2pielen = 0;
8149                         p2pie[ p2pielen++ ] = 0x50;
8150                         p2pie[ p2pielen++ ] = 0x6F;
8151                         p2pie[ p2pielen++ ] = 0x9A;
8152                         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
8153
8154                         //      Commented by Albert 20101109
8155                         //      According to the P2P Specification, the association request frame should contain 3 P2P attributes
8156                         //      1. P2P Capability
8157                         //      2. Extended Listen Timing
8158                         //      3. Device Info
8159                         //      Commented by Albert 20110516
8160                         //      4. P2P Interface
8161
8162                         //      P2P Capability
8163                         //      Type:
8164                         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
8165
8166                         //      Length:
8167                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
8168                         p2pielen += 2;
8169
8170                         //      Value:
8171                         //      Device Capability Bitmap, 1 byte
8172                         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
8173
8174                         //      Group Capability Bitmap, 1 byte
8175                         if ( pwdinfo->persistent_supported )
8176                                 p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
8177                         else
8178                                 p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
8179
8180                         //      Extended Listen Timing
8181                         //      Type:
8182                         p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
8183
8184                         //      Length:
8185                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
8186                         p2pielen += 2;
8187
8188                         //      Value:
8189                         //      Availability Period
8190                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
8191                         p2pielen += 2;
8192
8193                         //      Availability Interval
8194                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
8195                         p2pielen += 2;
8196
8197                         //      Device Info
8198                         //      Type:
8199                         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
8200
8201                         //      Length:
8202                         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
8203                         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
8204                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
8205                         p2pielen += 2;
8206
8207                         //      Value:
8208                         //      P2P Device Address
8209                         _rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
8210                         p2pielen += ETH_ALEN;
8211
8212                         //      Config Method
8213                         //      This field should be big endian. Noted by P2P specification.
8214                         if ( ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN ) ||
8215                                 ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN ) )
8216                         {
8217                                 *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
8218                         }
8219                         else
8220                         {
8221                                 *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_PBC );
8222                         }
8223
8224                         p2pielen += 2;
8225
8226                         //      Primary Device Type
8227                         //      Category ID
8228                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
8229                         p2pielen += 2;
8230
8231                         //      OUI
8232                         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
8233                         p2pielen += 4;
8234
8235                         //      Sub Category ID
8236                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
8237                         p2pielen += 2;
8238
8239                         //      Number of Secondary Device Types
8240                         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
8241
8242                         //      Device Name
8243                         //      Type:
8244                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
8245                         p2pielen += 2;
8246
8247                         //      Length:
8248                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
8249                         p2pielen += 2;
8250
8251                         //      Value:
8252                         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
8253                         p2pielen += pwdinfo->device_name_len;
8254
8255                         //      P2P Interface
8256                         //      Type:
8257                         p2pie[ p2pielen++ ] = P2P_ATTR_INTERFACE;
8258
8259                         //      Length:
8260                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x000D );
8261                         p2pielen += 2;
8262
8263                         //      Value:
8264                         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );        //      P2P Device Address
8265                         p2pielen += ETH_ALEN;
8266
8267                         p2pie[ p2pielen++ ] = 1;        //      P2P Interface Address Count
8268
8269                         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );        //      P2P Interface Address List
8270                         p2pielen += ETH_ALEN;
8271
8272                         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
8273
8274 #ifdef CONFIG_WFD
8275                         //wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);
8276                         //pframe += wfdielen;
8277                         //pattrib->pktlen += wfdielen;
8278 #endif //CONFIG_WFD
8279                 }
8280         }
8281
8282 #endif //CONFIG_P2P
8283
8284 #ifdef CONFIG_WFD
8285 #ifdef CONFIG_IOCTL_CFG80211
8286         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
8287 #endif //CONFIG_IOCTL_CFG80211
8288         {
8289                 wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);
8290                 pframe += wfdielen;
8291                 pattrib->pktlen += wfdielen;
8292         }
8293 #ifdef CONFIG_IOCTL_CFG80211
8294         else if (pmlmepriv->wfd_assoc_req_ie != NULL && pmlmepriv->wfd_assoc_req_ie_len>0)
8295         {
8296                 //WFD IE
8297                 _rtw_memcpy(pframe, pmlmepriv->wfd_assoc_req_ie, pmlmepriv->wfd_assoc_req_ie_len);
8298                 pattrib->pktlen += pmlmepriv->wfd_assoc_req_ie_len;
8299                 pframe += pmlmepriv->wfd_assoc_req_ie_len;
8300         }
8301 #endif //CONFIG_IOCTL_CFG80211
8302 #endif //CONFIG_WFD
8303
8304         pattrib->last_txcmdsz = pattrib->pktlen;
8305         dump_mgntframe(padapter, pmgntframe);
8306
8307         ret = _SUCCESS;
8308
8309 exit:
8310         if (ret == _SUCCESS)
8311                 rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen);
8312         else
8313                 rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
8314
8315         return;
8316 }
8317
8318 //when wait_ack is ture, this function shoule be called at process context
8319 static int _issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
8320 {
8321         int ret = _FAIL;
8322         struct xmit_frame                       *pmgntframe;
8323         struct pkt_attrib                       *pattrib;
8324         unsigned char                                   *pframe;
8325         struct rtw_ieee80211_hdr        *pwlanhdr;
8326         unsigned short                          *fctrl;
8327         struct xmit_priv        *pxmitpriv;
8328         struct mlme_ext_priv    *pmlmeext;
8329         struct mlme_ext_info    *pmlmeinfo;
8330
8331         //DBG_871X("%s:%d\n", __FUNCTION__, power_mode);
8332
8333         if(!padapter)
8334                 goto exit;
8335
8336         pxmitpriv = &(padapter->xmitpriv);
8337         pmlmeext = &(padapter->mlmeextpriv);
8338         pmlmeinfo = &(pmlmeext->mlmext_info);
8339
8340         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8341         {
8342                 goto exit;
8343         }
8344
8345         //update attribute
8346         pattrib = &pmgntframe->attrib;
8347         update_mgntframe_attrib(padapter, pattrib);
8348         pattrib->retry_ctrl = _FALSE;
8349
8350         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8351
8352         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8353         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8354
8355         fctrl = &(pwlanhdr->frame_ctl);
8356         *(fctrl) = 0;
8357
8358         if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
8359         {
8360                 SetFrDs(fctrl);
8361         }
8362         else if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
8363         {
8364                 SetToDs(fctrl);
8365         }
8366
8367         if (power_mode)
8368         {
8369                 SetPwrMgt(fctrl);
8370         }
8371
8372         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
8373         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8374         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8375
8376         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8377         pmlmeext->mgnt_seq++;
8378         SetFrameSubType(pframe, WIFI_DATA_NULL);
8379
8380         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8381         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8382
8383         pattrib->last_txcmdsz = pattrib->pktlen;
8384
8385         if(wait_ack)
8386         {
8387                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
8388         }
8389         else
8390         {
8391                 dump_mgntframe(padapter, pmgntframe);
8392                 ret = _SUCCESS;
8393         }
8394
8395 exit:
8396         return ret;
8397 }
8398
8399 /*
8400  * [IMPORTANT] Don't call this function in interrupt context
8401  *
8402  * When wait_ms > 0, this function shoule be called at process context
8403  * da == NULL for station mode
8404  */
8405 int issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
8406 {
8407         int ret;
8408         int i = 0;
8409         u32 start = rtw_get_current_time();
8410         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8411         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8412         struct sta_info *psta;
8413
8414
8415         /* da == NULL, assum it's null data for sta to ap*/
8416         if (da == NULL)
8417                 da = get_my_bssid(&(pmlmeinfo->network));
8418
8419         psta = rtw_get_stainfo(&padapter->stapriv, da);
8420         if (psta) {
8421                 if (power_mode)
8422                         rtw_hal_macid_sleep(padapter, psta->mac_id);
8423                 else
8424                         rtw_hal_macid_wakeup(padapter, psta->mac_id);
8425         } else {
8426                 DBG_871X(FUNC_ADPT_FMT ": Can't find sta info for " MAC_FMT ", skip macid %s!!\n",
8427                         FUNC_ADPT_ARG(padapter), MAC_ARG(da), power_mode?"sleep":"wakeup");
8428                 rtw_warn_on(1);
8429         }
8430
8431         do {
8432                 ret = _issue_nulldata(padapter, da, power_mode, wait_ms>0?_TRUE:_FALSE);
8433
8434                 i++;
8435
8436                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
8437                         break;
8438
8439                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
8440                         rtw_msleep_os(wait_ms);
8441
8442         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
8443
8444         if (ret != _FAIL) {
8445                 ret = _SUCCESS;
8446                 #ifndef DBG_XMIT_ACK
8447                 goto exit;
8448                 #endif
8449         }
8450
8451         if (try_cnt && wait_ms) {
8452                 if (da)
8453                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
8454                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
8455                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8456                 else
8457                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
8458                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
8459                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8460         }
8461 exit:
8462         return ret;
8463 }
8464
8465 /*
8466  * [IMPORTANT] This function run in interrupt context
8467  *
8468  * The null data packet would be sent without power bit,
8469  * and not guarantee success.
8470  */
8471 s32 issue_nulldata_in_interrupt(PADAPTER padapter, u8 *da, unsigned int power_mode)
8472 {
8473         int ret;
8474         struct mlme_ext_priv *pmlmeext;
8475         struct mlme_ext_info *pmlmeinfo;
8476
8477
8478         pmlmeext = &padapter->mlmeextpriv;
8479         pmlmeinfo = &pmlmeext->mlmext_info;
8480
8481         /* da == NULL, assum it's null data for sta to ap*/
8482         if (da == NULL)
8483                 da = get_my_bssid(&(pmlmeinfo->network));
8484
8485         ret = _issue_nulldata(padapter, da, power_mode, _FALSE);
8486
8487         return ret;
8488 }
8489
8490 //when wait_ack is ture, this function shoule be called at process context
8491 static int _issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int wait_ack)
8492 {
8493         int ret = _FAIL;
8494         struct xmit_frame                       *pmgntframe;
8495         struct pkt_attrib                       *pattrib;
8496         unsigned char                                   *pframe;
8497         struct rtw_ieee80211_hdr        *pwlanhdr;
8498         unsigned short                          *fctrl, *qc;
8499         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
8500         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8501         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8502
8503         DBG_871X("%s\n", __FUNCTION__);
8504
8505         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8506         {
8507                 goto exit;
8508         }
8509
8510         //update attribute
8511         pattrib = &pmgntframe->attrib;
8512         update_mgntframe_attrib(padapter, pattrib);
8513
8514         pattrib->hdrlen +=2;
8515         pattrib->qos_en = _TRUE;
8516         pattrib->eosp = 1;
8517         pattrib->ack_policy = 0;
8518         pattrib->mdata = 0;
8519
8520         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8521
8522         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8523         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8524
8525         fctrl = &(pwlanhdr->frame_ctl);
8526         *(fctrl) = 0;
8527
8528         if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
8529         {
8530                 SetFrDs(fctrl);
8531         }
8532         else if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
8533         {
8534                 SetToDs(fctrl);
8535         }
8536
8537         if(pattrib->mdata)
8538                 SetMData(fctrl);
8539
8540         qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
8541
8542         SetPriority(qc, tid);
8543
8544         SetEOSP(qc, pattrib->eosp);
8545
8546         SetAckpolicy(qc, pattrib->ack_policy);
8547
8548         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
8549         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8550         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8551
8552         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8553         pmlmeext->mgnt_seq++;
8554         SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
8555
8556         pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos);
8557         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos);
8558
8559         pattrib->last_txcmdsz = pattrib->pktlen;
8560
8561         if(wait_ack)
8562         {
8563                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
8564         }
8565         else
8566         {
8567                 dump_mgntframe(padapter, pmgntframe);
8568                 ret = _SUCCESS;
8569         }
8570
8571 exit:
8572         return ret;
8573 }
8574
8575 //when wait_ms >0 , this function shoule be called at process context
8576 //da == NULL for station mode
8577 int issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms)
8578 {
8579         int ret;
8580         int i = 0;
8581         u32 start = rtw_get_current_time();
8582         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8583         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8584
8585         /* da == NULL, assum it's null data for sta to ap*/
8586         if (da == NULL)
8587                 da = get_my_bssid(&(pmlmeinfo->network));
8588
8589         do
8590         {
8591                 ret = _issue_qos_nulldata(padapter, da, tid, wait_ms>0?_TRUE:_FALSE);
8592
8593                 i++;
8594
8595                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
8596                         break;
8597
8598                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
8599                         rtw_msleep_os(wait_ms);
8600
8601         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
8602
8603         if (ret != _FAIL) {
8604                 ret = _SUCCESS;
8605                 #ifndef DBG_XMIT_ACK
8606                 goto exit;
8607                 #endif
8608         }
8609
8610         if (try_cnt && wait_ms) {
8611                 if (da)
8612                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
8613                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
8614                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8615                 else
8616                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
8617                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
8618                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8619         }
8620 exit:
8621         return ret;
8622 }
8623
8624 static int _issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack)
8625 {
8626         struct xmit_frame                       *pmgntframe;
8627         struct pkt_attrib                       *pattrib;
8628         unsigned char                                   *pframe;
8629         struct rtw_ieee80211_hdr        *pwlanhdr;
8630         unsigned short                          *fctrl;
8631         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
8632         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8633         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8634         int ret = _FAIL;
8635 #ifdef CONFIG_P2P
8636         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
8637 #endif //CONFIG_P2P
8638
8639         //DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
8640
8641 #ifdef CONFIG_P2P
8642         if ( !( rtw_p2p_chk_state( pwdinfo, P2P_STATE_NONE ) ) && ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) )
8643         {
8644                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
8645                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
8646         }
8647 #endif //CONFIG_P2P
8648
8649         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8650         {
8651                 goto exit;
8652         }
8653
8654         //update attribute
8655         pattrib = &pmgntframe->attrib;
8656         update_mgntframe_attrib(padapter, pattrib);
8657         pattrib->retry_ctrl = _FALSE;
8658
8659         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8660
8661         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8662         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8663
8664         fctrl = &(pwlanhdr->frame_ctl);
8665         *(fctrl) = 0;
8666
8667         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
8668         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8669         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8670
8671         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8672         pmlmeext->mgnt_seq++;
8673         SetFrameSubType(pframe, WIFI_DEAUTH);
8674
8675         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8676         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8677
8678         reason = cpu_to_le16(reason);
8679         pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_ , (unsigned char *)&reason, &(pattrib->pktlen));
8680
8681         pattrib->last_txcmdsz = pattrib->pktlen;
8682
8683
8684         if(wait_ack)
8685         {
8686                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
8687         }
8688         else
8689         {
8690                 dump_mgntframe(padapter, pmgntframe);
8691                 ret = _SUCCESS;
8692         }
8693
8694 exit:
8695         return ret;
8696 }
8697
8698 int issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason)
8699 {
8700         DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
8701         return _issue_deauth(padapter, da, reason, _FALSE);
8702 }
8703
8704 int issue_deauth_ex(_adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
8705         int wait_ms)
8706 {
8707         int ret;
8708         int i = 0;
8709         u32 start = rtw_get_current_time();
8710
8711         do
8712         {
8713                 ret = _issue_deauth(padapter, da, reason, wait_ms>0?_TRUE:_FALSE);
8714
8715                 i++;
8716
8717                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
8718                         break;
8719
8720                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
8721                         rtw_msleep_os(wait_ms);
8722
8723         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
8724
8725         if (ret != _FAIL) {
8726                 ret = _SUCCESS;
8727                 #ifndef DBG_XMIT_ACK
8728                 goto exit;
8729                 #endif
8730         }
8731
8732         if (try_cnt && wait_ms) {
8733                 if (da)
8734                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
8735                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
8736                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8737                 else
8738                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
8739                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
8740                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
8741         }
8742 exit:
8743         return ret;
8744 }
8745
8746 void issue_action_spct_ch_switch(_adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset)
8747 {
8748         _irqL   irqL;
8749         _list           *plist, *phead;
8750         struct xmit_frame                       *pmgntframe;
8751         struct pkt_attrib                       *pattrib;
8752         unsigned char                           *pframe;
8753         struct rtw_ieee80211_hdr        *pwlanhdr;
8754         unsigned short                  *fctrl;
8755         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
8756         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
8757         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8758         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8759
8760
8761         DBG_871X(FUNC_NDEV_FMT" ra="MAC_FMT", ch:%u, offset:%u\n",
8762                 FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(ra), new_ch, ch_offset);
8763
8764         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8765                 return;
8766
8767         //update attribute
8768         pattrib = &pmgntframe->attrib;
8769         update_mgntframe_attrib(padapter, pattrib);
8770
8771         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8772
8773         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8774         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8775
8776         fctrl = &(pwlanhdr->frame_ctl);
8777         *(fctrl) = 0;
8778
8779         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN); /* RA */
8780         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); /* TA */
8781         _rtw_memcpy(pwlanhdr->addr3, ra, ETH_ALEN); /* DA = RA */
8782
8783         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8784         pmlmeext->mgnt_seq++;
8785         SetFrameSubType(pframe, WIFI_ACTION);
8786
8787         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8788         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8789
8790         /* category, action */
8791         {
8792                 u8 category, action;
8793                 category = RTW_WLAN_CATEGORY_SPECTRUM_MGMT;
8794                 action = RTW_WLAN_ACTION_SPCT_CHL_SWITCH;
8795
8796                 pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
8797                 pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
8798         }
8799
8800         pframe = rtw_set_ie_ch_switch(pframe, &(pattrib->pktlen), 0, new_ch, 0);
8801         pframe = rtw_set_ie_secondary_ch_offset(pframe, &(pattrib->pktlen),
8802                 hal_ch_offset_to_secondary_ch_offset(ch_offset));
8803
8804         pattrib->last_txcmdsz = pattrib->pktlen;
8805
8806         dump_mgntframe(padapter, pmgntframe);
8807
8808 }
8809
8810 #ifdef CONFIG_IEEE80211W
8811 void issue_action_SA_Query(_adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short tid)
8812 {
8813         u8      category = RTW_WLAN_CATEGORY_SA_QUERY;
8814         u16     reason_code;
8815         struct xmit_frame               *pmgntframe;
8816         struct pkt_attrib               *pattrib;
8817         u8                                      *pframe;
8818         struct rtw_ieee80211_hdr        *pwlanhdr;
8819         u16                                     *fctrl;
8820         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
8821         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8822         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8823         struct sta_info         *psta;
8824         struct sta_priv         *pstapriv = &padapter->stapriv;
8825         struct registry_priv            *pregpriv = &padapter->registrypriv;
8826
8827
8828         DBG_871X("%s\n", __FUNCTION__);
8829
8830         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8831         {
8832                 DBG_871X("%s: alloc_mgtxmitframe fail\n", __FUNCTION__);
8833                 return;
8834         }
8835
8836         //update attribute
8837         pattrib = &pmgntframe->attrib;
8838         update_mgntframe_attrib(padapter, pattrib);
8839
8840         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8841
8842         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8843         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8844
8845         fctrl = &(pwlanhdr->frame_ctl);
8846         *(fctrl) = 0;
8847
8848         if(raddr)
8849                 _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
8850         else
8851                 _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8852         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8853         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8854
8855         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8856         pmlmeext->mgnt_seq++;
8857         SetFrameSubType(pframe, WIFI_ACTION);
8858
8859         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8860         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8861
8862         pframe = rtw_set_fixed_ie(pframe, 1, &category, &pattrib->pktlen);
8863         pframe = rtw_set_fixed_ie(pframe, 1, &action, &pattrib->pktlen);
8864
8865         switch (action)
8866         {
8867                 case 0: //SA Query req
8868                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&pmlmeext->sa_query_seq, &pattrib->pktlen);
8869                         pmlmeext->sa_query_seq++;
8870                         //send sa query request to AP, AP should reply sa query response in 1 second
8871                         set_sa_query_timer(pmlmeext, 1000);
8872                         break;
8873
8874                 case 1: //SA Query rsp
8875                         tid = cpu_to_le16(tid);
8876                         pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&tid, &pattrib->pktlen);
8877                         break;
8878                 default:
8879                         break;
8880         }
8881
8882         pattrib->last_txcmdsz = pattrib->pktlen;
8883
8884         dump_mgntframe(padapter, pmgntframe);
8885 }
8886 #endif //CONFIG_IEEE80211W
8887
8888 void issue_action_BA(_adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short status)
8889 {
8890         u8      category = RTW_WLAN_CATEGORY_BACK;
8891         u16     start_seq;
8892         u16     BA_para_set;
8893         u16     reason_code;
8894         u16     BA_timeout_value;
8895         u16     BA_starting_seqctrl;
8896         HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor;
8897         u8 ba_rxbuf_sz;
8898         struct xmit_frame               *pmgntframe;
8899         struct pkt_attrib               *pattrib;
8900         u8                                      *pframe;
8901         struct rtw_ieee80211_hdr        *pwlanhdr;
8902         u16                                     *fctrl;
8903         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
8904         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8905         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8906         struct sta_info         *psta;
8907         struct sta_priv         *pstapriv = &padapter->stapriv;
8908         struct registry_priv            *pregpriv = &padapter->registrypriv;
8909
8910 #ifdef CONFIG_80211N_HT
8911
8912         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8913         {
8914                 return;
8915         }
8916
8917         //update attribute
8918         pattrib = &pmgntframe->attrib;
8919         update_mgntframe_attrib(padapter, pattrib);
8920
8921         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8922
8923         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8924         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8925
8926         fctrl = &(pwlanhdr->frame_ctl);
8927         *(fctrl) = 0;
8928
8929         //_rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8930         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
8931         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8932         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8933
8934         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8935         pmlmeext->mgnt_seq++;
8936         SetFrameSubType(pframe, WIFI_ACTION);
8937
8938         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8939         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8940
8941         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
8942         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
8943
8944       status = cpu_to_le16(status);
8945
8946
8947         if (category == 3)
8948         {
8949                 switch (action)
8950                 {
8951                         case 0: //ADDBA req
8952                                 do {
8953                                         pmlmeinfo->dialogToken++;
8954                                 } while (pmlmeinfo->dialogToken == 0);
8955                                 pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen));
8956
8957                                 #if defined(CONFIG_RTL8188E) && defined(CONFIG_SDIO_HCI)
8958                                 BA_para_set = (0x0802 | ((status & 0xf) << 2)); //immediate ack & 16 buffer size
8959                                 #else
8960                                 BA_para_set = (0x1002 | ((status & 0xf) << 2)); //immediate ack & 64 buffer size
8961                                 #endif
8962
8963                                 BA_para_set = cpu_to_le16(BA_para_set);
8964                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
8965
8966                                 //BA_timeout_value = 0xffff;//max: 65535 TUs(~ 65 ms)
8967                                 BA_timeout_value = 5000;//~ 5ms
8968                                 BA_timeout_value = cpu_to_le16(BA_timeout_value);
8969                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_timeout_value)), &(pattrib->pktlen));
8970
8971                                 //if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
8972                                 if ((psta = rtw_get_stainfo(pstapriv, raddr)) != NULL)
8973                                 {
8974                                         start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07]&0xfff) + 1;
8975
8976                                         DBG_871X("BA_starting_seqctrl = %d for TID=%d\n", start_seq, status & 0x07);
8977
8978                                         psta->BA_starting_seqctrl[status & 0x07] = start_seq;
8979
8980                                         BA_starting_seqctrl = start_seq << 4;
8981                                 }
8982
8983                                 BA_starting_seqctrl = cpu_to_le16(BA_starting_seqctrl);
8984                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_starting_seqctrl)), &(pattrib->pktlen));
8985                                 DBG_871X("%s, category=%d, action=%d, status=%d\n", __FUNCTION__, category, action, status);
8986                                 break;
8987
8988                         case 1: //ADDBA rsp
8989                                 pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen));
8990                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&status), &(pattrib->pktlen));
8991
8992                                 BA_para_set = le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set);
8993
8994                                 if(padapter->driver_rx_ampdu_factor != 0xFF)
8995                                         max_rx_ampdu_factor = (HT_CAP_AMPDU_FACTOR)padapter->driver_rx_ampdu_factor;
8996                                 else
8997                                         rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
8998
8999                                 if(MAX_AMPDU_FACTOR_64K == max_rx_ampdu_factor)
9000                                         ba_rxbuf_sz = 64;
9001                                 else if(MAX_AMPDU_FACTOR_32K == max_rx_ampdu_factor)
9002                                         ba_rxbuf_sz = 32;
9003                                 else if(MAX_AMPDU_FACTOR_16K == max_rx_ampdu_factor)
9004                                         ba_rxbuf_sz = 16;
9005                                 else if(MAX_AMPDU_FACTOR_8K == max_rx_ampdu_factor)
9006                                         ba_rxbuf_sz = 8;
9007                                 else
9008                                         ba_rxbuf_sz = 64;
9009
9010                                 #ifdef CONFIG_BT_COEXIST
9011                                 if (rtw_btcoex_IsBTCoexCtrlAMPDUSize(padapter) == _TRUE)
9012                                         ba_rxbuf_sz = rtw_btcoex_GetAMPDUSize(padapter);
9013                                 #endif
9014
9015                                 if (padapter->fix_ba_rxbuf_bz != 0xFF)
9016                                         ba_rxbuf_sz = padapter->fix_ba_rxbuf_bz;
9017
9018                                 if (ba_rxbuf_sz > 127)
9019                                         ba_rxbuf_sz = 127;
9020
9021                                 BA_para_set &= ~RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
9022                                 BA_para_set |= (ba_rxbuf_sz << 6) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
9023
9024                                 if (!padapter->registrypriv.wifi_spec) {
9025                                         if(pregpriv->ampdu_amsdu==0)//disabled
9026                                                 BA_para_set &= ~BIT(0);
9027                                         else if(pregpriv->ampdu_amsdu==1)//enabled
9028                                                 BA_para_set |= BIT(0);
9029                                 }
9030
9031                                 BA_para_set = cpu_to_le16(BA_para_set);
9032
9033                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
9034                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen));
9035                                 DBG_871X("%s, category=%d, action=%d, status=%d, rxbuf_sz=%u\n", __FUNCTION__, category, action, status, ba_rxbuf_sz);
9036                                 break;
9037                         case 2://DELBA
9038                                 BA_para_set = (status & 0x1F) << 3;
9039                                 BA_para_set = cpu_to_le16(BA_para_set);
9040                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
9041
9042                                 reason_code = 37;//Requested from peer STA as it does not want to use the mechanism
9043                                 reason_code = cpu_to_le16(reason_code);
9044                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(reason_code)), &(pattrib->pktlen));
9045                                 DBG_871X("%s, category=%d, action=%d, status=%d\n", __FUNCTION__, category, action, status);
9046                                 break;
9047                         default:
9048                                 break;
9049                 }
9050         }
9051
9052         pattrib->last_txcmdsz = pattrib->pktlen;
9053
9054         dump_mgntframe(padapter, pmgntframe);
9055 #endif //CONFIG_80211N_HT
9056 }
9057
9058 static void issue_action_BSSCoexistPacket(_adapter *padapter)
9059 {
9060         _irqL   irqL;
9061         _list           *plist, *phead;
9062         unsigned char category, action;
9063         struct xmit_frame                       *pmgntframe;
9064         struct pkt_attrib                       *pattrib;
9065         unsigned char                           *pframe;
9066         struct rtw_ieee80211_hdr        *pwlanhdr;
9067         unsigned short                  *fctrl;
9068         struct  wlan_network    *pnetwork = NULL;
9069         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
9070         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
9071         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
9072         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9073         _queue          *queue  = &(pmlmepriv->scanned_queue);
9074         u8 InfoContent[16] = {0};
9075         u8 ICS[8][15];
9076 #ifdef CONFIG_80211N_HT
9077         if((pmlmepriv->num_FortyMHzIntolerant==0) || (pmlmepriv->num_sta_no_ht==0))
9078                 return;
9079
9080         if(_TRUE == pmlmeinfo->bwmode_updated)
9081                 return;
9082
9083
9084         DBG_871X("%s\n", __FUNCTION__);
9085
9086
9087         category = RTW_WLAN_CATEGORY_PUBLIC;
9088         action = ACT_PUBLIC_BSSCOEXIST;
9089
9090         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
9091         {
9092                 return;
9093         }
9094
9095         //update attribute
9096         pattrib = &pmgntframe->attrib;
9097         update_mgntframe_attrib(padapter, pattrib);
9098
9099         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
9100
9101         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
9102         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
9103
9104         fctrl = &(pwlanhdr->frame_ctl);
9105         *(fctrl) = 0;
9106
9107         _rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
9108         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
9109         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
9110
9111         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
9112         pmlmeext->mgnt_seq++;
9113         SetFrameSubType(pframe, WIFI_ACTION);
9114
9115         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
9116         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
9117
9118         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
9119         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
9120
9121
9122         //
9123         if(pmlmepriv->num_FortyMHzIntolerant>0)
9124         {
9125                 u8 iedata=0;
9126
9127                 iedata |= BIT(2);//20 MHz BSS Width Request
9128
9129                 pframe = rtw_set_ie(pframe, EID_BSSCoexistence,  1, &iedata, &(pattrib->pktlen));
9130
9131         }
9132
9133
9134         //
9135         _rtw_memset(ICS, 0, sizeof(ICS));
9136         if(pmlmepriv->num_sta_no_ht>0)
9137         {
9138                 int i;
9139
9140                 _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
9141
9142                 phead = get_list_head(queue);
9143                 plist = get_next(phead);
9144
9145                 while(1)
9146                 {
9147                         int len;
9148                         u8 *p;
9149                         WLAN_BSSID_EX *pbss_network;
9150
9151                         if (rtw_end_of_queue_search(phead,plist)== _TRUE)
9152                                 break;
9153
9154                         pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
9155
9156                         plist = get_next(plist);
9157
9158                         pbss_network = (WLAN_BSSID_EX *)&pnetwork->network;
9159
9160                         p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
9161                         if((p==NULL) || (len==0))//non-HT
9162                         {
9163                                 if((pbss_network->Configuration.DSConfig<=0) || (pbss_network->Configuration.DSConfig>14))
9164                                         continue;
9165
9166                                 ICS[0][pbss_network->Configuration.DSConfig]=1;
9167
9168                                 if(ICS[0][0] == 0)
9169                                         ICS[0][0] = 1;
9170                         }
9171
9172                 }
9173
9174                 _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
9175
9176
9177                 for(i= 0;i<8;i++)
9178                 {
9179                         if(ICS[i][0] == 1)
9180                         {
9181                                 int j, k = 0;
9182
9183                                 InfoContent[k] = i;
9184                                 //SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent,i);
9185                                 k++;
9186
9187                                 for(j=1;j<=14;j++)
9188                                 {
9189                                         if(ICS[i][j]==1)
9190                                         {
9191                                                 if(k<16)
9192                                                 {
9193                                                         InfoContent[k] = j; //channel number
9194                                                         //SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j);
9195                                                         k++;
9196                                                 }
9197                                         }
9198                                 }
9199
9200                                 pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &(pattrib->pktlen));
9201
9202                         }
9203
9204                 }
9205
9206
9207         }
9208
9209
9210         pattrib->last_txcmdsz = pattrib->pktlen;
9211
9212         dump_mgntframe(padapter, pmgntframe);
9213 #endif //CONFIG_80211N_HT
9214 }
9215
9216 // Spatial Multiplexing Powersave (SMPS) action frame
9217 int _issue_action_SM_PS(_adapter *padapter ,  unsigned char *raddr , u8 NewMimoPsMode ,  u8 wait_ack)
9218 {
9219
9220         int ret=0;
9221         unsigned char category = RTW_WLAN_CATEGORY_HT;
9222         u8 action = RTW_WLAN_ACTION_HT_SM_PS;
9223         u8 sm_power_control=0;
9224         struct xmit_frame                       *pmgntframe;
9225         struct pkt_attrib                       *pattrib;
9226         unsigned char                                   *pframe;
9227         struct rtw_ieee80211_hdr        *pwlanhdr;
9228         unsigned short                          *fctrl;
9229         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
9230         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
9231         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9232
9233
9234         if(NewMimoPsMode==WLAN_HT_CAP_SM_PS_DISABLED)
9235         {
9236                 sm_power_control = sm_power_control  & ~(BIT(0)); // SM Power Save Enable = 0 SM Power Save Disable
9237         }
9238         else if(NewMimoPsMode==WLAN_HT_CAP_SM_PS_STATIC)
9239         {
9240                 sm_power_control = sm_power_control | BIT(0);    // SM Power Save Enable = 1 SM Power Save Enable
9241                 sm_power_control = sm_power_control & ~(BIT(1)); // SM Mode = 0 Static Mode
9242         }
9243         else if(NewMimoPsMode==WLAN_HT_CAP_SM_PS_DYNAMIC)
9244         {
9245                 sm_power_control = sm_power_control | BIT(0); // SM Power Save Enable = 1 SM Power Save Enable
9246                 sm_power_control = sm_power_control | BIT(1); // SM Mode = 1 Dynamic Mode
9247         }
9248         else
9249                 return ret;
9250
9251         DBG_871X("%s, sm_power_control=%u, NewMimoPsMode=%u\n", __FUNCTION__ , sm_power_control , NewMimoPsMode );
9252
9253         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
9254                 return ret;
9255
9256         //update attribute
9257         pattrib = &pmgntframe->attrib;
9258         update_mgntframe_attrib(padapter, pattrib);
9259
9260         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
9261
9262         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
9263         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
9264
9265         fctrl = &(pwlanhdr->frame_ctl);
9266         *(fctrl) = 0;
9267
9268         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); /* RA */
9269         _rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); /* TA */
9270         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); /* DA = RA */
9271
9272         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
9273         pmlmeext->mgnt_seq++;
9274         SetFrameSubType(pframe, WIFI_ACTION);
9275
9276         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
9277         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
9278
9279         /* category, action */
9280         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
9281         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
9282
9283         pframe = rtw_set_fixed_ie(pframe, 1, &(sm_power_control), &(pattrib->pktlen));
9284
9285         pattrib->last_txcmdsz = pattrib->pktlen;
9286
9287         if(wait_ack)
9288         {
9289                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
9290         }
9291         else
9292         {
9293                 dump_mgntframe(padapter, pmgntframe);
9294                 ret = _SUCCESS;
9295         }
9296
9297         if (ret != _SUCCESS)
9298             DBG_8192C("%s, ack to\n", __func__);
9299
9300         return ret;
9301 }
9302
9303 int issue_action_SM_PS_wait_ack(_adapter *padapter, unsigned char *raddr, u8 NewMimoPsMode, int try_cnt, int wait_ms)
9304 {
9305         int ret = 0;
9306         int i = 0;
9307         u32 start = rtw_get_current_time();
9308
9309         do {
9310                 ret = _issue_action_SM_PS(padapter, raddr, NewMimoPsMode , wait_ms>0?_TRUE:_FALSE );
9311
9312                 i++;
9313
9314                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
9315                         break;
9316
9317                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
9318                         rtw_msleep_os(wait_ms);
9319
9320         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
9321
9322         if (ret != _FAIL) {
9323                 ret = _SUCCESS;
9324                 #ifndef DBG_XMIT_ACK
9325                 goto exit;
9326                 #endif
9327         }
9328
9329         if (try_cnt && wait_ms) {
9330                 if (raddr)
9331                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", %s , %d/%d in %u ms\n",
9332                                 FUNC_ADPT_ARG(padapter), MAC_ARG(raddr),
9333                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
9334                 else
9335                         DBG_871X(FUNC_ADPT_FMT", %s , %d/%d in %u ms\n",
9336                                 FUNC_ADPT_ARG(padapter),
9337                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
9338         }
9339 exit:
9340
9341         return ret;
9342 }
9343
9344 int issue_action_SM_PS(_adapter *padapter ,  unsigned char *raddr , u8 NewMimoPsMode )
9345 {
9346         DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(raddr));
9347         return _issue_action_SM_PS(padapter, raddr, NewMimoPsMode , _FALSE );
9348 }
9349
9350 unsigned int send_delba(_adapter *padapter, u8 initiator, u8 *addr)
9351 {
9352         struct sta_priv *pstapriv = &padapter->stapriv;
9353         struct sta_info *psta = NULL;
9354         //struct recv_reorder_ctrl *preorder_ctrl;
9355         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9356         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9357         u16 tid;
9358
9359         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
9360                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
9361                         return _SUCCESS;
9362
9363         psta = rtw_get_stainfo(pstapriv, addr);
9364         if(psta==NULL)
9365                 return _SUCCESS;
9366
9367         //DBG_871X("%s:%s\n", __FUNCTION__, (initiator==0)?"RX_DIR":"TX_DIR");
9368
9369         if(initiator==0) // recipient
9370         {
9371                 for(tid = 0;tid<MAXTID;tid++)
9372                 {
9373                         if(psta->recvreorder_ctrl[tid].enable == _TRUE)
9374                         {
9375                                 DBG_871X("rx agg disable tid(%d)\n",tid);
9376                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F));
9377                                 psta->recvreorder_ctrl[tid].enable = _FALSE;
9378                                 psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
9379                                 #ifdef DBG_RX_SEQ
9380                                 DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
9381                                         psta->recvreorder_ctrl[tid].indicate_seq);
9382                                 #endif
9383                         }
9384                 }
9385         }
9386         else if(initiator == 1)// originator
9387         {
9388 #ifdef CONFIG_80211N_HT
9389                 //DBG_871X("tx agg_enable_bitmap(0x%08x)\n", psta->htpriv.agg_enable_bitmap);
9390                 for(tid = 0;tid<MAXTID;tid++)
9391                 {
9392                         if(psta->htpriv.agg_enable_bitmap & BIT(tid))
9393                         {
9394                                 DBG_871X("tx agg disable tid(%d)\n",tid);
9395                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F) );
9396                                 psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
9397                                 psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
9398
9399                         }
9400                 }
9401 #endif //CONFIG_80211N_HT
9402         }
9403
9404         return _SUCCESS;
9405
9406 }
9407
9408 unsigned int send_beacon(_adapter *padapter)
9409 {
9410         u8      bxmitok = _FALSE;
9411         int     issue=0;
9412         int poll = 0;
9413 //#ifdef CONFIG_CONCURRENT_MODE
9414         //struct mlme_ext_priv  *pmlmeext = &(padapter->mlmeextpriv);
9415         //struct mlme_ext_info  *pmlmeinfo = &(pmlmeext->mlmext_info);
9416         //_adapter *pbuddy_adapter = padapter->pbuddy_adapter;
9417         //struct mlme_priv *pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
9418 //#endif
9419
9420 #ifdef CONFIG_PCI_HCI
9421         //DBG_871X("%s\n", __FUNCTION__);
9422
9423         rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
9424
9425         /* 8192EE Port select for Beacon DL */
9426         rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
9427
9428         issue_beacon(padapter, 0);
9429
9430         return _SUCCESS;
9431 #endif
9432
9433 #if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
9434         u32 start = rtw_get_current_time();
9435
9436         rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
9437         rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
9438         do{
9439                 issue_beacon(padapter, 100);
9440                 issue++;
9441                 do {
9442                         rtw_yield_os();
9443                         rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
9444                         poll++;
9445                 }while((poll%10)!=0 && _FALSE == bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
9446
9447         }while(_FALSE == bxmitok && issue<100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
9448
9449         if(padapter->bSurpriseRemoved || padapter->bDriverStopped)
9450         {
9451                 return _FAIL;
9452         }
9453
9454
9455         if(_FALSE == bxmitok)
9456         {
9457                 DBG_871X("%s fail! %u ms\n", __FUNCTION__, rtw_get_passing_time_ms(start));
9458                 return _FAIL;
9459         }
9460         else
9461         {
9462                 u32 passing_time = rtw_get_passing_time_ms(start);
9463
9464                 if(passing_time > 100 || issue > 3)
9465                         DBG_871X("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start));
9466                 //else
9467                 //      DBG_871X("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start));
9468
9469                 return _SUCCESS;
9470         }
9471
9472 #endif
9473
9474 }
9475
9476 /****************************************************************************
9477
9478 Following are some utitity fuctions for WiFi MLME
9479
9480 *****************************************************************************/
9481
9482 BOOLEAN IsLegal5GChannel(
9483         IN PADAPTER                     Adapter,
9484         IN u8                   channel)
9485 {
9486
9487         int i=0;
9488         u8 Channel_5G[45] = {36,38,40,42,44,46,48,50,52,54,56,58,
9489                 60,62,64,100,102,104,106,108,110,112,114,116,118,120,122,
9490                 124,126,128,130,132,134,136,138,140,149,151,153,155,157,159,
9491                 161,163,165};
9492         for(i=0;i<sizeof(Channel_5G);i++)
9493                 if(channel == Channel_5G[i])
9494                         return _TRUE;
9495         return _FALSE;
9496 }
9497
9498 void site_survey(_adapter *padapter)
9499 {
9500         unsigned char           survey_channel = 0, val8;
9501         RT_SCAN_TYPE    ScanType = SCAN_PASSIVE;
9502         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9503         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9504         u32 initialgain = 0;
9505         u32 channel_scan_time_ms = 0,val32 = 0;
9506
9507 #ifdef CONFIG_P2P
9508
9509 #ifdef CONFIG_CONCURRENT_MODE
9510
9511 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
9512         u8 stay_buddy_ch = 0;
9513 #endif
9514         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
9515         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
9516         struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
9517
9518 #endif //CONFIG_CONCURRENT_MODE
9519         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
9520         static unsigned char  prev_survey_channel = 0;
9521         static unsigned int p2p_scan_count = 0;
9522
9523         if ( ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) || ( pwdinfo->p2p_info.scan_op_ch_only ) )
9524         {
9525                 if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
9526                 {
9527                         survey_channel = pwdinfo->rx_invitereq_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
9528                 }
9529                 else
9530                 {
9531                         survey_channel = pwdinfo->p2p_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
9532                 }
9533                 ScanType = SCAN_ACTIVE;
9534         }
9535         else if(rtw_p2p_findphase_ex_is_social(pwdinfo))
9536         {
9537                 //      Commented by Albert 2011/06/03
9538                 //      The driver is in the find phase, it should go through the social channel.
9539                 int ch_set_idx;
9540                 survey_channel = pwdinfo->social_chan[pmlmeext->sitesurvey_res.channel_idx];
9541                 ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, survey_channel);
9542                 if (ch_set_idx >= 0)
9543                         ScanType = pmlmeext->channel_set[ch_set_idx].ScanType;
9544                 else
9545                         ScanType = SCAN_ACTIVE;
9546         }
9547         else
9548 #endif //CONFIG_P2P
9549         {
9550                 struct rtw_ieee80211_channel *ch;
9551                 if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
9552                         ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
9553                         survey_channel = ch->hw_value;
9554                         ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
9555                 }
9556         }
9557
9558         if (0){
9559 #ifdef CONFIG_P2P
9560                 DBG_871X(FUNC_ADPT_FMT" ch:%u (cnt:%u,idx:%d) at %dms, %c%c%c\n"
9561                 , FUNC_ADPT_ARG(padapter)
9562                 , survey_channel
9563                 , pwdinfo->find_phase_state_exchange_cnt, pmlmeext->sitesurvey_res.channel_idx
9564                 , rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time)
9565                 , ScanType?'A':'P', pmlmeext->sitesurvey_res.scan_mode?'A':'P'
9566                 , pmlmeext->sitesurvey_res.ssid[0].SsidLength?'S':' '
9567                 );
9568 #else
9569                 DBG_871X(FUNC_ADPT_FMT" ch:%u (cnt:%u) at %dms, %c%c%c\n"
9570                 , FUNC_ADPT_ARG(padapter)
9571                 , survey_channel
9572                 , pmlmeext->sitesurvey_res.channel_idx
9573                 , rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time)
9574                 , ScanType?'A':'P', pmlmeext->sitesurvey_res.scan_mode?'A':'P'
9575                 , pmlmeext->sitesurvey_res.ssid[0].SsidLength?'S':' '
9576                 );
9577 #endif // CONFIG_P2P
9578                 #ifdef DBG_FIXED_CHAN
9579                 DBG_871X(FUNC_ADPT_FMT" fixed_chan:%u\n", pmlmeext->fixed_chan);
9580                 #endif
9581         }
9582
9583         if(survey_channel != 0)
9584         {
9585                 //PAUSE 4-AC Queue when site_survey
9586                 //rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
9587                 //val8 |= 0x0f;
9588                 //rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
9589 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
9590                 if((padapter->pbuddy_adapter->mlmeextpriv.mlmext_info.state&0x03) == WIFI_FW_AP_STATE)
9591                 {
9592                         if( pmlmeinfo->scan_cnt == RTW_SCAN_NUM_OF_CH )
9593                         {
9594                                 pmlmeinfo->scan_cnt = 0;
9595                                 survey_channel = pbuddy_mlmeext->cur_channel;
9596                                 stay_buddy_ch = 1;
9597                         }
9598                         else
9599                         {
9600                                 if( pmlmeinfo->scan_cnt == 0 )
9601                                         stay_buddy_ch = 2;
9602                                 pmlmeinfo->scan_cnt++;
9603                         }
9604                 }
9605 #endif
9606                 if(pmlmeext->sitesurvey_res.channel_idx == 0)
9607                 {
9608 #ifdef DBG_FIXED_CHAN
9609                         if(pmlmeext->fixed_chan !=0xff)
9610                                 set_channel_bwmode(padapter, pmlmeext->fixed_chan, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
9611                         else
9612 #endif
9613                                 set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
9614                 }
9615                 else
9616                 {
9617 #ifdef DBG_FIXED_CHAN
9618                         if(pmlmeext->fixed_chan!=0xff)
9619                                 SelectChannel(padapter, pmlmeext->fixed_chan);
9620                         else
9621 #endif
9622                                 SelectChannel(padapter, survey_channel);
9623                 }
9624
9625 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
9626                 if( stay_buddy_ch == 1 )
9627                 {
9628                         val8 = 0; //survey done
9629                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
9630
9631                         if(check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) &&
9632                                 check_buddy_fwstate(padapter, _FW_LINKED))
9633                         {
9634                                 update_beacon(padapter->pbuddy_adapter, 0, NULL, _TRUE);
9635                         }
9636                 }
9637                 else if( stay_buddy_ch == 2 )
9638                 {
9639                         val8 = 1; //under site survey
9640                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
9641                 }
9642 #endif
9643
9644                 if(ScanType == SCAN_ACTIVE) //obey the channel plan setting...
9645                 {
9646                         #ifdef CONFIG_P2P
9647                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) ||
9648                                 rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)
9649                         )
9650                         {
9651                                 issue_probereq_p2p(padapter, NULL);
9652                                 issue_probereq_p2p(padapter, NULL);
9653                                 issue_probereq_p2p(padapter, NULL);
9654                         }
9655                         else
9656                         #endif //CONFIG_P2P
9657                         {
9658                                 int i;
9659                                 for(i=0;i<RTW_SSID_SCAN_AMOUNT;i++){
9660                                         if(pmlmeext->sitesurvey_res.ssid[i].SsidLength) {
9661                                                 /* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */
9662                                                 if (padapter->registrypriv.wifi_spec)
9663                                                         issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
9664                                                 else
9665                                                         issue_probereq_ex(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL, 0, 0, 0, 0);
9666                                                 issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
9667                                         }
9668                                 }
9669
9670                                 if(pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
9671                                         /* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */
9672                                         if (padapter->registrypriv.wifi_spec)
9673                                                 issue_probereq(padapter, NULL, NULL);
9674                                         else
9675                                                 issue_probereq_ex(padapter, NULL, NULL, 0, 0, 0, 0);
9676                                         issue_probereq(padapter, NULL, NULL);
9677                                 }
9678                         }
9679                 }
9680
9681 #if  defined(CONFIG_ATMEL_RC_PATCH)
9682                 // treat wlan0 & p2p0 in same way, may be changed in near feature.
9683                 // assume home channel is 6, channel switch sequence will be
9684                 //      1,2-6-3,4-6-5,6-6-7,8-6-9,10-6-11,12-6-13,14
9685                 //if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)==_TRUE)
9686
9687                 if( stay_buddy_ch == 1 ){
9688                         channel_scan_time_ms = pmlmeext->chan_scan_time * RTW_STAY_AP_CH_MILLISECOND;
9689                 }
9690                 else {
9691                         if( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
9692                                 channel_scan_time_ms = 20;
9693                         else
9694                                 channel_scan_time_ms = 40
9695                 }
9696 #elif defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE)
9697                 if( stay_buddy_ch == 1 )
9698                         channel_scan_time_ms = pmlmeext->chan_scan_time * RTW_STAY_AP_CH_MILLISECOND ;
9699                 else
9700                         channel_scan_time_ms = pmlmeext->chan_scan_time;
9701 #else
9702                         channel_scan_time_ms = pmlmeext->chan_scan_time;
9703 #endif
9704
9705                 set_survey_timer(pmlmeext, channel_scan_time_ms);
9706 #if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
9707                 {
9708                         struct noise_info info;
9709                         info.bPauseDIG = _FALSE;
9710                         info.IGIValue = 0;
9711                         info.max_time = channel_scan_time_ms/2;//ms
9712                         info.chan = survey_channel;
9713                         rtw_hal_set_odm_var(padapter, HAL_ODM_NOISE_MONITOR,&info, _FALSE);
9714                 }
9715 #endif
9716
9717         }
9718         else
9719         {
9720
9721                 //      channel number is 0 or this channel is not valid.
9722
9723 #ifdef CONFIG_CONCURRENT_MODE
9724                 u8 cur_channel;
9725                 u8 cur_bwmode;
9726                 u8 cur_ch_offset;
9727
9728                 if (rtw_get_ch_setting_union(padapter, &cur_channel, &cur_bwmode, &cur_ch_offset) != 0)
9729                 {
9730                         if (0)
9731                         DBG_871X(FUNC_ADPT_FMT" back to linked/linking union - ch:%u, bw:%u, offset:%u\n",
9732                                 FUNC_ADPT_ARG(padapter), cur_channel, cur_bwmode, cur_ch_offset);
9733                 }
9734                 #ifdef CONFIG_IOCTL_CFG80211
9735                 else if(padapter->pbuddy_adapter
9736                         && pbuddy_adapter->wdinfo.driver_interface == DRIVER_CFG80211
9737                         && adapter_wdev_data(pbuddy_adapter)->p2p_enabled
9738                         && rtw_p2p_chk_state(&pbuddy_adapter->wdinfo, P2P_STATE_LISTEN)
9739                         )
9740                 {
9741                         cur_channel = pbuddy_adapter->wdinfo.listen_channel;
9742                         cur_bwmode = pbuddy_mlmeext->cur_bwmode;
9743                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
9744                 }
9745                 #endif
9746                 else
9747                 {
9748                         cur_channel = pmlmeext->cur_channel;
9749                         cur_bwmode = pmlmeext->cur_bwmode;
9750                         cur_ch_offset = pmlmeext->cur_ch_offset;
9751                 }
9752 #endif
9753
9754
9755 #ifdef CONFIG_P2P
9756                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
9757                 {
9758                         if( ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) || ( pwdinfo->p2p_info.scan_op_ch_only ) )
9759                         {
9760                                 //      Set the find_phase_state_exchange_cnt to P2P_FINDPHASE_EX_CNT.
9761                                 //      This will let the following flow to run the scanning end.
9762                                 rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
9763                         }
9764                         #ifdef CONFIG_DBG_P2P
9765                         DBG_871X( "[%s] find phase exchange cnt = %d\n", __FUNCTION__, pwdinfo->find_phase_state_exchange_cnt );
9766                         #endif
9767                 }
9768
9769                 if(rtw_p2p_findphase_ex_is_needed(pwdinfo))
9770                 {
9771                         //      Set the P2P State to the listen state of find phase and set the current channel to the listen channel
9772                         set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
9773                         rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN);
9774                         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
9775
9776                         //turn on phy-dynamic functions
9777                         Restore_DM_Func_Flag(padapter);
9778                         //Switch_DM_Func(padapter, DYNAMIC_FUNC_DIG|DYNAMIC_FUNC_HP|DYNAMIC_FUNC_SS, _TRUE);
9779
9780                         initialgain = 0xff; //restore RX GAIN
9781                         rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
9782
9783                         _set_timer( &pwdinfo->find_phase_timer, ( u32 ) ( ( u32 ) ( pwdinfo->listen_dwell ) * 100 ) );
9784                 }
9785                 else
9786 #endif //CONFIG_P2P
9787                 {
9788
9789 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
9790                         pmlmeinfo->scan_cnt = 0;
9791 #endif
9792
9793 #ifdef CONFIG_ANTENNA_DIVERSITY
9794                         // 20100721:Interrupt scan operation here.
9795                         // For SW antenna diversity before link, it needs to switch to another antenna and scan again.
9796                         // It compares the scan result and select beter one to do connection.
9797                         if(rtw_hal_antdiv_before_linked(padapter))
9798                         {
9799                                 pmlmeext->sitesurvey_res.bss_cnt = 0;
9800                                 pmlmeext->sitesurvey_res.channel_idx = -1;
9801                                 pmlmeext->chan_scan_time = SURVEY_TO /2;
9802                                 set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
9803                                 return;
9804                         }
9805 #endif
9806
9807 #ifdef CONFIG_P2P
9808                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
9809                         {
9810                         #ifdef CONFIG_CONCURRENT_MODE
9811                                 if( pwdinfo->driver_interface == DRIVER_WEXT )
9812                                 {
9813                                         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
9814                                         {
9815                                                 _set_timer( &pwdinfo->ap_p2p_switch_timer, 500 );
9816                                         }
9817                                 }
9818                                 rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
9819                         #else
9820                                 rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
9821                         #endif
9822                         }
9823                         rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
9824 #endif //CONFIG_P2P
9825
9826                         pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
9827
9828                         //switch back to the original channel
9829                         //SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset);
9830
9831                         {
9832 #ifdef CONFIG_CONCURRENT_MODE
9833                                 set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
9834 #else
9835 #ifdef CONFIG_DUALMAC_CONCURRENT
9836                                 dc_set_channel_bwmode_survey_done(padapter);
9837 #else
9838
9839 #ifdef CONFIG_P2P
9840                         if( (pwdinfo->driver_interface == DRIVER_WEXT) && (rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN)) )
9841                                 set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
9842                         else
9843 #endif //CONFIG_P2P
9844                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
9845
9846 #endif //CONFIG_DUALMAC_CONCURRENT
9847 #endif //CONFIG_CONCURRENT_MODE
9848                         }
9849
9850                         //flush 4-AC Queue after site_survey
9851                         //val8 = 0;
9852                         //rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
9853
9854                         //config MSR
9855                         Set_MSR(padapter, (pmlmeinfo->state & 0x3));
9856
9857                         //turn on phy-dynamic functions
9858                         Restore_DM_Func_Flag(padapter);
9859                         //Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE);
9860
9861                         initialgain = 0xff; //restore RX GAIN
9862                         rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
9863
9864
9865                         if (is_client_associated_to_ap(padapter) == _TRUE)
9866                         {
9867                                 issue_nulldata(padapter, NULL, 0, 3, 500);
9868
9869 #ifdef CONFIG_CONCURRENT_MODE
9870                                 if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
9871                                 {
9872                                         DBG_871X("adapter is surveydone(buddy_adapter is linked), issue nulldata(pwrbit=0)\n");
9873
9874                                         issue_nulldata(padapter->pbuddy_adapter, NULL, 0, 3, 500);
9875                                 }
9876 #endif
9877                         }
9878 #ifdef CONFIG_CONCURRENT_MODE
9879                         else if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
9880                         {
9881                                 issue_nulldata(padapter->pbuddy_adapter, NULL, 0, 3, 500);
9882                         }
9883 #endif
9884
9885                         val8 = 0; //survey done
9886                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
9887
9888                         report_surveydone_event(padapter);
9889
9890                         pmlmeext->chan_scan_time = SURVEY_TO;
9891                         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
9892
9893                         issue_action_BSSCoexistPacket(padapter);
9894                         issue_action_BSSCoexistPacket(padapter);
9895                         issue_action_BSSCoexistPacket(padapter);
9896
9897                 }
9898
9899 #ifdef CONFIG_CONCURRENT_MODE
9900                 if(check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) &&
9901                         check_buddy_fwstate(padapter, _FW_LINKED))
9902                 {
9903
9904                         DBG_871X("survey done, current CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
9905
9906                         DBG_871X("restart pbuddy_adapter's beacon\n");
9907
9908                         update_beacon(padapter->pbuddy_adapter, 0, NULL, _TRUE);
9909                 }
9910 #endif
9911
9912         }
9913
9914         return;
9915
9916 }
9917
9918 //collect bss info from Beacon and Probe request/response frames.
9919 u8 collect_bss_info(_adapter *padapter, union recv_frame *precv_frame, WLAN_BSSID_EX *bssid)
9920 {
9921         int     i;
9922         u32     len;
9923         u8      *p;
9924         u16     val16, subtype;
9925         u8      *pframe = precv_frame->u.hdr.rx_data;
9926         u32     packet_len = precv_frame->u.hdr.len;
9927         u8 ie_offset;
9928         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
9929         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9930         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9931
9932         len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
9933
9934         if (len > MAX_IE_SZ)
9935         {
9936                 //DBG_871X("IE too long for survey event\n");
9937                 return _FAIL;
9938         }
9939
9940         _rtw_memset(bssid, 0, sizeof(WLAN_BSSID_EX));
9941
9942         subtype = GetFrameSubType(pframe);
9943
9944         if(subtype==WIFI_BEACON) {
9945                 bssid->Reserved[0] = 1;
9946                 ie_offset = _BEACON_IE_OFFSET_;
9947         } else {
9948                 // FIXME : more type
9949                 if (subtype == WIFI_PROBERSP) {
9950                         ie_offset = _PROBERSP_IE_OFFSET_;
9951                         bssid->Reserved[0] = 3;
9952                 }
9953                 else if (subtype == WIFI_PROBEREQ) {
9954                         ie_offset = _PROBEREQ_IE_OFFSET_;
9955                         bssid->Reserved[0] = 2;
9956                 }
9957                 else {
9958                         bssid->Reserved[0] = 0;
9959                         ie_offset = _FIXED_IE_LENGTH_;
9960                 }
9961         }
9962
9963         bssid->Length = sizeof(WLAN_BSSID_EX) - MAX_IE_SZ + len;
9964
9965         //below is to copy the information element
9966         bssid->IELength = len;
9967         _rtw_memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength);
9968
9969         //get the signal strength
9970         //bssid->Rssi = precv_frame->u.hdr.attrib.SignalStrength; // 0-100 index.
9971         bssid->Rssi = precv_frame->u.hdr.attrib.phy_info.RecvSignalPower; // in dBM.raw data
9972         bssid->PhyInfo.SignalQuality = precv_frame->u.hdr.attrib.phy_info.SignalQuality;//in percentage
9973         bssid->PhyInfo.SignalStrength = precv_frame->u.hdr.attrib.phy_info.SignalStrength;//in percentage
9974 #ifdef CONFIG_ANTENNA_DIVERSITY
9975         //rtw_hal_get_hwreg(padapter, HW_VAR_CURRENT_ANTENNA, (u8 *)(&bssid->PhyInfo.Optimum_antenna));
9976         rtw_hal_get_def_var(padapter, HAL_DEF_CURRENT_ANTENNA,  &bssid->PhyInfo.Optimum_antenna);
9977 #endif
9978
9979         // checking SSID
9980         if ((p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset)) == NULL)
9981         {
9982                 DBG_871X("marc: cannot find SSID for survey event\n");
9983                 return _FAIL;
9984         }
9985
9986         if (*(p + 1))
9987         {
9988                 if (len > NDIS_802_11_LENGTH_SSID)
9989                 {
9990                         DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
9991                         return _FAIL;
9992                 }
9993                 _rtw_memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
9994                 bssid->Ssid.SsidLength = *(p + 1);
9995         }
9996         else
9997         {
9998                 bssid->Ssid.SsidLength = 0;
9999         }
10000
10001         _rtw_memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
10002
10003         //checking rate info...
10004         i = 0;
10005         p = rtw_get_ie(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
10006         if (p != NULL)
10007         {
10008                 if (len > NDIS_802_11_LENGTH_RATES_EX)
10009                 {
10010                         DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
10011                         return _FAIL;
10012                 }
10013                 _rtw_memcpy(bssid->SupportedRates, (p + 2), len);
10014                 i = len;
10015         }
10016
10017         p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
10018         if (p != NULL)
10019         {
10020                 if (len > (NDIS_802_11_LENGTH_RATES_EX-i))
10021                 {
10022                         DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
10023                         return _FAIL;
10024                 }
10025                 _rtw_memcpy(bssid->SupportedRates + i, (p + 2), len);
10026         }
10027
10028         //todo:
10029 #if 0
10030         if (judge_network_type(bssid->SupportedRates, (len + i)) == WIRELESS_11B)
10031         {
10032                 bssid->NetworkTypeInUse = Ndis802_11DS;
10033         }
10034         else
10035 #endif
10036         {
10037                 bssid->NetworkTypeInUse = Ndis802_11OFDM24;
10038         }
10039
10040 #ifdef CONFIG_P2P
10041         if (subtype == WIFI_PROBEREQ)
10042         {
10043                 u8 *p2p_ie;
10044                 u32     p2p_ielen;
10045                 // Set Listion Channel
10046                 if ((p2p_ie = rtw_get_p2p_ie(bssid->IEs, bssid->IELength, NULL, &p2p_ielen)))
10047                 {
10048                         u32     attr_contentlen = 0;
10049                         u8 listen_ch[5] = { 0x00 };
10050
10051                         if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_LISTEN_CH, listen_ch, &attr_contentlen) != NULL)
10052                                 bssid->Configuration.DSConfig = listen_ch[4];
10053                         else
10054                                 return _FALSE; // Intel device maybe no bring Listen Channel
10055                 } else
10056                 { // use current channel
10057                         bssid->Configuration.DSConfig = padapter->mlmeextpriv.cur_channel;
10058                         DBG_871X("%s()-%d: Cannot get p2p_ie. set DSconfig to op_ch(%d)\n", __FUNCTION__, __LINE__, bssid->Configuration.DSConfig);
10059                 }
10060
10061                 // FIXME
10062                 bssid->InfrastructureMode = Ndis802_11Infrastructure;
10063                 _rtw_memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
10064                 bssid->Privacy = 1;
10065                 return _SUCCESS;
10066         }
10067 #endif //CONFIG_P2P
10068
10069         if (bssid->IELength < 12)
10070                 return _FAIL;
10071
10072         // Checking for DSConfig
10073         p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
10074
10075         bssid->Configuration.DSConfig = 0;
10076         bssid->Configuration.Length = 0;
10077
10078         if (p)
10079         {
10080                 bssid->Configuration.DSConfig = *(p + 2);
10081         }
10082         else
10083         {// In 5G, some ap do not have DSSET IE
10084                 // checking HT info for channel
10085                 p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
10086                 if(p)
10087                 {
10088                         struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
10089                         bssid->Configuration.DSConfig = HT_info->primary_channel;
10090                 }
10091                 else
10092                 { // use current channel
10093                         bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter);
10094                 }
10095         }
10096
10097         _rtw_memcpy(&bssid->Configuration.BeaconPeriod, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
10098         bssid->Configuration.BeaconPeriod = le32_to_cpu(bssid->Configuration.BeaconPeriod);
10099
10100         val16 = rtw_get_capability((WLAN_BSSID_EX *)bssid);
10101
10102         if (val16 & BIT(0))
10103         {
10104                 bssid->InfrastructureMode = Ndis802_11Infrastructure;
10105                 _rtw_memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
10106         }
10107         else
10108         {
10109                 bssid->InfrastructureMode = Ndis802_11IBSS;
10110                 _rtw_memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN);
10111         }
10112
10113         if (val16 & BIT(4))
10114                 bssid->Privacy = 1;
10115         else
10116                 bssid->Privacy = 0;
10117
10118         bssid->Configuration.ATIMWindow = 0;
10119
10120         //20/40 BSS Coexistence check
10121         if((pregistrypriv->wifi_spec==1) && (_FALSE == pmlmeinfo->bwmode_updated))
10122         {
10123                 struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
10124 #ifdef CONFIG_80211N_HT
10125                 p = rtw_get_ie(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset);
10126                 if(p && len>0)
10127                 {
10128                         struct HT_caps_element  *pHT_caps;
10129                         pHT_caps = (struct HT_caps_element      *)(p + 2);
10130
10131                         if(pHT_caps->u.HT_cap_element.HT_caps_info&BIT(14))
10132                         {
10133                                 pmlmepriv->num_FortyMHzIntolerant++;
10134                         }
10135                 }
10136                 else
10137                 {
10138                         pmlmepriv->num_sta_no_ht++;
10139                 }
10140 #endif //CONFIG_80211N_HT
10141
10142         }
10143
10144 #ifdef CONFIG_INTEL_WIDI
10145         //process_intel_widi_query_or_tigger(padapter, bssid);
10146         if(process_intel_widi_query_or_tigger(padapter, bssid))
10147         {
10148                 return _FAIL;
10149         }
10150 #endif // CONFIG_INTEL_WIDI
10151
10152         #if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) & 1
10153         if(strcmp(bssid->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
10154                 DBG_871X("Receiving %s("MAC_FMT", DSConfig:%u) from ch%u with ss:%3u, sq:%3u, RawRSSI:%3ld\n"
10155                         , bssid->Ssid.Ssid, MAC_ARG(bssid->MacAddress), bssid->Configuration.DSConfig
10156                         , rtw_get_oper_ch(padapter)
10157                         , bssid->PhyInfo.SignalStrength, bssid->PhyInfo.SignalQuality, bssid->Rssi
10158                 );
10159         }
10160         #endif
10161
10162         // mark bss info receving from nearby channel as SignalQuality 101
10163         if(bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
10164         {
10165                 bssid->PhyInfo.SignalQuality= 101;
10166         }
10167
10168         return _SUCCESS;
10169 }
10170
10171 void start_create_ibss(_adapter* padapter)
10172 {
10173         unsigned short  caps;
10174         u8      val8;
10175         u8      join_type;
10176         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10177         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10178         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
10179         pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
10180         pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
10181
10182         //update wireless mode
10183         update_wireless_mode(padapter);
10184
10185         //udpate capability
10186         caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
10187         update_capinfo(padapter, caps);
10188         if(caps&cap_IBSS)//adhoc master
10189         {
10190                 //set_opmode_cmd(padapter, adhoc);//removed
10191
10192                 val8 = 0xcf;
10193                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
10194
10195                 rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
10196
10197                 //switch channel
10198                 //SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE);
10199                 set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
10200
10201                 beacon_timing_control(padapter);
10202
10203                 //set msr to WIFI_FW_ADHOC_STATE
10204                 pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
10205                 Set_MSR(padapter, (pmlmeinfo->state & 0x3));
10206
10207                 //issue beacon
10208                 if(send_beacon(padapter)==_FAIL)
10209                 {
10210                         RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("issuing beacon frame fail....\n"));
10211
10212                         report_join_res(padapter, -1);
10213                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
10214                 }
10215                 else
10216                 {
10217                         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
10218                         join_type = 0;
10219                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
10220
10221                         report_join_res(padapter, 1);
10222                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
10223                         rtw_indicate_connect(padapter);
10224                 }
10225         }
10226         else
10227         {
10228                 DBG_871X("start_create_ibss, invalid cap:%x\n", caps);
10229                 return;
10230         }
10231         //update bc/mc sta_info
10232         update_bmc_sta(padapter);
10233
10234 }
10235
10236 void start_clnt_join(_adapter* padapter)
10237 {
10238         unsigned short  caps;
10239         u8      val8;
10240         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10241         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10242         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
10243         int beacon_timeout;
10244         u8 ASIX_ID[]= {0x00, 0x0E, 0xC6};
10245
10246         //update wireless mode
10247         update_wireless_mode(padapter);
10248
10249         //udpate capability
10250         caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
10251         update_capinfo(padapter, caps);
10252
10253         //check if sta is ASIX peer and fix IOT issue if it is.
10254         if (_rtw_memcmp(get_my_bssid(&pmlmeinfo->network) ,ASIX_ID ,3)) {
10255                 u8 iot_flag = _TRUE;
10256                 rtw_hal_set_hwreg(padapter, HW_VAR_ASIX_IOT, (u8 *)(&iot_flag));
10257         }
10258         if (caps&cap_ESS)
10259         {
10260                 Set_MSR(padapter, WIFI_FW_STATION_STATE);
10261
10262                 val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf;
10263
10264 #ifdef CONFIG_WAPI_SUPPORT
10265                 if (padapter->wapiInfo.bWapiEnable && pmlmeinfo->auth_algo == dot11AuthAlgrthm_WAPI)
10266                 {
10267                         //Disable TxUseDefaultKey, RxUseDefaultKey, RxBroadcastUseDefaultKey.
10268                         val8 = 0x4c;
10269                 }
10270 #endif
10271                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
10272
10273                 #ifdef CONFIG_DEAUTH_BEFORE_CONNECT
10274                 // Because of AP's not receiving deauth before
10275                 // AP may: 1)not response auth or 2)deauth us after link is complete
10276                 // issue deauth before issuing auth to deal with the situation
10277
10278                 //      Commented by Albert 2012/07/21
10279                 //      For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it.
10280                 {
10281                         #ifdef CONFIG_P2P
10282                         _queue *queue = &(padapter->mlmepriv.scanned_queue);
10283                         _list   *head = get_list_head(queue);
10284                         _list *pos = get_next(head);
10285                         struct wlan_network *scanned = NULL;
10286                         u8 ie_offset = 0;
10287                         _irqL irqL;
10288                         bool has_p2p_ie = _FALSE;
10289
10290                         _enter_critical_bh(&(padapter->mlmepriv.scanned_queue.lock), &irqL);
10291
10292                         for (pos = get_next(head);!rtw_end_of_queue_search(head, pos); pos = get_next(pos)) {
10293
10294                                 scanned = LIST_CONTAINOR(pos, struct wlan_network, list);
10295                                 if(scanned==NULL)
10296                                         rtw_warn_on(1);
10297
10298                                 if (_rtw_memcmp(&(scanned->network.Ssid), &(pnetwork->Ssid), sizeof(NDIS_802_11_SSID)) == _TRUE
10299                                         && _rtw_memcmp(scanned->network.MacAddress, pnetwork->MacAddress, sizeof(NDIS_802_11_MAC_ADDRESS)) == _TRUE
10300                                 ) {
10301                                         ie_offset = (scanned->network.Reserved[0] == 2? 0:12);
10302                                         if (rtw_get_p2p_ie(scanned->network.IEs+ie_offset, scanned->network.IELength-ie_offset, NULL, NULL))
10303                                                 has_p2p_ie = _TRUE;
10304                                         break;
10305                                 }
10306                         }
10307
10308                         _exit_critical_bh(&(padapter->mlmepriv.scanned_queue.lock), &irqL);
10309
10310                         if (scanned == NULL || rtw_end_of_queue_search(head, pos) || has_p2p_ie == _FALSE)
10311                         #endif /* CONFIG_P2P */
10312                                 //To avoid connecting to AP fail during resume process, change retry count from 5 to 1
10313                                 issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
10314                 }
10315                 #endif /* CONFIG_DEAUTH_BEFORE_CONNECT */
10316
10317                 //here wait for receiving the beacon to start auth
10318                 //and enable a timer
10319                 beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
10320                 set_link_timer(pmlmeext, beacon_timeout);
10321                 _set_timer( &padapter->mlmepriv.assoc_timer,
10322                         (REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) +beacon_timeout);
10323
10324                 pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
10325         }
10326         else if (caps&cap_IBSS) //adhoc client
10327         {
10328                 Set_MSR(padapter, WIFI_FW_ADHOC_STATE);
10329
10330                 val8 = 0xcf;
10331                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
10332
10333                 beacon_timing_control(padapter);
10334
10335                 pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
10336
10337                 report_join_res(padapter, 1);
10338         }
10339         else
10340         {
10341                 //DBG_871X("marc: invalid cap:%x\n", caps);
10342                 return;
10343         }
10344
10345 }
10346
10347 void start_clnt_auth(_adapter* padapter)
10348 {
10349         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10350         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10351
10352         _cancel_timer_ex(&pmlmeext->link_timer);
10353
10354         pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
10355         pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
10356
10357         pmlmeinfo->auth_seq = 1;
10358         pmlmeinfo->reauth_count = 0;
10359         pmlmeinfo->reassoc_count = 0;
10360         pmlmeinfo->link_count = 0;
10361         pmlmeext->retry = 0;
10362
10363
10364         DBG_871X_LEVEL(_drv_always_, "start auth\n");
10365         issue_auth(padapter, NULL, 0);
10366
10367         set_link_timer(pmlmeext, REAUTH_TO);
10368
10369 }
10370
10371
10372 void start_clnt_assoc(_adapter* padapter)
10373 {
10374         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10375         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10376
10377         _cancel_timer_ex(&pmlmeext->link_timer);
10378
10379         pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
10380         pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
10381
10382         issue_assocreq(padapter);
10383
10384         set_link_timer(pmlmeext, REASSOC_TO);
10385 }
10386
10387 unsigned int receive_disconnect(_adapter *padapter, unsigned char *MacAddr, unsigned short reason)
10388 {
10389         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10390         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10391
10392         //check A3
10393         if (!(_rtw_memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
10394                 return _SUCCESS;
10395
10396         DBG_871X("%s\n", __FUNCTION__);
10397
10398         if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
10399         {
10400                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
10401                 {
10402                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
10403                         report_del_sta_event(padapter, MacAddr, reason);
10404
10405                 }
10406                 else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE)
10407                 {
10408                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
10409                         report_join_res(padapter, -2);
10410                 }
10411         }
10412
10413         return _SUCCESS;
10414 }
10415
10416 #ifdef CONFIG_80211D
10417 static void process_80211d(PADAPTER padapter, WLAN_BSSID_EX *bssid)
10418 {
10419         struct registry_priv *pregistrypriv;
10420         struct mlme_ext_priv *pmlmeext;
10421         RT_CHANNEL_INFO *chplan_new;
10422         u8 channel;
10423         u8 i;
10424
10425
10426         pregistrypriv = &padapter->registrypriv;
10427         pmlmeext = &padapter->mlmeextpriv;
10428
10429         // Adjust channel plan by AP Country IE
10430         if (pregistrypriv->enable80211d &&
10431                 (!pmlmeext->update_channel_plan_by_ap_done))
10432         {
10433                 u8 *ie, *p;
10434                 u32 len;
10435                 RT_CHANNEL_PLAN chplan_ap;
10436                 RT_CHANNEL_INFO chplan_sta[MAX_CHANNEL_NUM];
10437                 u8 country[4];
10438                 u8 fcn; // first channel number
10439                 u8 noc; // number of channel
10440                 u8 j, k;
10441
10442                 ie = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
10443                 if (!ie) return;
10444                 if (len < 6) return;
10445
10446                 ie += 2;
10447                 p = ie;
10448                 ie += len;
10449
10450                 _rtw_memset(country, 0, 4);
10451                 _rtw_memcpy(country, p, 3);
10452                 p += 3;
10453                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
10454                                 ("%s: 802.11d country=%s\n", __FUNCTION__, country));
10455
10456                 i = 0;
10457                 while ((ie - p) >= 3)
10458                 {
10459                         fcn = *(p++);
10460                         noc = *(p++);
10461                         p++;
10462
10463                         for (j = 0; j < noc; j++)
10464                         {
10465                                 if (fcn <= 14) channel = fcn + j; // 2.4 GHz
10466                                 else channel = fcn + j*4; // 5 GHz
10467
10468                                 chplan_ap.Channel[i++] = channel;
10469                         }
10470                 }
10471                 chplan_ap.Len = i;
10472
10473 #ifdef CONFIG_DEBUG_RTL871X
10474                 i = 0;
10475                 DBG_871X("%s: AP[%s] channel plan {", __FUNCTION__, bssid->Ssid.Ssid);
10476                 while ((i < chplan_ap.Len) && (chplan_ap.Channel[i] != 0))
10477                 {
10478                         DBG_8192C("%02d,", chplan_ap.Channel[i]);
10479                         i++;
10480                 }
10481                 DBG_871X("}\n");
10482 #endif
10483
10484                 _rtw_memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
10485 #ifdef CONFIG_DEBUG_RTL871X
10486                 i = 0;
10487                 DBG_871X("%s: STA channel plan {", __FUNCTION__);
10488                 while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
10489                 {
10490                         DBG_871X("%02d(%c),", chplan_sta[i].ChannelNum, chplan_sta[i].ScanType==SCAN_PASSIVE?'p':'a');
10491                         i++;
10492                 }
10493                 DBG_871X("}\n");
10494 #endif
10495
10496                 _rtw_memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
10497                 chplan_new = pmlmeext->channel_set;
10498
10499                 i = j = k = 0;
10500                 if (pregistrypriv->wireless_mode & WIRELESS_11G)
10501                 {
10502                         do {
10503                                 if ((i == MAX_CHANNEL_NUM) ||
10504                                         (chplan_sta[i].ChannelNum == 0) ||
10505                                         (chplan_sta[i].ChannelNum > 14))
10506                                         break;
10507
10508                                 if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
10509                                         break;
10510
10511                                 if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j])
10512                                 {
10513                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10514                                         chplan_new[k].ScanType = SCAN_ACTIVE;
10515                                         i++;
10516                                         j++;
10517                                         k++;
10518                                 }
10519                                 else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j])
10520                                 {
10521                                         chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10522 //                                      chplan_new[k].ScanType = chplan_sta[i].ScanType;
10523                                         chplan_new[k].ScanType = SCAN_PASSIVE;
10524                                         i++;
10525                                         k++;
10526                                 }
10527                                 else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j])
10528                                 {
10529                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10530                                         chplan_new[k].ScanType = SCAN_ACTIVE;
10531                                         j++;
10532                                         k++;
10533                                 }
10534                         } while (1);
10535
10536                         // change AP not support channel to Passive scan
10537                         while ((i < MAX_CHANNEL_NUM) &&
10538                                 (chplan_sta[i].ChannelNum != 0) &&
10539                                 (chplan_sta[i].ChannelNum <= 14))
10540                         {
10541                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10542 //                              chplan_new[k].ScanType = chplan_sta[i].ScanType;
10543                                 chplan_new[k].ScanType = SCAN_PASSIVE;
10544                                 i++;
10545                                 k++;
10546                         }
10547
10548                         // add channel AP supported
10549                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
10550                         {
10551                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10552                                 chplan_new[k].ScanType = SCAN_ACTIVE;
10553                                 j++;
10554                                 k++;
10555                         }
10556                 }
10557                 else
10558                 {
10559                         // keep original STA 2.4G channel plan
10560                         while ((i < MAX_CHANNEL_NUM) &&
10561                                 (chplan_sta[i].ChannelNum != 0) &&
10562                                 (chplan_sta[i].ChannelNum <= 14))
10563                         {
10564                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10565                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
10566                                 i++;
10567                                 k++;
10568                         }
10569
10570                         // skip AP 2.4G channel plan
10571                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
10572                         {
10573                                 j++;
10574                         }
10575                 }
10576
10577                 if (pregistrypriv->wireless_mode & WIRELESS_11A)
10578                 {
10579                         do {
10580                                 if ((i == MAX_CHANNEL_NUM) ||
10581                                         (chplan_sta[i].ChannelNum == 0))
10582                                         break;
10583
10584                                 if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0))
10585                                         break;
10586
10587                                 if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j])
10588                                 {
10589                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10590                                         chplan_new[k].ScanType = SCAN_ACTIVE;
10591                                         i++;
10592                                         j++;
10593                                         k++;
10594                                 }
10595                                 else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j])
10596                                 {
10597                                         chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10598 //                                      chplan_new[k].ScanType = chplan_sta[i].ScanType;
10599                                         chplan_new[k].ScanType = SCAN_PASSIVE;
10600                                         i++;
10601                                         k++;
10602                                 }
10603                                 else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j])
10604                                 {
10605                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10606                                         chplan_new[k].ScanType = SCAN_ACTIVE;
10607                                         j++;
10608                                         k++;
10609                                 }
10610                         } while (1);
10611
10612                         // change AP not support channel to Passive scan
10613                         while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
10614                         {
10615                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10616 //                              chplan_new[k].ScanType = chplan_sta[i].ScanType;
10617                                 chplan_new[k].ScanType = SCAN_PASSIVE;
10618                                 i++;
10619                                 k++;
10620                         }
10621
10622                         // add channel AP supported
10623                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] != 0))
10624                         {
10625                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
10626                                 chplan_new[k].ScanType = SCAN_ACTIVE;
10627                                 j++;
10628                                 k++;
10629                         }
10630                 }
10631                 else
10632                 {
10633                         // keep original STA 5G channel plan
10634                         while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
10635                         {
10636                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
10637                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
10638                                 i++;
10639                                 k++;
10640                         }
10641                 }
10642
10643                 pmlmeext->update_channel_plan_by_ap_done = 1;
10644
10645 #ifdef CONFIG_DEBUG_RTL871X
10646                 k = 0;
10647                 DBG_871X("%s: new STA channel plan {", __FUNCTION__);
10648                 while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0))
10649                 {
10650                         DBG_871X("%02d(%c),", chplan_new[k].ChannelNum, chplan_new[k].ScanType==SCAN_PASSIVE?'p':'c');
10651                         k++;
10652                 }
10653                 DBG_871X("}\n");
10654 #endif
10655
10656 #if 0
10657                 // recover the right channel index
10658                 channel = chplan_sta[pmlmeext->sitesurvey_res.channel_idx].ChannelNum;
10659                 k = 0;
10660                 while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0))
10661                 {
10662                         if (chplan_new[k].ChannelNum == channel) {
10663                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
10664                                                  ("%s: change mlme_ext sitesurvey channel index from %d to %d\n",
10665                                                   __FUNCTION__, pmlmeext->sitesurvey_res.channel_idx, k));
10666                                 pmlmeext->sitesurvey_res.channel_idx = k;
10667                                 break;
10668                         }
10669                         k++;
10670                 }
10671 #endif
10672         }
10673
10674         // If channel is used by AP, set channel scan type to active
10675         channel = bssid->Configuration.DSConfig;
10676         chplan_new = pmlmeext->channel_set;
10677         i = 0;
10678         while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0))
10679         {
10680                 if (chplan_new[i].ChannelNum == channel)
10681                 {
10682                         if (chplan_new[i].ScanType == SCAN_PASSIVE)
10683                         {
10684                                 //5G Bnad 2, 3 (DFS) doesn't change to active scan
10685                                 if(channel >= 52 && channel <= 144)
10686                                         break;
10687
10688                                 chplan_new[i].ScanType = SCAN_ACTIVE;
10689                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
10690                                                  ("%s: change channel %d scan type from passive to active\n",
10691                                                   __FUNCTION__, channel));
10692                         }
10693                         break;
10694                 }
10695                 i++;
10696         }
10697 }
10698 #endif
10699
10700 /****************************************************************************
10701
10702 Following are the functions to report events
10703
10704 *****************************************************************************/
10705
10706 void report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
10707 {
10708         struct cmd_obj *pcmd_obj;
10709         u8      *pevtcmd;
10710         u32 cmdsz;
10711         struct survey_event     *psurvey_evt;
10712         struct C2HEvent_Header *pc2h_evt_hdr;
10713         struct mlme_ext_priv *pmlmeext;
10714         struct cmd_priv *pcmdpriv;
10715         //u8 *pframe = precv_frame->u.hdr.rx_data;
10716         //uint len = precv_frame->u.hdr.len;
10717
10718         if(!padapter)
10719                 return;
10720
10721         pmlmeext = &padapter->mlmeextpriv;
10722         pcmdpriv = &padapter->cmdpriv;
10723
10724
10725         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10726         {
10727                 return;
10728         }
10729
10730         cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
10731         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
10732         {
10733                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10734                 return;
10735         }
10736
10737         _rtw_init_listhead(&pcmd_obj->list);
10738
10739         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
10740         pcmd_obj->cmdsz = cmdsz;
10741         pcmd_obj->parmbuf = pevtcmd;
10742
10743         pcmd_obj->rsp = NULL;
10744         pcmd_obj->rspsz  = 0;
10745
10746         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
10747         pc2h_evt_hdr->len = sizeof(struct survey_event);
10748         pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
10749         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
10750
10751         psurvey_evt = (struct survey_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
10752
10753         if (collect_bss_info(padapter, precv_frame, (WLAN_BSSID_EX *)&psurvey_evt->bss) == _FAIL)
10754         {
10755                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10756                 rtw_mfree((u8 *)pevtcmd, cmdsz);
10757                 return;
10758         }
10759
10760 #ifdef CONFIG_80211D
10761         process_80211d(padapter, &psurvey_evt->bss);
10762 #endif
10763
10764         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
10765
10766         pmlmeext->sitesurvey_res.bss_cnt++;
10767
10768         return;
10769
10770 }
10771
10772 void report_surveydone_event(_adapter *padapter)
10773 {
10774         struct cmd_obj *pcmd_obj;
10775         u8      *pevtcmd;
10776         u32 cmdsz;
10777         struct surveydone_event *psurveydone_evt;
10778         struct C2HEvent_Header  *pc2h_evt_hdr;
10779         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
10780         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
10781
10782         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10783         {
10784                 return;
10785         }
10786
10787         cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
10788         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
10789         {
10790                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10791                 return;
10792         }
10793
10794         _rtw_init_listhead(&pcmd_obj->list);
10795
10796         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
10797         pcmd_obj->cmdsz = cmdsz;
10798         pcmd_obj->parmbuf = pevtcmd;
10799
10800         pcmd_obj->rsp = NULL;
10801         pcmd_obj->rspsz  = 0;
10802
10803         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
10804         pc2h_evt_hdr->len = sizeof(struct surveydone_event);
10805         pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
10806         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
10807
10808         psurveydone_evt = (struct surveydone_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
10809         psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
10810
10811         DBG_871X("survey done event(%x) band:%d for "ADPT_FMT"\n", psurveydone_evt->bss_cnt, padapter->setband, ADPT_ARG(padapter));
10812
10813         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
10814
10815         return;
10816
10817 }
10818
10819 void report_join_res(_adapter *padapter, int res)
10820 {
10821         struct cmd_obj *pcmd_obj;
10822         u8      *pevtcmd;
10823         u32 cmdsz;
10824         struct joinbss_event            *pjoinbss_evt;
10825         struct C2HEvent_Header  *pc2h_evt_hdr;
10826         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
10827         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10828         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
10829
10830         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10831         {
10832                 return;
10833         }
10834
10835         cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
10836         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
10837         {
10838                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10839                 return;
10840         }
10841
10842         _rtw_init_listhead(&pcmd_obj->list);
10843
10844         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
10845         pcmd_obj->cmdsz = cmdsz;
10846         pcmd_obj->parmbuf = pevtcmd;
10847
10848         pcmd_obj->rsp = NULL;
10849         pcmd_obj->rspsz  = 0;
10850
10851         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
10852         pc2h_evt_hdr->len = sizeof(struct joinbss_event);
10853         pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
10854         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
10855
10856         pjoinbss_evt = (struct joinbss_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
10857         _rtw_memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
10858         pjoinbss_evt->network.join_res  = pjoinbss_evt->network.aid = res;
10859
10860         DBG_871X("report_join_res(%d)\n", res);
10861
10862
10863         rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network);
10864
10865
10866         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
10867
10868         return;
10869
10870 }
10871
10872 void report_wmm_edca_update(_adapter *padapter)
10873 {
10874         struct cmd_obj *pcmd_obj;
10875         u8      *pevtcmd;
10876         u32 cmdsz;
10877         struct wmm_event                *pwmm_event;
10878         struct C2HEvent_Header  *pc2h_evt_hdr;
10879         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
10880         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10881         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
10882
10883         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10884         {
10885                 return;
10886         }
10887
10888         cmdsz = (sizeof(struct wmm_event) + sizeof(struct C2HEvent_Header));
10889         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
10890         {
10891                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10892                 return;
10893         }
10894
10895         _rtw_init_listhead(&pcmd_obj->list);
10896
10897         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
10898         pcmd_obj->cmdsz = cmdsz;
10899         pcmd_obj->parmbuf = pevtcmd;
10900
10901         pcmd_obj->rsp = NULL;
10902         pcmd_obj->rspsz  = 0;
10903
10904         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
10905         pc2h_evt_hdr->len = sizeof(struct wmm_event);
10906         pc2h_evt_hdr->ID = GEN_EVT_CODE(_WMM);
10907         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
10908
10909         pwmm_event = (struct wmm_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
10910         pwmm_event->wmm =0;
10911
10912         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
10913
10914         return;
10915
10916 }
10917
10918 void report_del_sta_event(_adapter *padapter, unsigned char* MacAddr, unsigned short reason)
10919 {
10920         struct cmd_obj *pcmd_obj;
10921         u8      *pevtcmd;
10922         u32 cmdsz;
10923         struct sta_info *psta;
10924         int     mac_id;
10925         struct stadel_event                     *pdel_sta_evt;
10926         struct C2HEvent_Header  *pc2h_evt_hdr;
10927         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
10928         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
10929
10930         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10931         {
10932                 return;
10933         }
10934
10935         cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
10936         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
10937         {
10938                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10939                 return;
10940         }
10941
10942         _rtw_init_listhead(&pcmd_obj->list);
10943
10944         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
10945         pcmd_obj->cmdsz = cmdsz;
10946         pcmd_obj->parmbuf = pevtcmd;
10947
10948         pcmd_obj->rsp = NULL;
10949         pcmd_obj->rspsz  = 0;
10950
10951         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
10952         pc2h_evt_hdr->len = sizeof(struct stadel_event);
10953         pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
10954         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
10955
10956         pdel_sta_evt = (struct stadel_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
10957         _rtw_memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
10958         _rtw_memcpy((unsigned char *)(pdel_sta_evt->rsvd),(unsigned char *)(&reason),2);
10959
10960
10961         psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
10962         if(psta)
10963                 mac_id = (int)psta->mac_id;
10964         else
10965                 mac_id = (-1);
10966
10967         pdel_sta_evt->mac_id = mac_id;
10968
10969         DBG_871X("report_del_sta_event: delete STA, mac_id=%d\n", mac_id);
10970
10971         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
10972
10973         return;
10974 }
10975
10976 void report_add_sta_event(_adapter *padapter, unsigned char* MacAddr, int cam_idx)
10977 {
10978         struct cmd_obj *pcmd_obj;
10979         u8      *pevtcmd;
10980         u32 cmdsz;
10981         struct stassoc_event            *padd_sta_evt;
10982         struct C2HEvent_Header  *pc2h_evt_hdr;
10983         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
10984         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
10985
10986         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10987         {
10988                 return;
10989         }
10990
10991         cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
10992         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
10993         {
10994                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
10995                 return;
10996         }
10997
10998         _rtw_init_listhead(&pcmd_obj->list);
10999
11000         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
11001         pcmd_obj->cmdsz = cmdsz;
11002         pcmd_obj->parmbuf = pevtcmd;
11003
11004         pcmd_obj->rsp = NULL;
11005         pcmd_obj->rspsz  = 0;
11006
11007         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
11008         pc2h_evt_hdr->len = sizeof(struct stassoc_event);
11009         pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
11010         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
11011
11012         padd_sta_evt = (struct stassoc_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
11013         _rtw_memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN);
11014         padd_sta_evt->cam_id = cam_idx;
11015
11016         DBG_871X("report_add_sta_event: add STA\n");
11017
11018         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
11019
11020         return;
11021 }
11022
11023
11024 bool rtw_port_switch_chk(_adapter *adapter)
11025 {
11026         bool switch_needed = _FALSE;
11027 #ifdef CONFIG_CONCURRENT_MODE
11028 #ifdef CONFIG_RUNTIME_PORT_SWITCH
11029         struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
11030         struct pwrctrl_priv *pwrctl = dvobj_to_pwrctl(dvobj);
11031         _adapter *if_port0 = NULL;
11032         _adapter *if_port1 = NULL;
11033         struct mlme_ext_info *if_port0_mlmeinfo = NULL;
11034         struct mlme_ext_info *if_port1_mlmeinfo = NULL;
11035         int i;
11036
11037         for (i = 0; i < dvobj->iface_nums; i++) {
11038                 if (get_iface_type(dvobj->padapters[i]) == IFACE_PORT0) {
11039                         if_port0 = dvobj->padapters[i];
11040                         if_port0_mlmeinfo = &(if_port0->mlmeextpriv.mlmext_info);
11041                 }
11042                 else if (get_iface_type(dvobj->padapters[i]) == IFACE_PORT1) {
11043                         if_port1 = dvobj->padapters[i];
11044                         if_port1_mlmeinfo = &(if_port1->mlmeextpriv.mlmext_info);
11045                 }
11046         }
11047
11048         if (if_port0 == NULL) {
11049                 rtw_warn_on(1);
11050                 goto exit;
11051         }
11052
11053         if (if_port1 == NULL) {
11054                 rtw_warn_on(1);
11055                 goto exit;
11056         }
11057
11058 #ifdef DBG_RUNTIME_PORT_SWITCH
11059         DBG_871X(FUNC_ADPT_FMT" wowlan_mode:%u\n"
11060                 ADPT_FMT", port0, mlmeinfo->state:0x%08x, p2p_state:%d, %d\n"
11061                 ADPT_FMT", port1, mlmeinfo->state:0x%08x, p2p_state:%d, %d\n",
11062                 FUNC_ADPT_ARG(adapter), pwrctl->wowlan_mode,
11063                 ADPT_ARG(if_port0), if_port0_mlmeinfo->state, rtw_p2p_state(&if_port0->wdinfo), rtw_p2p_chk_state(&if_port0->wdinfo, P2P_STATE_NONE),
11064                 ADPT_ARG(if_port1), if_port1_mlmeinfo->state, rtw_p2p_state(&if_port1->wdinfo), rtw_p2p_chk_state(&if_port1->wdinfo, P2P_STATE_NONE));
11065 #endif /* DBG_RUNTIME_PORT_SWITCH */
11066
11067 #ifdef CONFIG_WOWLAN
11068         /* WOWLAN interface(primary, for now) should be port0 */
11069         if (pwrctl->wowlan_mode == _TRUE) {
11070                 if(!is_primary_adapter(if_port0)) {
11071                         DBG_871X("%s "ADPT_FMT" enable WOWLAN\n", __func__, ADPT_ARG(if_port1));
11072                         switch_needed = _TRUE;
11073                 }
11074                 goto exit;
11075         }
11076 #endif /* CONFIG_WOWLAN */
11077
11078         /* AP should use port0 for ctl frame's ack */
11079         if ((if_port1_mlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
11080                 DBG_871X("%s "ADPT_FMT" is AP/GO\n", __func__, ADPT_ARG(if_port1));
11081                 switch_needed = _TRUE;
11082                 goto exit;
11083         }
11084
11085         /* GC should use port0 for p2p ps */
11086         if (((if_port1_mlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE)
11087                 && (if_port1_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
11088                 && !rtw_p2p_chk_state(&if_port1->wdinfo, P2P_STATE_NONE)
11089                 && !check_fwstate(&if_port1->mlmepriv, WIFI_UNDER_WPS)
11090         ) {
11091                 DBG_871X("%s "ADPT_FMT" is GC\n", __func__, ADPT_ARG(if_port1));
11092                 switch_needed = _TRUE;
11093                 goto exit;
11094         }
11095
11096         /* port1 linked, but port0 not linked */
11097         if ((if_port1_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
11098                 && !(if_port0_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
11099                 && ((if_port0_mlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
11100         ) {
11101                 DBG_871X("%s "ADPT_FMT" is SINGLE_LINK\n", __func__, ADPT_ARG(if_port1));
11102                 switch_needed = _TRUE;
11103                 goto exit;
11104         }
11105
11106 exit:
11107 #ifdef DBG_RUNTIME_PORT_SWITCH
11108         DBG_871X(FUNC_ADPT_FMT" ret:%d\n", FUNC_ADPT_ARG(adapter), switch_needed);
11109 #endif /* DBG_RUNTIME_PORT_SWITCH */
11110 #endif /* CONFIG_RUNTIME_PORT_SWITCH */
11111 #endif /* CONFIG_CONCURRENT_MODE */
11112         return switch_needed;
11113 }
11114
11115 /****************************************************************************
11116
11117 Following are the event callback functions
11118
11119 *****************************************************************************/
11120
11121 //for sta/adhoc mode
11122 void update_sta_info(_adapter *padapter, struct sta_info *psta)
11123 {
11124         _irqL   irqL;
11125         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
11126         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11127         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11128
11129         //ERP
11130         VCS_update(padapter, psta);
11131
11132 #ifdef CONFIG_80211N_HT
11133         //HT
11134         if(pmlmepriv->htpriv.ht_option)
11135         {
11136                 psta->htpriv.ht_option = _TRUE;
11137
11138                 psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
11139
11140                 psta->htpriv.rx_ampdu_min_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para&IEEE80211_HT_CAP_AMPDU_DENSITY)>>2;
11141
11142                 if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_20))
11143                         psta->htpriv.sgi_20m = _TRUE;
11144
11145                 if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_40))
11146                         psta->htpriv.sgi_40m = _TRUE;
11147
11148                 psta->qos_option = _TRUE;
11149
11150                 psta->htpriv.ldpc_cap = pmlmepriv->htpriv.ldpc_cap;
11151                 psta->htpriv.stbc_cap = pmlmepriv->htpriv.stbc_cap;
11152                 psta->htpriv.beamform_cap = pmlmepriv->htpriv.beamform_cap;
11153
11154                 _rtw_memcpy(&psta->htpriv.ht_cap, &pmlmeinfo->HT_caps, sizeof(struct rtw_ieee80211_ht_cap));
11155         }
11156         else
11157 #endif //CONFIG_80211N_HT
11158         {
11159 #ifdef CONFIG_80211N_HT
11160                 psta->htpriv.ht_option = _FALSE;
11161
11162                 psta->htpriv.ampdu_enable = _FALSE;
11163
11164                 psta->htpriv.sgi_20m = _FALSE;
11165                 psta->htpriv.sgi_40m = _FALSE;
11166 #endif //CONFIG_80211N_HT
11167                 psta->qos_option = _FALSE;
11168
11169         }
11170
11171 #ifdef CONFIG_80211N_HT
11172         psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
11173
11174         psta->htpriv.agg_enable_bitmap = 0x0;//reset
11175         psta->htpriv.candidate_tid_bitmap = 0x0;//reset
11176 #endif //CONFIG_80211N_HT
11177
11178         psta->bw_mode = pmlmeext->cur_bwmode;
11179
11180         //QoS
11181         if(pmlmepriv->qospriv.qos_option)
11182                 psta->qos_option = _TRUE;
11183
11184 #ifdef CONFIG_80211AC_VHT
11185         _rtw_memcpy(&psta->vhtpriv, &pmlmepriv->vhtpriv, sizeof(struct vht_priv));
11186 #endif //CONFIG_80211AC_VHT
11187
11188         update_ldpc_stbc_cap(psta);
11189
11190         _enter_critical_bh(&psta->lock, &irqL);
11191         psta->state = _FW_LINKED;
11192         _exit_critical_bh(&psta->lock, &irqL);
11193
11194 }
11195
11196 static void rtw_mlmeext_disconnect(_adapter *padapter)
11197 {
11198         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
11199         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11200         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11201         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
11202         u8 state_backup = (pmlmeinfo->state&0x03);
11203         u8 ASIX_ID[]= {0x00, 0x0E, 0xC6};
11204
11205         //set_opmode_cmd(padapter, infra_client_with_mlme);
11206
11207 #if 1
11208         /*
11209          * For safety, prevent from keeping macid sleep.
11210          * If we can sure all power mode enter/leave are paired,
11211          * this check can be removed.
11212          * Lucas@20131113
11213          */
11214         /* wakeup macid after disconnect. */
11215         {
11216                 struct sta_info *psta;
11217                 psta = rtw_get_stainfo(&padapter->stapriv, get_my_bssid(pnetwork));
11218                 if (psta)
11219                         rtw_hal_macid_wakeup(padapter, psta->mac_id);
11220         }
11221 #endif
11222
11223         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
11224         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
11225
11226         //set MSR to no link state -> infra. mode
11227         Set_MSR(padapter, _HW_STATE_STATION_);
11228
11229         //check if sta is ASIX peer and fix IOT issue if it is.
11230         if (_rtw_memcmp(get_my_bssid(&pmlmeinfo->network) ,ASIX_ID ,3)) {
11231                 u8 iot_flag = _FALSE;
11232                 rtw_hal_set_hwreg(padapter, HW_VAR_ASIX_IOT, (u8 *)(&iot_flag));
11233         }
11234         pmlmeinfo->state = WIFI_FW_NULL_STATE;
11235
11236         if(state_backup == WIFI_FW_STATION_STATE)
11237         {
11238                 if (rtw_port_switch_chk(padapter) == _TRUE) {
11239                         rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
11240                         #ifdef CONFIG_LPS
11241                         {
11242                                 _adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter));
11243                                 if (port0_iface)
11244                                         rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0);
11245                         }
11246                         #endif
11247                 }
11248         }
11249
11250 #ifdef CONFIG_DUALMAC_CONCURRENT
11251         dc_set_channel_bwmode_disconnect(padapter);
11252 #else //!CONFIG_DUALMAC_CONCURRENT
11253 #ifdef CONFIG_CONCURRENT_MODE
11254         if((check_buddy_fwstate(padapter, _FW_LINKED)) != _TRUE)
11255         {
11256 #endif //CONFIG_CONCURRENT_MODE
11257                 //switch to the 20M Hz mode after disconnect
11258                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
11259                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
11260
11261                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
11262 #ifdef CONFIG_CONCURRENT_MODE
11263         }
11264 #endif //CONFIG_CONCURRENT_MODE
11265 #ifdef CONFIG_FCS_MODE
11266         else
11267         {
11268                 PADAPTER pbuddy_adapter;
11269                 struct mlme_ext_priv *pbuddy_mlmeext;
11270
11271                 if(EN_FCS(padapter))
11272                 {
11273                         pbuddy_adapter = padapter->pbuddy_adapter;
11274                         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
11275
11276                         rtw_hal_set_hwreg(padapter, HW_VAR_STOP_FCS_MODE, NULL);
11277
11278                         //switch to buddy's channel setting if buddy is linked
11279                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
11280                 }
11281         }
11282 #endif //!CONFIG_FCS_MODE
11283 #endif //!CONFIG_DUALMAC_CONCURRENT
11284
11285
11286         flush_all_cam_entry(padapter);
11287
11288         _cancel_timer_ex(&pmlmeext->link_timer);
11289
11290         //pmlmepriv->LinkDetectInfo.TrafficBusyState = _FALSE;
11291         pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
11292         pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
11293
11294 }
11295
11296 void mlmeext_joinbss_event_callback(_adapter *padapter, int join_res)
11297 {
11298         struct sta_info         *psta, *psta_bmc;
11299         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11300         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11301         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
11302         struct sta_priv         *pstapriv = &padapter->stapriv;
11303         u8      join_type;
11304
11305         if(join_res < 0)
11306         {
11307                 join_type = 1;
11308                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
11309                 rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
11310
11311                 goto exit_mlmeext_joinbss_event_callback;
11312         }
11313
11314         if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
11315         {
11316                 //update bc/mc sta_info
11317                 update_bmc_sta(padapter);
11318         }
11319
11320
11321         //turn on dynamic functions
11322         Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE);
11323
11324         // update IOT-releated issue
11325         update_IOT_info(padapter);
11326
11327         rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates);
11328
11329         //BCN interval
11330         rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
11331
11332         //udpate capability
11333         update_capinfo(padapter, pmlmeinfo->capability);
11334
11335         //WMM, Update EDCA param
11336         WMMOnAssocRsp(padapter);
11337
11338         //HT
11339         HTOnAssocRsp(padapter);
11340
11341 #ifdef CONFIG_80211AC_VHT
11342         //VHT
11343         VHTOnAssocRsp(padapter);
11344 #endif
11345
11346 #ifndef CONFIG_CONCURRENT_MODE
11347         //      Call set_channel_bwmode when the CONFIG_CONCURRENT_MODE doesn't be defined.
11348         //Set cur_channel&cur_bwmode&cur_ch_offset
11349         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
11350 #endif
11351
11352         psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
11353         if (psta) //only for infra. mode
11354         {
11355                 pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
11356
11357                 //DBG_871X("set_sta_rate\n");
11358
11359                 psta->wireless_mode = pmlmeext->cur_wireless_mode;
11360
11361                 //set per sta rate after updating HT cap.
11362                 set_sta_rate(padapter, psta);
11363
11364                 rtw_sta_media_status_rpt(padapter, psta, 1);
11365
11366                 /* wakeup macid after join bss successfully to ensure
11367                         the subsequent data frames can be sent out normally */
11368                 rtw_hal_macid_wakeup(padapter, psta->mac_id);
11369         }
11370
11371         if (rtw_port_switch_chk(padapter) == _TRUE)
11372                 rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
11373
11374         join_type = 2;
11375         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
11376
11377         if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
11378         {
11379                 // correcting TSF
11380                 correct_TSF(padapter, pmlmeext);
11381
11382                 //set_link_timer(pmlmeext, DISCONNECT_TO);
11383         }
11384
11385 #ifdef CONFIG_LPS
11386         if(get_iface_type(padapter) == IFACE_PORT0)
11387                 rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
11388 #endif
11389
11390 #ifdef CONFIG_BEAMFORMING
11391         if (psta)
11392                 beamforming_wk_cmd(padapter, BEAMFORMING_CTRL_ENTER, (u8 *)psta, sizeof(struct sta_info), 0);
11393 #endif
11394
11395 exit_mlmeext_joinbss_event_callback:
11396
11397 #ifdef CONFIG_DUALMAC_CONCURRENT
11398         dc_handle_join_done(padapter, join_res);
11399 #endif
11400 #ifdef CONFIG_CONCURRENT_MODE
11401         concurrent_chk_joinbss_done(padapter, join_res);
11402 #endif
11403
11404         DBG_871X("=>%s\n", __FUNCTION__);
11405
11406 }
11407
11408 //currently only adhoc mode will go here
11409 void mlmeext_sta_add_event_callback(_adapter *padapter, struct sta_info *psta)
11410 {
11411         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
11412         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11413         u8      join_type;
11414
11415         DBG_871X("%s\n", __FUNCTION__);
11416
11417         if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
11418         {
11419                 if(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)//adhoc master or sta_count>1
11420                 {
11421                         //nothing to do
11422                 }
11423                 else//adhoc client
11424                 {
11425                         //update TSF Value
11426                         //update_TSF(pmlmeext, pframe, len);
11427
11428                         // correcting TSF
11429                         correct_TSF(padapter, pmlmeext);
11430
11431                         //start beacon
11432                         if(send_beacon(padapter)==_FAIL)
11433                         {
11434                                 pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
11435
11436                                 pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
11437
11438                                 return;
11439                         }
11440
11441                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
11442
11443                 }
11444
11445                 join_type = 2;
11446                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
11447         }
11448
11449         pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
11450
11451         psta->bssratelen = rtw_get_rateset_len(pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates);
11452         _rtw_memcpy(psta->bssrateset, pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates, psta->bssratelen);
11453
11454         //update adhoc sta_info
11455         update_sta_info(padapter, psta);
11456
11457         rtw_hal_update_sta_rate_mask(padapter, psta);
11458
11459         // ToDo: HT for Ad-hoc
11460         psta->wireless_mode = rtw_check_network_type(psta->bssrateset, psta->bssratelen, pmlmeext->cur_channel);
11461         psta->raid = rtw_hal_networktype_to_raid(padapter, psta);
11462
11463         //rate radaptive
11464         Update_RA_Entry(padapter, psta);
11465 }
11466
11467 void mlmeext_sta_del_event_callback(_adapter *padapter)
11468 {
11469         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11470         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11471
11472         if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter))
11473         {
11474                 rtw_mlmeext_disconnect(padapter);
11475         }
11476
11477 }
11478
11479 /****************************************************************************
11480
11481 Following are the functions for the timer handlers
11482
11483 *****************************************************************************/
11484 void _linked_info_dump(_adapter *padapter)
11485 {
11486         int i;
11487         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11488         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11489         int UndecoratedSmoothedPWDB;
11490         struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
11491         struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
11492
11493         if(padapter->bLinkInfoDump){
11494
11495                 DBG_871X("\n============["ADPT_FMT"] linked status check ===================\n",ADPT_ARG(padapter));
11496
11497                 if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
11498                 {
11499                         rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
11500
11501                         DBG_871X("AP[" MAC_FMT "] - UndecoratedSmoothedPWDB:%d\n",
11502                                 MAC_ARG(padapter->mlmepriv.cur_network.network.MacAddress),UndecoratedSmoothedPWDB);
11503                 }
11504                 else if((pmlmeinfo->state&0x03) == _HW_STATE_AP_)
11505                 {
11506                         _irqL irqL;
11507                         _list   *phead, *plist;
11508
11509                         struct sta_info *psta=NULL;
11510                         struct sta_priv *pstapriv = &padapter->stapriv;
11511
11512                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
11513                         phead = &pstapriv->asoc_list;
11514                         plist = get_next(phead);
11515                         while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
11516                         {
11517                                 psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
11518                                 plist = get_next(plist);
11519
11520                                 DBG_871X("STA[" MAC_FMT "]:UndecoratedSmoothedPWDB:%d\n",
11521                                         MAC_ARG(psta->hwaddr),psta->rssi_stat.UndecoratedSmoothedPWDB);
11522                         }
11523                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
11524
11525                 }
11526                 for(i=0; i<macid_ctl->num; i++)
11527                 {
11528                         if(rtw_macid_is_used(macid_ctl, i)
11529                                 && !rtw_macid_is_bmc(macid_ctl, i) /* skip bc/mc sta */
11530                         ) {
11531                                 //============  tx info ============
11532                                 rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, &i);
11533                         }
11534                 }
11535                 rtw_hal_set_def_var(padapter, HAL_DEF_DBG_RX_INFO_DUMP, NULL);
11536
11537
11538         }
11539
11540
11541 }
11542
11543 u8 chk_ap_is_alive(_adapter *padapter, struct sta_info *psta)
11544 {
11545         u8 ret = _FALSE;
11546         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11547         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11548
11549         #ifdef DBG_EXPIRATION_CHK
11550         DBG_871X(FUNC_ADPT_FMT" rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu"
11551                                 /*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/
11552                                 ", retry:%u\n"
11553                 , FUNC_ADPT_ARG(padapter)
11554                 , STA_RX_PKTS_DIFF_ARG(psta)
11555                 , psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts
11556                 , psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts
11557                 /*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts
11558                 , psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts
11559                 , psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts
11560                 , pmlmeinfo->bcn_interval*/
11561                 , pmlmeext->retry
11562         );
11563
11564         DBG_871X(FUNC_ADPT_FMT" tx_pkts:%llu, link_count:%u\n", FUNC_ADPT_ARG(padapter)
11565                 , padapter->xmitpriv.tx_pkts
11566                 , pmlmeinfo->link_count
11567         );
11568         #endif
11569
11570         if((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta))
11571                 && sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta)
11572                 && sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta)
11573         )
11574         {
11575                 ret = _FALSE;
11576         }
11577         else
11578         {
11579                 ret = _TRUE;
11580         }
11581
11582         sta_update_last_rx_pkts(psta);
11583
11584         return ret;
11585 }
11586
11587 #ifdef CONFIG_TDLS
11588 void linked_status_chk_tdls(_adapter *padapter)
11589 {
11590 struct candidate_pool{
11591         struct sta_info *psta;
11592         u8 addr[ETH_ALEN];
11593 };
11594         struct sta_priv *pstapriv = &padapter->stapriv;
11595         _irqL irqL;
11596         u8 ack_chk;
11597         struct sta_info *psta;
11598         int i, num_teardown=0, num_checkalive=0;
11599         _list   *plist, *phead;
11600         struct tdls_txmgmt txmgmt;
11601         struct candidate_pool checkalive[NUM_STA];
11602         struct candidate_pool teardown[NUM_STA];
11603 #define ALIVE_MIN 2
11604 #define ALIVE_MAX 5
11605
11606         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
11607         _rtw_memset(checkalive, 0x00, sizeof(checkalive));
11608         _rtw_memset(teardown, 0x00, sizeof(teardown));
11609
11610         if((padapter->tdlsinfo.link_established == _TRUE)){
11611                 _enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
11612                 for(i=0; i< NUM_STA; i++)
11613                 {
11614                         phead = &(pstapriv->sta_hash[i]);
11615                         plist = get_next(phead);
11616
11617                         while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
11618                         {
11619                                 psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
11620                                 plist = get_next(plist);
11621
11622                                 if(psta->tdls_sta_state & TDLS_LINKED_STATE)
11623                                 {
11624                                         psta->alive_count++;
11625                                         if(psta->alive_count >= ALIVE_MIN)
11626                                         {
11627 #ifdef CONFIG_XMIT_ACK
11628                                                 if(psta->sta_stats.last_rx_data_pkts >= psta->sta_stats.rx_data_pkts)
11629 #else
11630                                                 if((psta->sta_stats.last_rx_data_pkts >= psta->sta_stats.rx_data_pkts) &&
11631                                                         (!(psta->tdls_sta_state & TDLS_ALIVE_STATE)) )
11632 #endif
11633                                                 {
11634                                                         if(psta->alive_count < ALIVE_MAX)
11635                                                         {
11636                                                                 _rtw_memcpy(checkalive[num_checkalive].addr, psta->hwaddr, ETH_ALEN);
11637                                                                 checkalive[num_checkalive].psta = psta;
11638                                                                 num_checkalive++;
11639                                                         }
11640                                                         else
11641                                                         {
11642                                                                 _rtw_memcpy(teardown[num_teardown].addr, psta->hwaddr, ETH_ALEN);
11643                                                                 teardown[num_teardown].psta = psta;
11644                                                                 num_teardown++;
11645                                                         }
11646                                                 }
11647                                                 else
11648                                                 {
11649                                                         psta->tdls_sta_state &= (~TDLS_ALIVE_STATE);
11650                                                         psta->alive_count = 0;
11651                                                 }
11652                                         }
11653                                         psta->sta_stats.last_rx_data_pkts = psta->sta_stats.rx_data_pkts;
11654                                 }
11655                         }
11656                 }
11657                 _exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
11658
11659                 if(num_checkalive > 0)
11660                 {
11661                         for(i=0; i< num_checkalive; i++)
11662                         {
11663 #ifdef CONFIG_XMIT_ACK
11664                                 //TDLS: Should we set wait_ms to 300 for keeping alive?
11665                                 ack_chk = issue_nulldata_to_TDLS_peer_STA(padapter,     checkalive[i].addr, 0, 3, 300);
11666
11667                                 if(ack_chk == _SUCCESS)
11668                                 {
11669                                         checkalive[i].psta->alive_count = 0;
11670                                 }
11671 #else
11672                                 checkalive[i].psta->tdls_sta_state &= (~TDLS_ALIVE_STATE);
11673                                 _rtw_memcpy(txmgmt.peer, checkalive[i].addr, ETH_ALEN);
11674                                 issue_tdls_dis_req(padapter, &txmgmt);
11675                                 issue_tdls_dis_req(padapter, &txmgmt);
11676                                 issue_tdls_dis_req(padapter, &txmgmt);
11677 #endif //CONFIG_XMIT_ACK
11678                         }
11679                 }
11680
11681                 if(num_teardown > 0)
11682                 {
11683                         for(i=0; i< num_teardown; i++)
11684                         {
11685                                 DBG_871X("[%s %d] Send teardown to "MAC_FMT" \n", __FUNCTION__, __LINE__, MAC_ARG(teardown[i].addr));
11686                                 txmgmt.status_code = _RSON_TDLS_TEAR_TOOFAR_;
11687                                 _rtw_memcpy(txmgmt.peer, teardown[i].addr, ETH_ALEN);
11688                                 issue_tdls_teardown(padapter, &txmgmt, _FALSE);
11689                         }
11690                 }
11691         }
11692
11693 }
11694 #endif //CONFIG_TDLS
11695
11696 //from_timer == 1 means driver is in LPS
11697 void linked_status_chk(_adapter *padapter, u8 from_timer)
11698 {
11699         u32     i;
11700         struct sta_info         *psta;
11701         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
11702         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11703         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11704         struct sta_priv         *pstapriv = &padapter->stapriv;
11705
11706
11707         if (is_client_associated_to_ap(padapter))
11708         {
11709                 //linked infrastructure client mode
11710
11711                 int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
11712                 int rx_chk_limit;
11713                 int link_count_limit;
11714
11715                 #if defined(DBG_ROAMING_TEST)
11716                 rx_chk_limit = 1;
11717                 #elif defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && !defined(CONFIG_LPS_LCLK_WD_TIMER)
11718                 rx_chk_limit = 4;
11719                 #else
11720                 rx_chk_limit = 8;
11721                 #endif
11722 #ifdef CONFIG_P2P
11723                 if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE))
11724                 {
11725                         if(!from_timer)
11726                                 link_count_limit = 3; // 8 sec
11727                         else
11728                                 link_count_limit = 15; // 32 sec
11729                 }
11730                 else
11731 #endif // CONFIG_P2P
11732                 {
11733                         if(!from_timer)
11734                                 link_count_limit = 7; // 16 sec
11735                         else
11736                                 link_count_limit = 29; // 60 sec
11737                 }
11738
11739                 // Marked by Kurt 20130715
11740                 // 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.
11741                 // todo: To check why we under miracast session, rx_chk would be _FALSE
11742                 //#ifdef CONFIG_INTEL_WIDI
11743                 //if (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_NONE)
11744                 //      rx_chk_limit = 1;
11745                 //#endif
11746
11747                 if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
11748                 {
11749                         bool is_p2p_enable = _FALSE;
11750                         #ifdef CONFIG_P2P
11751                         is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
11752                         #endif
11753
11754                         if (chk_ap_is_alive(padapter, psta) == _FALSE)
11755                                 rx_chk = _FAIL;
11756
11757                         if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
11758                                 tx_chk = _FAIL;
11759
11760                         #if defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && !defined(CONFIG_LPS_LCLK_WD_TIMER)
11761                         if (pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL)) {
11762                                 u8 backup_oper_channel=0;
11763
11764                                 /* switch to correct channel of current network  before issue keep-alive frames */
11765                                 if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
11766                                         backup_oper_channel = rtw_get_oper_ch(padapter);
11767                                         SelectChannel(padapter, pmlmeext->cur_channel);
11768                                 }
11769
11770                                 if (rx_chk != _SUCCESS)
11771                                         issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 0, 0, 3, 1);
11772
11773                                 if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit) || rx_chk != _SUCCESS) {
11774                                         tx_chk = issue_nulldata(padapter, psta->hwaddr, 0, 3, 1);
11775                                         /* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */
11776                                         if (tx_chk == _SUCCESS && !is_p2p_enable)
11777                                                 rx_chk = _SUCCESS;
11778                                 }
11779
11780                                 /* back to the original operation channel */
11781                                 if(backup_oper_channel>0)
11782                                         SelectChannel(padapter, backup_oper_channel);
11783
11784                         }
11785                         else
11786                         #endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */
11787                         {
11788                                 if (rx_chk != _SUCCESS) {
11789                                         if (pmlmeext->retry == 0) {
11790                                                 #ifdef DBG_EXPIRATION_CHK
11791                                                 DBG_871X("issue_probereq to trigger probersp, retry=%d\n", pmlmeext->retry);
11792                                                 #endif
11793                                                 issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
11794                                                 issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
11795                                                 issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
11796                                         }
11797                                 }
11798
11799                                 if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit) {
11800                                         #ifdef DBG_EXPIRATION_CHK
11801                                         DBG_871X("%s issue_nulldata(%d)\n", __FUNCTION__, from_timer?1:0);
11802                                         #endif
11803                                         tx_chk = issue_nulldata_in_interrupt(padapter, NULL, from_timer?1:0);
11804                                 }
11805                         }
11806
11807                         if (rx_chk == _FAIL) {
11808                                 pmlmeext->retry++;
11809                                 if (pmlmeext->retry > rx_chk_limit) {
11810                                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n",
11811                                                 FUNC_ADPT_ARG(padapter));
11812                                         receive_disconnect(padapter, pmlmeinfo->network.MacAddress
11813                                                 , WLAN_REASON_EXPIRATION_CHK);
11814                                         return;
11815                                 }
11816                         } else {
11817                                 pmlmeext->retry = 0;
11818                         }
11819
11820                         if (tx_chk == _FAIL) {
11821                                 pmlmeinfo->link_count %= (link_count_limit+1);
11822                         } else {
11823                                 pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
11824                                 pmlmeinfo->link_count = 0;
11825                         }
11826
11827                 } //end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL)
11828
11829 #if defined (CONFIG_TDLS) && defined (CONFIG_TDLS_AUTOCHECKALIVE)
11830                 linked_status_chk_tdls(padapter);
11831 #endif //CONFIG_TDLS
11832
11833         }
11834         else if (is_client_associated_to_ibss(padapter))
11835         {
11836                 //linked IBSS mode
11837                 //for each assoc list entry to check the rx pkt counter
11838                 for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
11839                 {
11840                         if (pmlmeinfo->FW_sta_info[i].status == 1)
11841                         {
11842                                 psta = pmlmeinfo->FW_sta_info[i].psta;
11843
11844                                 if(NULL==psta) continue;
11845
11846                                 if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta))
11847                                 {
11848
11849                                         if(pmlmeinfo->FW_sta_info[i].retry<3)
11850                                         {
11851                                                 pmlmeinfo->FW_sta_info[i].retry++;
11852                                         }
11853                                         else
11854                                         {
11855                                                 pmlmeinfo->FW_sta_info[i].retry = 0;
11856                                                 pmlmeinfo->FW_sta_info[i].status = 0;
11857                                                 report_del_sta_event(padapter, psta->hwaddr
11858                                                         , 65535// indicate disconnect caused by no rx
11859                                                 );
11860                                         }
11861                                 }
11862                                 else
11863                                 {
11864                                         pmlmeinfo->FW_sta_info[i].retry = 0;
11865                                         pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
11866                                 }
11867                         }
11868                 }
11869
11870                 //set_link_timer(pmlmeext, DISCONNECT_TO);
11871
11872         }
11873
11874 }
11875
11876 void survey_timer_hdl(_adapter *padapter)
11877 {
11878         struct cmd_obj  *ph2c;
11879         struct sitesurvey_parm  *psurveyPara;
11880         struct cmd_priv                                 *pcmdpriv=&padapter->cmdpriv;
11881         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
11882 #ifdef CONFIG_P2P
11883         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
11884 #endif
11885
11886         //DBG_871X("marc: survey timer\n");
11887
11888         //issue rtw_sitesurvey_cmd
11889         if (pmlmeext->sitesurvey_res.state > SCAN_START)
11890         {
11891                 if(pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
11892                 {
11893 #if defined(CONFIG_STA_MODE_SCAN_UNDER_AP_MODE) || defined(CONFIG_ATMEL_RC_PATCH)
11894                         if( padapter->mlmeextpriv.mlmext_info.scan_cnt != RTW_SCAN_NUM_OF_CH )
11895 #endif
11896                                 pmlmeext->sitesurvey_res.channel_idx++;
11897                 }
11898
11899                 if(pmlmeext->scan_abort == _TRUE)
11900                 {
11901                         #ifdef CONFIG_P2P
11902                         if(!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE))
11903                         {
11904                                 rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
11905                                 pmlmeext->sitesurvey_res.channel_idx = 3;
11906                                 DBG_871X("%s idx:%d, cnt:%u\n", __FUNCTION__
11907                                         , pmlmeext->sitesurvey_res.channel_idx
11908                                         , pwdinfo->find_phase_state_exchange_cnt
11909                                 );
11910                         }
11911                         else
11912                         #endif
11913                         {
11914                                 pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
11915                                 DBG_871X("%s idx:%d\n", __FUNCTION__
11916                                         , pmlmeext->sitesurvey_res.channel_idx
11917                                 );
11918                         }
11919
11920                         pmlmeext->scan_abort = _FALSE;//reset
11921                 }
11922
11923                 if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
11924                 {
11925                         goto exit_survey_timer_hdl;
11926                 }
11927
11928                 if ((psurveyPara = (struct sitesurvey_parm*)rtw_zmalloc(sizeof(struct sitesurvey_parm))) == NULL)
11929                 {
11930                         rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
11931                         goto exit_survey_timer_hdl;
11932                 }
11933
11934                 init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
11935                 rtw_enqueue_cmd(pcmdpriv, ph2c);
11936         }
11937
11938
11939 exit_survey_timer_hdl:
11940
11941         return;
11942 }
11943
11944 void link_timer_hdl(_adapter *padapter)
11945 {
11946         //static unsigned int           rx_pkt = 0;
11947         //static u64                            tx_cnt = 0;
11948         //struct xmit_priv              *pxmitpriv = &(padapter->xmitpriv);
11949         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11950         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11951         //struct sta_priv               *pstapriv = &padapter->stapriv;
11952
11953
11954         if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
11955         {
11956                 DBG_871X("link_timer_hdl:no beacon while connecting\n");
11957                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
11958                 report_join_res(padapter, -3);
11959         }
11960         else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE)
11961         {
11962                 //re-auth timer
11963                 if (++pmlmeinfo->reauth_count > REAUTH_LIMIT)
11964                 {
11965                         //if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto)
11966                         //{
11967                                 pmlmeinfo->state = 0;
11968                                 report_join_res(padapter, -1);
11969                                 return;
11970                         //}
11971                         //else
11972                         //{
11973                         //      pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
11974                         //      pmlmeinfo->reauth_count = 0;
11975                         //}
11976                 }
11977
11978                 DBG_871X("link_timer_hdl: auth timeout and try again\n");
11979                 pmlmeinfo->auth_seq = 1;
11980                 issue_auth(padapter, NULL, 0);
11981                 set_link_timer(pmlmeext, REAUTH_TO);
11982         }
11983         else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)
11984         {
11985                 //re-assoc timer
11986                 if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT)
11987                 {
11988                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
11989                         report_join_res(padapter, -2);
11990                         return;
11991                 }
11992
11993                 DBG_871X("link_timer_hdl: assoc timeout and try again\n");
11994                 issue_assocreq(padapter);
11995                 set_link_timer(pmlmeext, REASSOC_TO);
11996         }
11997 #if 0
11998         else if (is_client_associated_to_ap(padapter))
11999         {
12000                 //linked infrastructure client mode
12001                 if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
12002                 {
12003                         /*to monitor whether the AP is alive or not*/
12004                         if (rx_pkt == psta->sta_stats.rx_pkts)
12005                         {
12006                                 receive_disconnect(padapter, pmlmeinfo->network.MacAddress);
12007                                 return;
12008                         }
12009                         else
12010                         {
12011                                 rx_pkt = psta->sta_stats.rx_pkts;
12012                                 set_link_timer(pmlmeext, DISCONNECT_TO);
12013                         }
12014
12015                         //update the EDCA paramter according to the Tx/RX mode
12016                         update_EDCA_param(padapter);
12017
12018                         /*to send the AP a nulldata if no frame is xmitted in order to keep alive*/
12019                         if (pmlmeinfo->link_count++ == 0)
12020                         {
12021                                 tx_cnt = pxmitpriv->tx_pkts;
12022                         }
12023                         else if ((pmlmeinfo->link_count & 0xf) == 0)
12024                         {
12025                                 if (tx_cnt == pxmitpriv->tx_pkts)
12026                                 {
12027                                         issue_nulldata_in_interrupt(padapter, NULL, 0);
12028                                 }
12029
12030                                 tx_cnt = pxmitpriv->tx_pkts;
12031                         }
12032                 } //end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL)
12033         }
12034         else if (is_client_associated_to_ibss(padapter))
12035         {
12036                 //linked IBSS mode
12037                 //for each assoc list entry to check the rx pkt counter
12038                 for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
12039                 {
12040                         if (pmlmeinfo->FW_sta_info[i].status == 1)
12041                         {
12042                                 psta = pmlmeinfo->FW_sta_info[i].psta;
12043
12044                                 if (pmlmeinfo->FW_sta_info[i].rx_pkt == psta->sta_stats.rx_pkts)
12045                                 {
12046                                         pmlmeinfo->FW_sta_info[i].status = 0;
12047                                         report_del_sta_event(padapter, psta->hwaddr);
12048                                 }
12049                                 else
12050                                 {
12051                                         pmlmeinfo->FW_sta_info[i].rx_pkt = psta->sta_stats.rx_pkts;
12052                                 }
12053                         }
12054                 }
12055
12056                 set_link_timer(pmlmeext, DISCONNECT_TO);
12057         }
12058 #endif
12059
12060         return;
12061 }
12062
12063 void addba_timer_hdl(struct sta_info *psta)
12064 {
12065 #ifdef CONFIG_80211N_HT
12066         struct ht_priv  *phtpriv;
12067
12068         if(!psta)
12069                 return;
12070
12071         phtpriv = &psta->htpriv;
12072
12073         if((phtpriv->ht_option==_TRUE) && (phtpriv->ampdu_enable==_TRUE))
12074         {
12075                 if(phtpriv->candidate_tid_bitmap)
12076                         phtpriv->candidate_tid_bitmap=0x0;
12077
12078         }
12079 #endif //CONFIG_80211N_HT
12080 }
12081
12082 #ifdef CONFIG_IEEE80211W
12083 void sa_query_timer_hdl(_adapter *padapter)
12084 {
12085         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12086         struct mlme_priv * pmlmepriv = &padapter->mlmepriv;
12087         _irqL irqL;
12088         //disconnect
12089         _enter_critical_bh(&pmlmepriv->lock, &irqL);
12090
12091         if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
12092         {
12093                 rtw_disassoc_cmd(padapter, 0, _TRUE);
12094                 rtw_indicate_disconnect(padapter);
12095                 rtw_free_assoc_resources(padapter, 1);
12096         }
12097
12098         _exit_critical_bh(&pmlmepriv->lock, &irqL);
12099         DBG_871X("SA query timeout disconnect\n");
12100 }
12101 #endif //CONFIG_IEEE80211W
12102
12103 u8 NULL_hdl(_adapter *padapter, u8 *pbuf)
12104 {
12105         return H2C_SUCCESS;
12106 }
12107
12108 #ifdef CONFIG_AUTO_AP_MODE
12109 void rtw_start_auto_ap(_adapter *adapter)
12110 {
12111         DBG_871X("%s\n", __FUNCTION__);
12112
12113         rtw_set_802_11_infrastructure_mode(adapter, Ndis802_11APMode);
12114
12115         rtw_setopmode_cmd(adapter, Ndis802_11APMode,_TRUE);
12116 }
12117
12118 static int rtw_auto_ap_start_beacon(_adapter *adapter)
12119 {
12120         int ret=0;
12121         u8 *pbuf = NULL;
12122         uint len;
12123         u8      supportRate[16];
12124         int     sz = 0, rateLen;
12125         u8 *    ie;
12126         u8      wireless_mode, oper_channel;
12127         u8 ssid[3] = {0}; //hidden ssid
12128         u32 ssid_len = sizeof(ssid);
12129         struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
12130
12131
12132         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) != _TRUE)
12133                 return -EINVAL;
12134
12135
12136         len = 128;
12137         pbuf = rtw_zmalloc(len);
12138         if(!pbuf)
12139                 return -ENOMEM;
12140
12141
12142         //generate beacon
12143         ie = pbuf;
12144
12145         //timestamp will be inserted by hardware
12146         sz += 8;
12147         ie += sz;
12148
12149         //beacon interval : 2bytes
12150         *(u16*)ie = cpu_to_le16((u16)100);//BCN_INTERVAL=100;
12151         sz += 2;
12152         ie += 2;
12153
12154         //capability info
12155         *(u16*)ie = 0;
12156         *(u16*)ie |= cpu_to_le16(cap_ESS);
12157         *(u16*)ie |= cpu_to_le16(cap_ShortPremble);
12158         //*(u16*)ie |= cpu_to_le16(cap_Privacy);
12159         sz += 2;
12160         ie += 2;
12161
12162         //SSID
12163         ie = rtw_set_ie(ie, _SSID_IE_, ssid_len, ssid, &sz);
12164
12165         //supported rates
12166         wireless_mode = WIRELESS_11BG_24N;
12167         rtw_set_supported_rate(supportRate, wireless_mode) ;
12168         rateLen = rtw_get_rateset_len(supportRate);
12169         if (rateLen > 8)
12170         {
12171                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, supportRate, &sz);
12172         }
12173         else
12174         {
12175                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, supportRate, &sz);
12176         }
12177
12178
12179         //DS parameter set
12180         if(check_buddy_fwstate(adapter, _FW_LINKED) &&
12181                 check_buddy_fwstate(adapter, WIFI_STATION_STATE))
12182         {
12183                 PADAPTER pbuddy_adapter = adapter->pbuddy_adapter;
12184                 struct mlme_ext_priv *pbuddy_mlmeext  = &pbuddy_adapter->mlmeextpriv;
12185
12186                 oper_channel = pbuddy_mlmeext->cur_channel;
12187         }
12188         else
12189         {
12190                 oper_channel = adapter_to_dvobj(adapter)->oper_channel;
12191         }
12192         ie = rtw_set_ie(ie, _DSSET_IE_, 1, &oper_channel, &sz);
12193
12194         //ext supported rates
12195         if (rateLen > 8)
12196         {
12197                 ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (supportRate + 8), &sz);
12198         }
12199
12200         DBG_871X("%s, start auto ap beacon sz=%d\n", __FUNCTION__, sz);
12201
12202         //lunch ap mode & start to issue beacon
12203         if(rtw_check_beacon_data(adapter, pbuf,  sz) == _SUCCESS)
12204         {
12205
12206         }
12207         else
12208         {
12209                 ret = -EINVAL;
12210         }
12211
12212
12213         rtw_mfree(pbuf, len);
12214
12215         return ret;
12216
12217 }
12218 #endif//CONFIG_AUTO_AP_MODE
12219
12220 u8 setopmode_hdl(_adapter *padapter, u8 *pbuf)
12221 {
12222         u8      type;
12223         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12224         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12225         struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
12226
12227         if(psetop->mode == Ndis802_11APMode)
12228         {
12229                 pmlmeinfo->state = WIFI_FW_AP_STATE;
12230                 type = _HW_STATE_AP_;
12231 #ifdef CONFIG_NATIVEAP_MLME
12232                 //start_ap_mode(padapter);
12233 #endif
12234         }
12235         else if(psetop->mode == Ndis802_11Infrastructure)
12236         {
12237                 pmlmeinfo->state &= ~(BIT(0)|BIT(1));// clear state
12238                 pmlmeinfo->state |= WIFI_FW_STATION_STATE;//set to      STATION_STATE
12239                 type = _HW_STATE_STATION_;
12240         }
12241         else if(psetop->mode == Ndis802_11IBSS)
12242         {
12243                 type = _HW_STATE_ADHOC_;
12244         }
12245         else
12246         {
12247                 type = _HW_STATE_NOLINK_;
12248         }
12249
12250         rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
12251         //Set_NETYPE0_MSR(padapter, type);
12252
12253
12254 #ifdef CONFIG_AUTO_AP_MODE
12255         if(psetop->mode == Ndis802_11APMode)
12256                 rtw_auto_ap_start_beacon(padapter);
12257 #endif
12258
12259         if (rtw_port_switch_chk(padapter) == _TRUE)
12260         {
12261                 rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
12262
12263                 if(psetop->mode == Ndis802_11APMode)
12264                         adapter_to_pwrctl(padapter)->fw_psmode_iface_id = 0xff; //ap mode won't dowload rsvd pages
12265                 else if (psetop->mode == Ndis802_11Infrastructure) {
12266                         #ifdef CONFIG_LPS
12267                         _adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter));
12268                         if (port0_iface)
12269                                 rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0);
12270                         #endif
12271                 }
12272         }
12273
12274 #ifdef CONFIG_BT_COEXIST
12275         if (psetop->mode == Ndis802_11APMode)
12276         {
12277                 // Do this after port switch to
12278                 // prevent from downloading rsvd page to wrong port
12279                 rtw_btcoex_MediaStatusNotify(padapter, 1); //connect
12280         }
12281 #endif // CONFIG_BT_COEXIST
12282
12283         return H2C_SUCCESS;
12284
12285 }
12286
12287 u8 createbss_hdl(_adapter *padapter, u8 *pbuf)
12288 {
12289         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12290         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12291         WLAN_BSSID_EX   *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
12292         struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
12293         //u32   initialgain;
12294
12295 #ifdef CONFIG_AP_MODE
12296         if (pmlmeinfo->state == WIFI_FW_AP_STATE) {
12297                 WLAN_BSSID_EX *network = &padapter->mlmepriv.cur_network.network;
12298                 start_bss_network(padapter, (u8*)network);
12299                 return H2C_SUCCESS;
12300         }
12301 #endif
12302
12303         //below is for ad-hoc master
12304         if(pparm->network.InfrastructureMode == Ndis802_11IBSS)
12305         {
12306                 rtw_joinbss_reset(padapter);
12307
12308                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
12309                 pmlmeext->cur_ch_offset= HAL_PRIME_CHNL_OFFSET_DONT_CARE;
12310                 pmlmeinfo->ERP_enable = 0;
12311                 pmlmeinfo->WMM_enable = 0;
12312                 pmlmeinfo->HT_enable = 0;
12313                 pmlmeinfo->HT_caps_enable = 0;
12314                 pmlmeinfo->HT_info_enable = 0;
12315                 pmlmeinfo->agg_enable_bitmap = 0;
12316                 pmlmeinfo->candidate_tid_bitmap = 0;
12317
12318                 //config the initial gain under linking, need to write the BB registers
12319                 //initialgain = 0x1E;
12320                 //rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
12321
12322                 //disable dynamic functions, such as high power, DIG
12323                 Save_DM_Func_Flag(padapter);
12324                 Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
12325
12326                 //cancel link timer
12327                 _cancel_timer_ex(&pmlmeext->link_timer);
12328
12329                 //clear CAM
12330                 flush_all_cam_entry(padapter);
12331
12332                 _rtw_memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength));
12333                 pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength;
12334
12335                 if(pnetwork->IELength>MAX_IE_SZ)//Check pbuf->IELength
12336                         return H2C_PARAMETERS_ERROR;
12337
12338                 _rtw_memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength);
12339
12340                 start_create_ibss(padapter);
12341
12342         }
12343
12344         return H2C_SUCCESS;
12345
12346 }
12347
12348 u8 join_cmd_hdl(_adapter *padapter, u8 *pbuf)
12349 {
12350         u8      join_type;
12351         PNDIS_802_11_VARIABLE_IEs       pIE;
12352         struct registry_priv    *pregpriv = &padapter->registrypriv;
12353         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12354         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12355         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
12356 #ifdef CONFIG_ANTENNA_DIVERSITY
12357         struct joinbss_parm     *pparm = (struct joinbss_parm *)pbuf;
12358 #endif //CONFIG_ANTENNA_DIVERSITY
12359         u32 i;
12360         u8      cbw40_enable=0;
12361         //u32   initialgain;
12362         //u32   acparm;
12363         u8 ch, bw, offset;
12364
12365         //check already connecting to AP or not
12366         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
12367         {
12368                 if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
12369                 {
12370                         issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
12371                 }
12372                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
12373
12374                 //clear CAM
12375                 flush_all_cam_entry(padapter);
12376
12377                 _cancel_timer_ex(&pmlmeext->link_timer);
12378
12379                 //set MSR to nolink -> infra. mode
12380                 //Set_MSR(padapter, _HW_STATE_NOLINK_);
12381                 Set_MSR(padapter, _HW_STATE_STATION_);
12382
12383
12384                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
12385         }
12386
12387 #ifdef CONFIG_ANTENNA_DIVERSITY
12388         rtw_antenna_select_cmd(padapter, pparm->network.PhyInfo.Optimum_antenna, _FALSE);
12389 #endif
12390
12391 #ifdef CONFIG_WAPI_SUPPORT
12392         rtw_wapi_clear_all_cam_entry(padapter);
12393 #endif
12394
12395         rtw_joinbss_reset(padapter);
12396
12397         pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
12398         pmlmeext->cur_ch_offset= HAL_PRIME_CHNL_OFFSET_DONT_CARE;
12399         pmlmeinfo->ERP_enable = 0;
12400         pmlmeinfo->WMM_enable = 0;
12401         pmlmeinfo->HT_enable = 0;
12402         pmlmeinfo->HT_caps_enable = 0;
12403         pmlmeinfo->HT_info_enable = 0;
12404         pmlmeinfo->agg_enable_bitmap = 0;
12405         pmlmeinfo->candidate_tid_bitmap = 0;
12406         pmlmeinfo->bwmode_updated = _FALSE;
12407         //pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX;
12408         pmlmeinfo->VHT_enable = 0;
12409
12410         _rtw_memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength));
12411         pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength;
12412
12413         if(pnetwork->IELength>MAX_IE_SZ)//Check pbuf->IELength
12414                 return H2C_PARAMETERS_ERROR;
12415
12416         _rtw_memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength);
12417
12418         pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
12419         pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
12420
12421         //Check AP vendor to move rtw_joinbss_cmd()
12422         //pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->IEs, pnetwork->IELength);
12423
12424         //sizeof(NDIS_802_11_FIXED_IEs)
12425         for (i = _FIXED_IE_LENGTH_; i < pnetwork->IELength;)
12426         {
12427                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pnetwork->IEs + i);
12428
12429                 switch (pIE->ElementID)
12430                 {
12431                         case _VENDOR_SPECIFIC_IE_://Get WMM IE.
12432                                 if ( _rtw_memcmp(pIE->data, WMM_OUI, 4) )
12433                                 {
12434                                         WMM_param_handler(padapter, pIE);
12435                                 }
12436                                 break;
12437
12438                         case _HT_CAPABILITY_IE_:        //Get HT Cap IE.
12439                                 pmlmeinfo->HT_caps_enable = 1;
12440                                 break;
12441
12442                         case _HT_EXTRA_INFO_IE_:        //Get HT Info IE.
12443 #ifdef CONFIG_80211N_HT
12444                                 pmlmeinfo->HT_info_enable = 1;
12445
12446                                 //spec case only for cisco's ap because cisco's ap issue assoc rsp using mcs rate @40MHz or @20MHz
12447 //#if !defined(CONFIG_CONCURRENT_MODE) && !defined(CONFIG_DUALMAC_CONCURRENT)
12448 //                              if(pmlmeinfo->assoc_AP_vendor == ciscoAP)
12449 //#endif
12450                                 {
12451                                         struct HT_info_element *pht_info = (struct HT_info_element *)(pIE->data);
12452
12453                                         if (pnetwork->Configuration.DSConfig > 14) {
12454                                                 if ((pregpriv->bw_mode >> 4) > CHANNEL_WIDTH_20)
12455                                                         cbw40_enable = 1;
12456                                         } else {
12457                                                 if ((pregpriv->bw_mode & 0x0f) > CHANNEL_WIDTH_20)
12458                                                         cbw40_enable = 1;
12459                                         }
12460
12461                                         if ((cbw40_enable) && (pht_info->infos[0] & BIT(2)))
12462                                         {
12463                                                 //switch to the 40M Hz mode according to the AP
12464                                                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_40;
12465                                                 switch (pht_info->infos[0] & 0x3)
12466                                                 {
12467                                                         case 1:
12468                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
12469                                                                 break;
12470
12471                                                         case 3:
12472                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
12473                                                                 break;
12474
12475                                                         default:
12476                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
12477                                                                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
12478                                                                 break;
12479                                                 }
12480
12481                                                 DBG_871X("set HT ch/bw before connected\n");
12482                                         }
12483                                 }
12484 #endif //CONFIG_80211N_HT
12485                                 break;
12486 #ifdef CONFIG_80211AC_VHT
12487                         case EID_VHTCapability://Get VHT Cap IE.
12488                                 pmlmeinfo->VHT_enable = 1;
12489                                 break;
12490
12491                         case EID_VHTOperation://Get VHT Operation IE.
12492                                 if((GET_VHT_OPERATION_ELE_CHL_WIDTH(pIE->data) >= 1)
12493                                         && ((pregpriv->bw_mode >> 4) >= CHANNEL_WIDTH_80)) {
12494                                         pmlmeext->cur_bwmode = CHANNEL_WIDTH_80;
12495                                         DBG_871X("VHT center ch = %d\n", GET_VHT_OPERATION_ELE_CENTER_FREQ1(pIE->data));
12496                                         DBG_871X("set VHT ch/bw before connected\n");
12497                                 }
12498                                 break;
12499 #endif
12500                         default:
12501                                 break;
12502                 }
12503
12504                 i += (pIE->Length + 2);
12505         }
12506 #if 0
12507         if (padapter->registrypriv.wifi_spec) {
12508                 // for WiFi test, follow WMM test plan spec
12509                 acparm = 0x002F431C; // VO
12510                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
12511                 acparm = 0x005E541C; // VI
12512                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
12513                 acparm = 0x0000A525; // BE
12514                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
12515                 acparm = 0x0000A549; // BK
12516                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
12517
12518                 // for WiFi test, mixed mode with intel STA under bg mode throughput issue
12519                 if (padapter->mlmepriv.htpriv.ht_option == _FALSE){
12520                         acparm = 0x00004320;
12521                         rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
12522                 }
12523         }
12524         else {
12525                 acparm = 0x002F3217; // VO
12526                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
12527                 acparm = 0x005E4317; // VI
12528                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
12529                 acparm = 0x00105320; // BE
12530                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
12531                 acparm = 0x0000A444; // BK
12532                 rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
12533         }
12534 #endif
12535
12536         /* check channel, bandwidth, offset and switch */
12537 #ifdef CONFIG_DUALMAC_CONCURRENT
12538         if(dc_handle_join_request(padapter, &ch, &bw, &offset) == _FAIL) {
12539                 DBG_871X("dc_handle_join_request fail !!!\n");
12540                 return H2C_SUCCESS;
12541         }
12542 #else //NON CONFIG_DUALMAC_CONCURRENT
12543         if(rtw_chk_start_clnt_join(padapter, &ch, &bw, &offset) == _FAIL) {
12544                 report_join_res(padapter, (-4));
12545                 return H2C_SUCCESS;
12546         }
12547 #endif
12548
12549         //disable dynamic functions, such as high power, DIG
12550         //Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
12551
12552         //config the initial gain under linking, need to write the BB registers
12553         //initialgain = 0x1E;
12554         //rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
12555
12556         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
12557         join_type = 0;
12558         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
12559         rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
12560
12561         set_channel_bwmode(padapter, ch, offset, bw);
12562
12563         //cancel link timer
12564         _cancel_timer_ex(&pmlmeext->link_timer);
12565
12566         start_clnt_join(padapter);
12567
12568         return H2C_SUCCESS;
12569
12570 }
12571
12572 u8 disconnect_hdl(_adapter *padapter, unsigned char *pbuf)
12573 {
12574         struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
12575         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12576         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12577         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
12578         u8 val8;
12579
12580         if (is_client_associated_to_ap(padapter))
12581         {
12582 #ifdef CONFIG_DFS
12583                 if(padapter->mlmepriv.handle_dfs == _FALSE)
12584 #endif //CONFIG_DFS
12585 #ifdef CONFIG_PLATFORM_ROCKCHIPS
12586                         //To avoid connecting to AP fail during resume process, change retry count from 5 to 1
12587                         issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
12588 #else
12589                         issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100);
12590 #endif //CONFIG_PLATFORM_ROCKCHIPS
12591         }
12592
12593 #ifdef CONFIG_DFS
12594         if( padapter->mlmepriv.handle_dfs == _TRUE )
12595                 padapter->mlmepriv.handle_dfs = _FALSE;
12596 #endif //CONFIG_DFS
12597
12598         if(((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
12599         {
12600                 //Stop BCN
12601                 val8 = 0;
12602                 rtw_hal_set_hwreg(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8));
12603         }
12604
12605         rtw_mlmeext_disconnect(padapter);
12606
12607         rtw_free_uc_swdec_pending_queue(padapter);
12608
12609         return  H2C_SUCCESS;
12610 }
12611
12612 int rtw_scan_ch_decision(_adapter *padapter, struct rtw_ieee80211_channel *out,
12613         u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
12614 {
12615         int i, j;
12616         int scan_ch_num = 0;
12617         int set_idx;
12618         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12619
12620         /* clear first */
12621         _rtw_memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num);
12622
12623         /* acquire channels from in */
12624         j = 0;
12625         for (i=0;i<in_num;i++) {
12626
12627                 if (0)
12628                 DBG_871X(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[i]));
12629
12630                 if(in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED)
12631                         && (set_idx=rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value)) >=0
12632                         && rtw_mlme_band_check(padapter, in[i].hw_value) == _TRUE
12633                 )
12634                 {
12635                         if (j >= out_num) {
12636                                 DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" out_num:%u not enough\n",
12637                                         FUNC_ADPT_ARG(padapter), out_num);
12638                                 break;
12639                         }
12640
12641                         _rtw_memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel));
12642
12643                         if(pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
12644                                 out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
12645
12646                         j++;
12647                 }
12648                 if(j>=out_num)
12649                         break;
12650         }
12651
12652         /* if out is empty, use channel_set as default */
12653         if(j == 0) {
12654                 for (i=0;i<pmlmeext->max_chan_nums;i++) {
12655
12656                         if (0)
12657                         DBG_871X(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter), pmlmeext->channel_set[i].ChannelNum);
12658
12659                         if (rtw_mlme_band_check(padapter, pmlmeext->channel_set[i].ChannelNum) == _TRUE) {
12660
12661                                 if (j >= out_num) {
12662                                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" out_num:%u not enough\n",
12663                                                 FUNC_ADPT_ARG(padapter), out_num);
12664                                         break;
12665                                 }
12666
12667                                 out[j].hw_value = pmlmeext->channel_set[i].ChannelNum;
12668
12669                                 if(pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
12670                                         out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
12671
12672                                 j++;
12673                         }
12674                 }
12675         }
12676
12677 #ifdef CONFIG_SCAN_SPARSE //partial scan, ASUS RK3188 use the feature
12678         /* assume j>6 is normal scan */
12679         if ((j > 6) && (padapter->registrypriv.wifi_spec != 1))
12680         {
12681                 static u8 token = 0;
12682                 u32 interval;
12683
12684                 if (pmlmeext->last_scan_time == 0)
12685                         pmlmeext->last_scan_time = rtw_get_current_time();
12686
12687                 interval = rtw_get_passing_time_ms(pmlmeext->last_scan_time);
12688                 if ((interval > ALLOW_SCAN_INTERVAL)
12689 #if 0 // Miracast can't do AP scan
12690                         || (padapter->mlmepriv.LinkDetectInfo.bBusyTraffic == _TRUE)
12691 #ifdef CONFIG_CONCURRENT_MODE
12692                         || (padapter->pbuddy_adapter
12693                                 && (padapter->pbuddy_adapter->mlmepriv.LinkDetectInfo.bBusyTraffic == _TRUE))
12694 #endif // CONFIG_CONCURRENT_MODE
12695 #endif
12696                         )
12697                 {
12698                         // modify scan plan
12699                         int k = 0;
12700                         _rtw_memset(in, 0, sizeof(struct rtw_ieee80211_channel)*in_num);
12701                         _rtw_memcpy(in, out, sizeof(struct rtw_ieee80211_channel)*j);
12702                         _rtw_memset(out, 0, sizeof(struct rtw_ieee80211_channel)*j);
12703
12704                         for (i=0;i<j;i++) {
12705                                 if (in[i].hw_value && (i%SCAN_DIVISION_NUM) == token) {
12706                                         _rtw_memcpy(&out[k], &in[i], sizeof(struct rtw_ieee80211_channel));
12707                                         k++;
12708                                 }
12709                                 if(k>=out_num)
12710                                         break;
12711                         }
12712
12713                         j = k;
12714                         token  = (token+1)%SCAN_DIVISION_NUM;
12715                 }
12716
12717                 pmlmeext->last_scan_time = rtw_get_current_time();
12718         }
12719 #endif //CONFIG_SCAN_SPARSE
12720
12721         return j;
12722 }
12723
12724 u8 sitesurvey_cmd_hdl(_adapter *padapter, u8 *pbuf)
12725 {
12726         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12727         struct sitesurvey_parm  *pparm = (struct sitesurvey_parm *)pbuf;
12728         u8      bdelayscan = _FALSE;
12729         u8      val8;
12730         u32     initialgain;
12731         u32     i;
12732         struct dvobj_priv *psdpriv = padapter->dvobj;
12733         struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
12734
12735 #ifdef CONFIG_P2P
12736         struct wifidirect_info* pwdinfo = &padapter->wdinfo;
12737 #endif
12738 #ifdef DBG_CHECK_FW_PS_STATE
12739         if(rtw_fw_ps_state(padapter) == _FAIL)
12740         {
12741                 DBG_871X("scan without leave 32k\n");
12742                 pdbgpriv->dbg_scan_pwr_state_cnt++;
12743         }
12744 #endif //DBG_CHECK_FW_PS_STATE
12745
12746         if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE)
12747         {
12748 #ifdef CONFIG_CONCURRENT_MODE
12749                 //for first time sitesurvey_cmd
12750                 rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
12751 #endif //CONFIG_CONCURRENT_MODE
12752
12753                 pmlmeext->sitesurvey_res.state = SCAN_START;
12754                 pmlmeext->sitesurvey_res.bss_cnt = 0;
12755                 pmlmeext->sitesurvey_res.channel_idx = 0;
12756
12757                 for(i=0;i<RTW_SSID_SCAN_AMOUNT;i++){
12758                         if(pparm->ssid[i].SsidLength) {
12759                                 _rtw_memcpy(pmlmeext->sitesurvey_res.ssid[i].Ssid, pparm->ssid[i].Ssid, IW_ESSID_MAX_SIZE);
12760                                 pmlmeext->sitesurvey_res.ssid[i].SsidLength= pparm->ssid[i].SsidLength;
12761                         } else {
12762                                 pmlmeext->sitesurvey_res.ssid[i].SsidLength= 0;
12763                         }
12764                 }
12765
12766                 pmlmeext->sitesurvey_res.ch_num = rtw_scan_ch_decision(padapter
12767                         , pmlmeext->sitesurvey_res.ch, RTW_CHANNEL_SCAN_AMOUNT
12768                         , pparm->ch, pparm->ch_num
12769                 );
12770
12771                 pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
12772
12773 #ifdef CONFIG_DUALMAC_CONCURRENT
12774                 bdelayscan = dc_handle_site_survey(padapter);
12775 #endif
12776
12777                 //issue null data if associating to the AP
12778                 if (is_client_associated_to_ap(padapter) == _TRUE)
12779                 {
12780                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
12781
12782                         issue_nulldata(padapter, NULL, 1, 3, 500);
12783
12784 #ifdef CONFIG_CONCURRENT_MODE
12785                         if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
12786                         {
12787                                 DBG_871X("adapter is scanning(buddy_adapter is linked), issue nulldata(pwrbit=1)\n");
12788
12789                                 issue_nulldata(padapter->pbuddy_adapter, NULL, 1, 3, 500);
12790                         }
12791 #endif
12792                         bdelayscan = _TRUE;
12793                 }
12794 #ifdef CONFIG_CONCURRENT_MODE
12795                 else if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
12796                 {
12797                         #ifdef CONFIG_TDLS
12798                         if(padapter->pbuddy_adapter->wdinfo.wfd_tdls_enable == 1)
12799                         {
12800                                 issue_tunneled_probe_req(padapter->pbuddy_adapter);
12801                         }
12802                         #endif //CONFIG_TDLS
12803
12804                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
12805
12806                         issue_nulldata(padapter->pbuddy_adapter, NULL, 1, 3, 500);
12807
12808                         bdelayscan = _TRUE;
12809                 }
12810 #endif
12811                 if(bdelayscan)
12812                 {
12813                         //delay 50ms to protect nulldata(1).
12814                         set_survey_timer(pmlmeext, 50);
12815                         return H2C_SUCCESS;
12816                 }
12817         }
12818
12819         if ((pmlmeext->sitesurvey_res.state == SCAN_START) || (pmlmeext->sitesurvey_res.state == SCAN_TXNULL))
12820         {
12821 #ifdef CONFIG_FIND_BEST_CHANNEL
12822 #if 0
12823                 for (i=0; pmlmeext->channel_set[i].ChannelNum !=0; i++) {
12824                         pmlmeext->channel_set[i].rx_count = 0;
12825                 }
12826 #endif
12827 #endif /* CONFIG_FIND_BEST_CHANNEL */
12828
12829                 //config the initial gain under scaning, need to write the BB registers
12830 #ifdef CONFIG_P2P
12831 #ifdef CONFIG_IOCTL_CFG80211
12832                 if(adapter_wdev_data(padapter)->p2p_enabled == _TRUE && pwdinfo->driver_interface == DRIVER_CFG80211 )
12833                         initialgain = 0x30;
12834                 else
12835 #endif //CONFIG_IOCTL_CFG80211
12836                 if ( !rtw_p2p_chk_state( pwdinfo, P2P_STATE_NONE ) )
12837                         initialgain = 0x28;
12838                 else
12839 #endif //CONFIG_P2P
12840                         initialgain = 0x1e;
12841
12842                 rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
12843
12844                 //disable dynamic functions, such as high power, DIG
12845                 Save_DM_Func_Flag(padapter);
12846                 Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
12847
12848                 //set MSR to no link state
12849                 Set_MSR(padapter, _HW_STATE_NOLINK_);
12850
12851                 val8 = 1; //under site survey
12852                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
12853
12854                 pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
12855         }
12856
12857         site_survey(padapter);
12858
12859         return H2C_SUCCESS;
12860
12861 }
12862
12863 u8 setauth_hdl(_adapter *padapter, unsigned char *pbuf)
12864 {
12865         struct setauth_parm             *pparm = (struct setauth_parm *)pbuf;
12866         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12867         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12868
12869         if (pparm->mode < 4)
12870         {
12871                 pmlmeinfo->auth_algo = pparm->mode;
12872         }
12873
12874         return  H2C_SUCCESS;
12875 }
12876
12877 u8 setkey_hdl(_adapter *padapter, u8 *pbuf)
12878 {
12879         u16     ctrl = 0;
12880         s16 cam_id = 0;
12881         struct setkey_parm              *pparm = (struct setkey_parm *)pbuf;
12882         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12883         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12884         unsigned char null_addr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
12885         u8 *addr;
12886
12887         //main tx key for wep.
12888         if(pparm->set_tx)
12889                 pmlmeinfo->key_index = pparm->keyid;
12890
12891         cam_id = rtw_camid_alloc(padapter, NULL, pparm->keyid);
12892
12893         if (cam_id < 0){
12894         } else {
12895                 if (cam_id > 3) /* not default key, searched by A2 */
12896                         addr = get_bssid(&padapter->mlmepriv);
12897                 else
12898                         addr = null_addr;
12899
12900                 ctrl = BIT(15) | BIT6 |((pparm->algorithm) << 2) | pparm->keyid;
12901                 write_cam(padapter, cam_id, ctrl, addr, pparm->key);
12902                 DBG_871X_LEVEL(_drv_always_, "set group key camid:%d, addr:"MAC_FMT", kid:%d, type:%s\n"
12903                         ,cam_id, MAC_ARG(addr), pparm->keyid, security_type_str(pparm->algorithm));
12904         }
12905
12906         #ifdef DYNAMIC_CAMID_ALLOC
12907         if (cam_id >=0 && cam_id <=3)
12908                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8*)_TRUE);
12909         #endif
12910
12911         //allow multicast packets to driver
12912         padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_ON_RCR_AM, null_addr);
12913
12914         return H2C_SUCCESS;
12915 }
12916
12917 u8 set_stakey_hdl(_adapter *padapter, u8 *pbuf)
12918 {
12919         u16 ctrl = 0;
12920         s16 cam_id = 0;
12921         u8 ret = H2C_SUCCESS;
12922         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12923         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12924         struct set_stakey_parm  *pparm = (struct set_stakey_parm *)pbuf;
12925         struct sta_priv *pstapriv = &padapter->stapriv;
12926         struct sta_info *psta;
12927 #ifdef CONFIG_TDLS
12928         struct tdls_info        *ptdlsinfo = &padapter->tdlsinfo;
12929 #endif //CONFIG_TDLS
12930
12931         if(pparm->algorithm == _NO_PRIVACY_)
12932                 goto write_to_cam;
12933
12934         psta = rtw_get_stainfo(pstapriv, pparm->addr);
12935         if (!psta) {
12936                 DBG_871X_LEVEL(_drv_always_, "%s sta:"MAC_FMT" not found\n", __func__, MAC_ARG(pparm->addr));
12937                 ret = H2C_REJECTED;
12938                 goto exit;
12939         }
12940
12941         pmlmeinfo->enc_algo = pparm->algorithm;
12942         cam_id = rtw_camid_alloc(padapter, psta, 0);
12943         if (cam_id < 0)
12944                 goto exit;
12945
12946 write_to_cam:
12947         if(pparm->algorithm == _NO_PRIVACY_) {
12948                 while((cam_id = rtw_camid_search(padapter, pparm->addr, -1)) >= 0) {
12949                         DBG_871X_LEVEL(_drv_always_, "clear key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(pparm->addr), cam_id);
12950                         clear_cam_entry(padapter, cam_id);
12951                         rtw_camid_free(padapter,cam_id);
12952                 }
12953         } else {
12954                 DBG_871X_LEVEL(_drv_always_, "set pairwise key camid:%d, addr:"MAC_FMT", kid:%d, type:%s\n",
12955                         cam_id, MAC_ARG(pparm->addr), pparm->keyid, security_type_str(pparm->algorithm));
12956                 ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;
12957                 write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
12958         }
12959         ret = H2C_SUCCESS_RSP;
12960
12961 exit:
12962         return ret;
12963 }
12964
12965 u8 add_ba_hdl(_adapter *padapter, unsigned char *pbuf)
12966 {
12967         struct addBaReq_parm    *pparm = (struct addBaReq_parm *)pbuf;
12968         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12969         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
12970
12971         struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr);
12972
12973         if(!psta)
12974                 return  H2C_SUCCESS;
12975
12976 #ifdef CONFIG_80211N_HT
12977         if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
12978                 ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
12979         {
12980                 //pmlmeinfo->ADDBA_retry_count = 0;
12981                 //pmlmeinfo->candidate_tid_bitmap |= (0x1 << pparm->tid);
12982                 //psta->htpriv.candidate_tid_bitmap |= BIT(pparm->tid);
12983                 issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
12984                 //_set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO);
12985                 _set_timer(&psta->addba_retry_timer, ADDBA_TO);
12986         }
12987 #ifdef CONFIG_TDLS
12988         else if((psta->tdls_sta_state & TDLS_LINKED_STATE)&&
12989                 (psta->htpriv.ht_option==_TRUE) &&
12990                 (psta->htpriv.ampdu_enable==_TRUE) )
12991         {
12992                 issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
12993                 //_set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO);
12994                 _set_timer(&psta->addba_retry_timer, ADDBA_TO);
12995         }
12996 #endif //CONFIG
12997         else
12998         {
12999                 psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
13000         }
13001 #endif //CONFIG_80211N_HT
13002         return  H2C_SUCCESS;
13003 }
13004
13005
13006 u8 chk_bmc_sleepq_cmd(_adapter* padapter)
13007 {
13008         struct cmd_obj *ph2c;
13009         struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
13010         u8 res = _SUCCESS;
13011
13012 _func_enter_;
13013
13014         if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
13015         {
13016                 res= _FAIL;
13017                 goto exit;
13018         }
13019
13020         init_h2fwcmd_w_parm_no_parm_rsp(ph2c, GEN_CMD_CODE(_ChkBMCSleepq));
13021
13022         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
13023
13024 exit:
13025
13026 _func_exit_;
13027
13028         return res;
13029 }
13030
13031 u8 set_tx_beacon_cmd(_adapter* padapter)
13032 {
13033         struct cmd_obj  *ph2c;
13034         struct Tx_Beacon_param  *ptxBeacon_parm;
13035         struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
13036         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13037         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13038         u8      res = _SUCCESS;
13039         int len_diff = 0;
13040
13041 _func_enter_;
13042
13043         if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
13044         {
13045                 res= _FAIL;
13046                 goto exit;
13047         }
13048
13049         if ((ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param))) == NULL)
13050         {
13051                 rtw_mfree((unsigned char *)ph2c, sizeof(struct  cmd_obj));
13052                 res= _FAIL;
13053                 goto exit;
13054         }
13055
13056         _rtw_memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
13057
13058         len_diff = update_hidden_ssid(
13059                 ptxBeacon_parm->network.IEs+_BEACON_IE_OFFSET_
13060                 , ptxBeacon_parm->network.IELength-_BEACON_IE_OFFSET_
13061                 , pmlmeinfo->hidden_ssid_mode
13062         );
13063         ptxBeacon_parm->network.IELength += len_diff;
13064
13065         init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
13066
13067         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
13068
13069
13070 exit:
13071
13072 _func_exit_;
13073
13074         return res;
13075 }
13076
13077
13078 u8 mlme_evt_hdl(_adapter *padapter, unsigned char *pbuf)
13079 {
13080         u8 evt_code, evt_seq;
13081         u16 evt_sz;
13082         uint    *peventbuf;
13083         void (*event_callback)(_adapter *dev, u8 *pbuf);
13084         struct evt_priv *pevt_priv = &(padapter->evtpriv);
13085
13086         if (pbuf == NULL)
13087                 goto _abort_event_;
13088
13089         peventbuf = (uint*)pbuf;
13090         evt_sz = (u16)(*peventbuf&0xffff);
13091         evt_seq = (u8)((*peventbuf>>24)&0x7f);
13092         evt_code = (u8)((*peventbuf>>16)&0xff);
13093
13094
13095         #ifdef CHECK_EVENT_SEQ
13096         // checking event sequence...
13097         if (evt_seq != (ATOMIC_READ(&pevt_priv->event_seq) & 0x7f) )
13098         {
13099                 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)));
13100
13101                 pevt_priv->event_seq = (evt_seq+1)&0x7f;
13102
13103                 goto _abort_event_;
13104         }
13105         #endif
13106
13107         // checking if event code is valid
13108         if (evt_code >= MAX_C2HEVT)
13109         {
13110                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nEvent Code(%d) mismatch!\n", evt_code));
13111                 goto _abort_event_;
13112         }
13113
13114         // checking if event size match the event parm size
13115         if ((wlanevents[evt_code].parmsize != 0) &&
13116                         (wlanevents[evt_code].parmsize != evt_sz))
13117         {
13118
13119                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
13120                         evt_code, wlanevents[evt_code].parmsize, evt_sz));
13121                 goto _abort_event_;
13122
13123         }
13124
13125         ATOMIC_INC(&pevt_priv->event_seq);
13126
13127         peventbuf += 2;
13128
13129         if(peventbuf)
13130         {
13131                 event_callback = wlanevents[evt_code].event_callback;
13132                 event_callback(padapter, (u8*)peventbuf);
13133
13134                 pevt_priv->evt_done_cnt++;
13135         }
13136
13137
13138 _abort_event_:
13139
13140
13141         return H2C_SUCCESS;
13142
13143 }
13144
13145 u8 h2c_msg_hdl(_adapter *padapter, unsigned char *pbuf)
13146 {
13147         if(!pbuf)
13148                 return H2C_PARAMETERS_ERROR;
13149
13150         return H2C_SUCCESS;
13151 }
13152
13153 u8 chk_bmc_sleepq_hdl(_adapter *padapter, unsigned char *pbuf)
13154 {
13155 #ifdef CONFIG_AP_MODE
13156         _irqL irqL;
13157         struct sta_info *psta_bmc;
13158         _list   *xmitframe_plist, *xmitframe_phead;
13159         struct xmit_frame *pxmitframe=NULL;
13160         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
13161         struct sta_priv  *pstapriv = &padapter->stapriv;
13162
13163         //for BC/MC Frames
13164         psta_bmc = rtw_get_bcmc_stainfo(padapter);
13165         if(!psta_bmc)
13166                 return H2C_SUCCESS;
13167
13168         if((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len>0))
13169         {
13170 #ifndef CONFIG_PCI_HCI
13171                 rtw_msleep_os(10);// 10ms, ATIM(HIQ) Windows
13172 #endif
13173                 //_enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
13174                 _enter_critical_bh(&pxmitpriv->lock, &irqL);
13175
13176                 xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
13177                 xmitframe_plist = get_next(xmitframe_phead);
13178
13179                 while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
13180                 {
13181                         pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
13182
13183                         xmitframe_plist = get_next(xmitframe_plist);
13184
13185                         rtw_list_delete(&pxmitframe->list);
13186
13187                         psta_bmc->sleepq_len--;
13188                         if(psta_bmc->sleepq_len>0)
13189                                 pxmitframe->attrib.mdata = 1;
13190                         else
13191                                 pxmitframe->attrib.mdata = 0;
13192
13193                         pxmitframe->attrib.triggered=1;
13194
13195                         if (xmitframe_hiq_filter(pxmitframe) == _TRUE)
13196                                 pxmitframe->attrib.qsel = QSLT_HIGH;//HIQ
13197
13198                         #if 0
13199                         _exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
13200                         if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE)
13201                         {
13202                                 rtw_os_xmit_complete(padapter, pxmitframe);
13203                         }
13204                         _enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
13205                         #endif
13206                         rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
13207                 }
13208
13209                 //_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
13210                 _exit_critical_bh(&pxmitpriv->lock, &irqL);
13211
13212                 if (padapter->interface_type != RTW_PCIE) {
13213                         /* check hi queue and bmc_sleepq */
13214                         rtw_chk_hi_queue_cmd(padapter);
13215                 }
13216         }
13217 #endif
13218
13219         return H2C_SUCCESS;
13220 }
13221
13222 u8 tx_beacon_hdl(_adapter *padapter, unsigned char *pbuf)
13223 {
13224         if(send_beacon(padapter)==_FAIL)
13225         {
13226                 DBG_871X("issue_beacon, fail!\n");
13227                 return H2C_PARAMETERS_ERROR;
13228         }
13229
13230         /* tx bc/mc frames after update TIM */
13231         chk_bmc_sleepq_hdl(padapter, NULL);
13232
13233         return H2C_SUCCESS;
13234 }
13235
13236 void change_band_update_ie(_adapter *padapter, WLAN_BSSID_EX *pnetwork)
13237 {
13238         u8      network_type,rate_len, total_rate_len,remainder_rate_len;
13239         struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
13240         struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
13241         u8      erpinfo=0x4;
13242
13243         //DBG_871X("%s\n", __FUNCTION__);
13244
13245         if(pmlmeext->cur_channel >= 36)
13246         {
13247                 network_type = WIRELESS_11A;
13248                 total_rate_len = IEEE80211_NUM_OFDM_RATESLEN;
13249                 DBG_871X("%s(): change to 5G Band\n",__FUNCTION__);
13250                 rtw_remove_bcn_ie(padapter, pnetwork, _ERPINFO_IE_);
13251         }
13252         else
13253         {
13254                 network_type = WIRELESS_11BG;
13255                 total_rate_len = IEEE80211_CCK_RATE_LEN+IEEE80211_NUM_OFDM_RATESLEN;
13256                 DBG_871X("%s(): change to 2.4G Band\n",__FUNCTION__);
13257                 rtw_add_bcn_ie(padapter, pnetwork, _ERPINFO_IE_, &erpinfo, 1);
13258         }
13259
13260         rtw_set_supported_rate(pnetwork->SupportedRates, network_type);
13261
13262         UpdateBrateTbl(padapter, pnetwork->SupportedRates);
13263         rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
13264
13265         if(total_rate_len > 8)
13266         {
13267                 rate_len = 8;
13268                 remainder_rate_len = total_rate_len - 8;
13269         }
13270         else
13271         {
13272                 rate_len = total_rate_len;
13273                 remainder_rate_len = 0;
13274         }
13275
13276         rtw_add_bcn_ie(padapter, pnetwork, _SUPPORTEDRATES_IE_, pnetwork->SupportedRates, rate_len);
13277
13278         if(remainder_rate_len)
13279         {
13280                 rtw_add_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_, (pnetwork->SupportedRates+8), remainder_rate_len);
13281         }
13282         else
13283         {
13284                 rtw_remove_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_);
13285         }
13286 }
13287
13288 #ifdef CONFIG_DUALMAC_CONCURRENT
13289 void dc_SelectChannel(_adapter *padapter, unsigned char channel)
13290 {
13291         PADAPTER ptarget_adapter;
13292
13293         if( (padapter->pbuddy_adapter != NULL) &&
13294                 (padapter->DualMacConcurrent == _TRUE) &&
13295                 (padapter->adapter_type == SECONDARY_ADAPTER))
13296         {
13297                 // only mac0 could control BB&RF
13298                 ptarget_adapter = padapter->pbuddy_adapter;
13299         }
13300         else
13301         {
13302                 ptarget_adapter = padapter;
13303         }
13304
13305         _enter_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setch_mutex), NULL);
13306
13307         rtw_hal_set_chan(ptarget_adapter, channel);
13308
13309         _exit_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setch_mutex), NULL);
13310 }
13311
13312 void dc_SetBWMode(_adapter *padapter, unsigned short bwmode, unsigned char channel_offset)
13313 {
13314         PADAPTER ptarget_adapter;
13315
13316         if( (padapter->pbuddy_adapter != NULL) &&
13317                 (padapter->DualMacConcurrent == _TRUE) &&
13318                 (padapter->adapter_type == SECONDARY_ADAPTER))
13319         {
13320                 // only mac0 could control BB&RF
13321                 ptarget_adapter = padapter->pbuddy_adapter;
13322         }
13323         else
13324         {
13325                 ptarget_adapter = padapter;
13326         }
13327
13328         _enter_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setbw_mutex), NULL);
13329
13330         rtw_hal_set_bwmode(ptarget_adapter, (CHANNEL_WIDTH)bwmode, channel_offset);
13331
13332         _exit_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setbw_mutex), NULL);
13333 }
13334
13335 void dc_set_channel_bwmode_disconnect(_adapter *padapter)
13336 {
13337         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13338         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13339         struct mlme_priv *pbuddy_mlmepriv = NULL;
13340
13341         if(pbuddy_adapter != NULL &&
13342                 padapter->DualMacConcurrent == _TRUE)
13343         {
13344                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13345                 if((check_fwstate(pbuddy_mlmepriv, _FW_LINKED)) != _TRUE)
13346                 {
13347                         //switch to the 20M Hz mode after disconnect
13348                         pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
13349                         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13350
13351                         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
13352                 }
13353         }
13354         else
13355         {
13356                 //switch to the 20M Hz mode after disconnect
13357                 pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
13358                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13359
13360                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
13361         }
13362 }
13363
13364 u8 dc_handle_join_request(_adapter *padapter, u8 *ch, u8 *bw, u8 *offset)
13365 {
13366         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13367         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13368         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
13369         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13370         struct mlme_ext_priv *pbuddy_mlmeext = NULL;
13371         struct mlme_priv        *pbuddy_mlmepriv = NULL;
13372         u8      ret = _SUCCESS;
13373
13374         if(pbuddy_adapter != NULL &&
13375                 padapter->DualMacConcurrent == _TRUE)
13376         {
13377                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13378                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13379
13380                 if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel ||
13381                         pmlmeext->cur_bwmode != pbuddy_mlmeext->cur_bwmode ||
13382                         pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
13383                 {
13384                         if((check_fwstate(pbuddy_mlmepriv, WIFI_AP_STATE)) == _TRUE)
13385                         {
13386                                 //issue deauth to all stas if if2 is at ap mode
13387                                 rtw_sta_flush(pbuddy_adapter);
13388
13389                                 //rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
13390                                 rtw_hal_set_hwreg(pbuddy_adapter, HW_VAR_CHECK_TXBUF, 0);
13391                         }
13392                         else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED) == _TRUE)
13393                         {
13394                                 if(pmlmeext->cur_channel == pbuddy_mlmeext->cur_channel)
13395                                 {
13396                                         // CHANNEL_WIDTH_40 or CHANNEL_WIDTH_20 but channel offset is different
13397                                         if((pmlmeext->cur_bwmode == pbuddy_mlmeext->cur_bwmode) &&
13398                                                 (pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset) )
13399                                         {
13400                                                 report_join_res(padapter, -4);
13401                                                 ret = _FAIL;
13402                                         }
13403                                 }
13404                                 else
13405                                 {
13406                                         report_join_res(padapter, -4);
13407                                         ret = _FAIL;
13408                                 }
13409                         }
13410                 }
13411                 else     if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
13412                 {
13413                         issue_nulldata(pbuddy_adapter, NULL, 1, 0, 0);
13414                 }
13415         }
13416
13417         if (!ch || !bw || !offset) {
13418                 rtw_warn_on(1);
13419                 ret = _FAIL;
13420         }
13421
13422         if (ret == _SUCCESS) {
13423                 *ch = pmlmeext->cur_channel;
13424                 *bw = pmlmeext->cur_bwmode;
13425                 *offset = pmlmeext->cur_ch_offset;
13426         }
13427
13428 exit:
13429         return ret;
13430 }
13431
13432 void dc_handle_join_done(_adapter *padapter, u8 join_res)
13433 {
13434         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13435         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13436         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13437         struct mlme_priv *pbuddy_mlmepriv = NULL;
13438         struct mlme_ext_priv *pbuddy_mlmeext = NULL;
13439         struct mlme_ext_info *pbuddy_mlmeinfo = NULL;
13440         WLAN_BSSID_EX *pbuddy_network_mlmeext = NULL;
13441         u8      change_band = _FALSE;
13442
13443
13444         if(pbuddy_adapter != NULL &&
13445                 padapter->DualMacConcurrent == _TRUE)
13446         {
13447                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13448                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13449                 pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
13450                 pbuddy_network_mlmeext = &(pbuddy_mlmeinfo->network);
13451
13452                 if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
13453                                 check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
13454                 {
13455                         //restart and update beacon
13456                         DBG_871X("after join, current adapter, CH=%d, BW=%d, offset=%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
13457
13458                         if(join_res >= 0)
13459                         {
13460                                 u8 *p;
13461                                 int     ie_len;
13462                                 struct HT_info_element *pht_info=NULL;
13463
13464                                 if((pbuddy_mlmeext->cur_channel <= 14 && pmlmeext->cur_channel >= 36) ||
13465                                         (pbuddy_mlmeext->cur_channel >= 36 && pmlmeext->cur_channel <= 14))
13466                                 {
13467                                         change_band = _TRUE;
13468                                 }
13469
13470                                 //sync channel/bwmode/ch_offset with another adapter
13471                                 pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
13472
13473                                 if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40)
13474                                 {
13475                                         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)));
13476                                         if( p && ie_len)
13477                                         {
13478                                                 pht_info = (struct HT_info_element *)(p+2);
13479                                                 pht_info->infos[0] &= ~(BIT(0)|BIT(1)); //no secondary channel is present
13480                                         }
13481
13482                                         if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_40)
13483                                         {
13484                                                 pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
13485
13486                                                 //to update cur_ch_offset value in beacon
13487                                                 if( pht_info )
13488                                                 {
13489                                                         switch(pmlmeext->cur_ch_offset)
13490                                                         {
13491                                                                 case HAL_PRIME_CHNL_OFFSET_LOWER:
13492                                                                         pht_info->infos[0] |= 0x1;
13493                                                                         break;
13494                                                                 case HAL_PRIME_CHNL_OFFSET_UPPER:
13495                                                                         pht_info->infos[0] |= 0x3;
13496                                                                         break;
13497                                                                 case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
13498                                                                 default:
13499                                                                         break;
13500                                                         }
13501                                                 }
13502                                         }
13503                                         else if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_20)
13504                                         {
13505                                                 pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_20;
13506                                                 pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13507
13508                                                 if(pmlmeext->cur_channel>0 && pmlmeext->cur_channel<5)
13509                                                 {
13510                                                         if(pht_info)
13511                                                                 pht_info->infos[0] |= 0x1;
13512
13513                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
13514                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
13515                                                 }
13516
13517                                                 if(pmlmeext->cur_channel>7 && pmlmeext->cur_channel<(14+1))
13518                                                 {
13519                                                         if(pht_info)
13520                                                                 pht_info->infos[0] |= 0x3;
13521
13522                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
13523                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
13524                                                 }
13525
13526                                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13527                                         }
13528                                 }
13529
13530                                 // to update channel value in beacon
13531                                 pbuddy_network_mlmeext->Configuration.DSConfig = pmlmeext->cur_channel;
13532                                 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)));
13533                                 if(p && ie_len>0)
13534                                         *(p + 2) = pmlmeext->cur_channel;
13535
13536                                 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)));
13537                                 if( p && ie_len)
13538                                 {
13539                                         pht_info = (struct HT_info_element *)(p+2);
13540                                         pht_info->primary_channel = pmlmeext->cur_channel;
13541                                 }
13542
13543                                 // update mlmepriv's cur_network
13544                                 _rtw_memcpy(&pbuddy_mlmepriv->cur_network.network, pbuddy_network_mlmeext, pbuddy_network_mlmeext->Length);
13545                         }
13546                         else
13547                         {
13548                                 // switch back to original channel/bwmode/ch_offset;
13549                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13550                         }
13551
13552                         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);
13553
13554                         if(change_band == _TRUE)
13555                                 change_band_update_ie(pbuddy_adapter, pbuddy_network_mlmeext);
13556
13557                         DBG_871X("update pbuddy_adapter's beacon\n");
13558
13559                         update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
13560                 }
13561                 else     if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
13562                 {
13563                         if((pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
13564                                 (pmlmeext->cur_bwmode == CHANNEL_WIDTH_20))
13565                         {
13566                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
13567                         }
13568
13569                         issue_nulldata(pbuddy_adapter, NULL, 0, 0, 0);
13570                 }
13571         }
13572 }
13573
13574 sint    dc_check_fwstate(_adapter *padapter, sint fw_state)
13575 {
13576         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13577         struct mlme_priv *pbuddy_mlmepriv = NULL;
13578
13579         if(padapter->pbuddy_adapter != NULL &&
13580                 padapter->DualMacConcurrent == _TRUE)
13581
13582         {
13583                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13584
13585                 return check_fwstate(pbuddy_mlmepriv, fw_state);
13586         }
13587
13588         return _FALSE;
13589 }
13590
13591 u8 dc_handle_site_survey(_adapter *padapter)
13592 {
13593         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13594         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13595         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13596
13597         // only mac0 can do scan request, help issue nulldata(1) for mac1
13598         if(pbuddy_adapter != NULL &&
13599                 padapter->DualMacConcurrent == _TRUE)
13600         {
13601                 if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
13602                 {
13603                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
13604
13605                         issue_nulldata(pbuddy_adapter, NULL, 1, 2, 0);
13606
13607                         return _TRUE;
13608                 }
13609         }
13610
13611         return _FALSE;
13612 }
13613
13614 void    dc_report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
13615 {
13616         if(padapter->pbuddy_adapter != NULL &&
13617                 padapter->DualMacConcurrent == _TRUE)
13618         {
13619                 report_survey_event(padapter->pbuddy_adapter, precv_frame);
13620         }
13621 }
13622
13623 void dc_set_channel_bwmode_survey_done(_adapter *padapter)
13624 {
13625         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
13626         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
13627         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13628         struct mlme_priv *pbuddy_mlmepriv = NULL;
13629         struct mlme_ext_priv *pbuddy_mlmeext = NULL;
13630         struct mlme_ext_info *pbuddy_mlmeinfo = NULL;
13631         u8 cur_channel;
13632         u8 cur_bwmode;
13633         u8 cur_ch_offset;
13634
13635         if(pbuddy_adapter != NULL &&
13636                 padapter->DualMacConcurrent == _TRUE)
13637         {
13638                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13639                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13640                 pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
13641
13642                 if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
13643                 {
13644                         if(check_fwstate(pmlmepriv, _FW_LINKED) &&
13645                                 (pmlmeext->cur_bwmode == CHANNEL_WIDTH_40))
13646                         {
13647                                 cur_channel = pmlmeext->cur_channel;
13648                                 cur_bwmode = pmlmeext->cur_bwmode;
13649                                 cur_ch_offset = pmlmeext->cur_ch_offset;
13650                         }
13651                         else
13652                         {
13653                                 cur_channel = pbuddy_mlmeext->cur_channel;
13654                                 cur_bwmode = pbuddy_mlmeext->cur_bwmode;
13655                                 cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
13656                         }
13657                 }
13658                 else
13659                 {
13660                         cur_channel = pmlmeext->cur_channel;
13661                         cur_bwmode = pmlmeext->cur_bwmode;
13662                         cur_ch_offset = pmlmeext->cur_ch_offset;
13663                 }
13664
13665                 set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
13666
13667                 if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
13668                 {
13669                         //issue null data
13670                         issue_nulldata(pbuddy_adapter, NULL, 0, 0, 0);
13671                 }
13672
13673                 if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
13674                         check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
13675                 {
13676
13677                         DBG_871X("survey done, current CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
13678
13679                         DBG_871X("restart pbuddy_adapter's beacon\n");
13680
13681                         update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
13682                 }
13683         }
13684         else
13685         {
13686                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
13687         }
13688 }
13689
13690 void dc_set_ap_channel_bandwidth(_adapter *padapter, u8 channel, u8 channel_offset, u8 bwmode)
13691 {
13692         u8      *p;
13693         u8      val8, cur_channel, cur_bwmode, cur_ch_offset, change_band;
13694         int     ie_len;
13695         struct registry_priv    *pregpriv = &padapter->registrypriv;
13696         struct mlme_priv                *pmlmepriv = &(padapter->mlmepriv);
13697         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;
13698         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
13699         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
13700         struct HT_info_element  *pht_info=NULL;
13701         _adapter        *pbuddy_adapter = padapter->pbuddy_adapter;
13702         struct mlme_priv        *pbuddy_mlmepriv = NULL;
13703         struct mlme_ext_priv    *pbuddy_mlmeext = NULL;
13704
13705         DBG_871X("dualmac_concurrent_ap_set_channel_bwmode ==>\n");
13706
13707         cur_channel = channel;
13708         cur_bwmode = bwmode;
13709         cur_ch_offset = channel_offset;
13710         change_band = _FALSE;
13711
13712         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)));
13713         if( p && ie_len)
13714         {
13715                 pht_info = (struct HT_info_element *)(p+2);
13716         }
13717
13718         if(pbuddy_adapter != NULL &&
13719                 padapter->DualMacConcurrent == _TRUE)
13720         {
13721                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13722                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13723
13724                 if(!check_fwstate(pbuddy_mlmepriv, _FW_LINKED|_FW_UNDER_LINKING|_FW_UNDER_SURVEY))
13725                 {
13726                         set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
13727                 }
13728                 else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED)==_TRUE)
13729                 {
13730                         //To sync cur_channel/cur_bwmode/cur_ch_offset with another adapter
13731                         DBG_871X("Another iface is at linked state, sync cur_channel/cur_bwmode/cur_ch_offset\n");
13732                         DBG_871X("Another adapter, CH=%d, BW=%d, offset=%d\n", pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
13733                         DBG_871X("Current adapter, CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
13734
13735                         cur_channel = pbuddy_mlmeext->cur_channel;
13736                         if(cur_bwmode == CHANNEL_WIDTH_40)
13737                         {
13738                                 if(pht_info)
13739                                         pht_info->infos[0] &= ~(BIT(0)|BIT(1));
13740
13741                                 if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40)
13742                                 {
13743                                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
13744
13745                                         //to update cur_ch_offset value in beacon
13746                                         if(pht_info)
13747                                         {
13748                                                 switch(cur_ch_offset)
13749                                                 {
13750                                                         case HAL_PRIME_CHNL_OFFSET_LOWER:
13751                                                                 pht_info->infos[0] |= 0x1;
13752                                                                 break;
13753                                                         case HAL_PRIME_CHNL_OFFSET_UPPER:
13754                                                                 pht_info->infos[0] |= 0x3;
13755                                                                 break;
13756                                                         case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
13757                                                         default:
13758                                                                 break;
13759                                                 }
13760                                         }
13761                                 }
13762                                 else if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_20)
13763                                 {
13764                                         cur_bwmode = CHANNEL_WIDTH_20;
13765                                         cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
13766
13767                                         if(cur_channel>0 && cur_channel<5)
13768                                         {
13769                                                 if(pht_info)
13770                                                         pht_info->infos[0] |= 0x1;
13771
13772                                                 cur_bwmode = CHANNEL_WIDTH_40;
13773                                                 cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
13774                                         }
13775
13776                                         if(cur_channel>7 && cur_channel<(14+1))
13777                                         {
13778                                                 if(pht_info)
13779                                                         pht_info->infos[0] |= 0x3;
13780
13781                                                 cur_bwmode = CHANNEL_WIDTH_40;
13782                                                 cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
13783                                         }
13784
13785                                         set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
13786                                 }
13787                         }
13788
13789                         // to update channel value in beacon
13790                         pnetwork->Configuration.DSConfig = cur_channel;
13791                         p = rtw_get_ie((pnetwork->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (pnetwork->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
13792                         if(p && ie_len>0)
13793                                 *(p + 2) = cur_channel;
13794
13795                         if(pht_info)
13796                                 pht_info->primary_channel = cur_channel;
13797                 }
13798         }
13799         else
13800         {
13801                 set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
13802         }
13803
13804         DBG_871X("CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
13805
13806         if((channel <= 14 && cur_channel >= 36) ||
13807                 (channel >= 36 && cur_channel <= 14))
13808         {
13809                 change_band = _TRUE;
13810         }
13811
13812         pmlmeext->cur_channel = cur_channel;
13813         pmlmeext->cur_bwmode = cur_bwmode;
13814         pmlmeext->cur_ch_offset = cur_ch_offset;
13815
13816         if(change_band == _TRUE)
13817                 change_band_update_ie(padapter, pnetwork);
13818
13819         DBG_871X("dualmac_concurrent_ap_set_channel_bwmode <==\n");
13820 }
13821
13822 void dc_resume_xmit(_adapter *padapter)
13823 {
13824         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13825
13826         if(pbuddy_adapter != NULL &&
13827                 padapter->DualMacConcurrent == _TRUE)
13828         {
13829                 DBG_871X("dc_resume_xmit,  resume pbuddy_adapter Tx\n");
13830                 rtw_os_xmit_schedule(pbuddy_adapter);
13831         }
13832 }
13833
13834 u8      dc_check_xmit(_adapter *padapter)
13835 {
13836         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
13837         struct mlme_priv *pbuddy_mlmepriv = NULL;
13838
13839         if(pbuddy_adapter != NULL &&
13840                 padapter->DualMacConcurrent == _TRUE)
13841         {
13842                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13843                 if (check_fwstate(pbuddy_mlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == _TRUE)
13844                 {
13845                         DBG_871X("dc_check_xmit  pbuddy_adapter is under survey or under linking\n");
13846                         return _FALSE;
13847                 }
13848         }
13849
13850         return _TRUE;
13851 }
13852 #endif
13853
13854 #ifdef CONFIG_CONCURRENT_MODE
13855 sint check_buddy_mlmeinfo_state(_adapter *padapter, u32 state)
13856 {
13857         PADAPTER pbuddy_adapter;
13858         struct mlme_ext_priv *pbuddy_mlmeext;
13859         struct mlme_ext_info *pbuddy_mlmeinfo;
13860
13861         if(padapter == NULL)
13862                 return _FALSE;
13863
13864         pbuddy_adapter = padapter->pbuddy_adapter;
13865
13866         if(pbuddy_adapter == NULL)
13867                 return _FALSE;
13868
13869
13870         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13871         pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
13872
13873         if((pbuddy_mlmeinfo->state&0x03) == state)
13874                 return _TRUE;
13875
13876         return _FALSE;
13877
13878 }
13879
13880 void concurrent_chk_joinbss_done(_adapter *padapter, int join_res)
13881 {
13882         struct mlme_ext_priv    *pmlmeext;
13883         struct mlme_ext_info    *pmlmeinfo;
13884         PADAPTER pbuddy_adapter;
13885         struct mlme_priv *pbuddy_mlmepriv;
13886         struct mlme_ext_priv *pbuddy_mlmeext;
13887         struct mlme_ext_info *pbuddy_mlmeinfo;
13888         WLAN_BSSID_EX *pbuddy_network_mlmeext;
13889         WLAN_BSSID_EX *pnetwork;
13890
13891         pmlmeext = &padapter->mlmeextpriv;
13892         pmlmeinfo = &(pmlmeext->mlmext_info);
13893
13894
13895         if(!rtw_buddy_adapter_up(padapter))
13896         {
13897                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
13898                 return;
13899         }
13900
13901         pbuddy_adapter = padapter->pbuddy_adapter;
13902         pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
13903         pnetwork = (WLAN_BSSID_EX *)&pbuddy_mlmepriv->cur_network.network;
13904         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
13905         pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
13906         pbuddy_network_mlmeext = &(pbuddy_mlmeinfo->network);
13907
13908         if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
13909                         check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
13910         {
13911                 //restart and update beacon
13912
13913                 DBG_871X("after join,primary adapter, CH=%d, BW=%d, offset=%d\n"
13914                         , pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
13915
13916
13917                 if(join_res >= 0)
13918                 {
13919                         u8 *p;
13920                         int     ie_len;
13921                         u8      change_band = _FALSE;
13922                         struct HT_info_element *pht_info=NULL;
13923
13924                         if((pmlmeext->cur_channel <= 14 && pbuddy_mlmeext->cur_channel >= 36) ||
13925                                 (pmlmeext->cur_channel >= 36 && pbuddy_mlmeext->cur_channel <= 14))
13926                                 change_band = _TRUE;
13927
13928                         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)));
13929                         if( p && ie_len)
13930                         {
13931                                 pht_info = (struct HT_info_element *)(p+2);
13932                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK; //no secondary channel is present
13933                         }
13934
13935                         pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
13936 #ifdef CONFIG_80211AC_VHT
13937                         if(pbuddy_mlmeext->cur_bwmode  == CHANNEL_WIDTH_80)
13938                         {
13939                                 u8 *pvht_cap_ie, *pvht_op_ie;
13940                                 int vht_cap_ielen, vht_op_ielen;
13941
13942                                 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)));
13943                                 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)));
13944
13945                                 if(pmlmeext->cur_channel <= 14) // downgrade to 20/40Mhz
13946                                 {
13947                                         //modify vht cap ie
13948                                         if( pvht_cap_ie && vht_cap_ielen)
13949                                         {
13950                                                 SET_VHT_CAPABILITY_ELE_SHORT_GI80M(pvht_cap_ie+2, 0);
13951                                         }
13952
13953                                         //modify vht op ie
13954                                         if( pvht_op_ie && vht_op_ielen)
13955                                         {
13956                                                 SET_VHT_OPERATION_ELE_CHL_WIDTH(pvht_op_ie+2, 0); //change to 20/40Mhz
13957                                                 SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(pvht_op_ie+2, 0);
13958                                                 SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ2(pvht_op_ie+2, 0);
13959                                                 //SET_VHT_OPERATION_ELE_BASIC_MCS_SET(p+2, 0xFFFF);
13960                                                 pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
13961                                         }
13962                                 }
13963                                 else
13964                                 {
13965                                         u8      center_freq;
13966
13967                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_80;
13968
13969                                         if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
13970                                                 pmlmeext->cur_bwmode == CHANNEL_WIDTH_80)
13971                                         {
13972                                                 pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
13973                                         }
13974                                         else if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_20)
13975                                         {
13976                                                 pbuddy_mlmeext->cur_ch_offset =  rtw_get_offset_by_ch(pmlmeext->cur_channel);
13977                                         }
13978
13979                                         //modify ht info ie
13980                                         if(pht_info)
13981                                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK;
13982
13983                                         switch(pbuddy_mlmeext->cur_ch_offset)
13984                                         {
13985                                                 case HAL_PRIME_CHNL_OFFSET_LOWER:
13986                                                         if(pht_info)
13987                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;
13988                                                         //cur_bwmode = CHANNEL_WIDTH_40;
13989                                                         break;
13990                                                 case HAL_PRIME_CHNL_OFFSET_UPPER:
13991                                                         if(pht_info)
13992                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
13993                                                         //cur_bwmode = CHANNEL_WIDTH_40;
13994                                                         break;
13995                                                 case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
13996                                                 default:
13997                                                         if(pht_info)
13998                                                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK;
13999                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_20;
14000                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
14001                                                         break;
14002                                         }
14003
14004                                         //modify vht op ie
14005                                         center_freq = rtw_get_center_ch(pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, HAL_PRIME_CHNL_OFFSET_LOWER);
14006                                         if( pvht_op_ie && vht_op_ielen)
14007                                                 SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(pvht_op_ie+2, center_freq);
14008
14009                                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
14010
14011                                 }
14012
14013                         }
14014 #endif //CONFIG_80211AC_VHT
14015
14016                         if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40)
14017                         {
14018                                 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)));
14019                                 if( p && ie_len)
14020                                 {
14021                                         pht_info = (struct HT_info_element *)(p+2);
14022                                         pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK; //no secondary channel is present
14023                                 }
14024
14025                                 if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
14026                                         pmlmeext->cur_bwmode == CHANNEL_WIDTH_80)
14027                                 {
14028                                         pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
14029
14030                                         //to update cur_ch_offset value in beacon
14031                                         if( pht_info )
14032                                         {
14033                                                 switch(pmlmeext->cur_ch_offset)
14034                                                 {
14035                                                         case HAL_PRIME_CHNL_OFFSET_LOWER:
14036                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;
14037                                                                 break;
14038                                                         case HAL_PRIME_CHNL_OFFSET_UPPER:
14039                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
14040                                                                 break;
14041                                                         case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
14042                                                         default:
14043                                                                 break;
14044                                                 }
14045
14046                                         }
14047
14048                                 }
14049                                 else if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_20)
14050                                 {
14051                                         pbuddy_mlmeext->cur_ch_offset =  rtw_get_offset_by_ch(pmlmeext->cur_channel);
14052
14053                                         switch(pbuddy_mlmeext->cur_ch_offset)
14054                                         {
14055                                                 case HAL_PRIME_CHNL_OFFSET_LOWER:
14056                                                         if(pht_info)
14057                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;
14058                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
14059                                                         break;
14060                                                 case HAL_PRIME_CHNL_OFFSET_UPPER:
14061                                                         if(pht_info)
14062                                                                 pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
14063                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_40;
14064                                                         break;
14065                                                 case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
14066                                                 default:
14067                                                         if(pht_info)
14068                                                                 pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK;
14069                                                         pbuddy_mlmeext->cur_bwmode = CHANNEL_WIDTH_20;
14070                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
14071                                                         break;
14072                                         }
14073
14074                                 }
14075
14076                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
14077
14078                         }
14079                         else
14080                         {
14081                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
14082                         }
14083
14084
14085                         // to update channel value in beacon
14086                         pbuddy_network_mlmeext->Configuration.DSConfig = pmlmeext->cur_channel;
14087                         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)));
14088                         if(p && ie_len>0)
14089                                 *(p + 2) = pmlmeext->cur_channel;
14090
14091                         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)));
14092                         if( p && ie_len)
14093                         {
14094                                 pht_info = (struct HT_info_element *)(p+2);
14095                                 pht_info->primary_channel = pmlmeext->cur_channel;
14096                         }
14097
14098                         //buddy interface band is different from current interface, update ERP, support rate, ext support rate IE
14099                         if(change_band == _TRUE)
14100                                 change_band_update_ie(pbuddy_adapter, pbuddy_network_mlmeext);
14101                 }
14102                 else
14103                 {
14104                         // switch back to original channel/bwmode/ch_offset;
14105                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
14106                 }
14107
14108                 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);
14109
14110                 DBG_871X("update pbuddy_adapter's beacon\n");
14111
14112                 _rtw_memcpy(pnetwork, pbuddy_network_mlmeext, sizeof(WLAN_BSSID_EX));
14113                 //update bmc rate to avoid bb cck hang
14114                 update_bmc_sta(pbuddy_adapter);
14115                 update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
14116
14117         }
14118         else if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) &&
14119                         check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
14120         {
14121                 if(join_res >= 0)
14122                 {
14123                         pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
14124                         if(pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
14125                                 pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80)
14126                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
14127                         else if(pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 ||
14128                                 pmlmeext->cur_bwmode == CHANNEL_WIDTH_80)
14129                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
14130                         else
14131                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
14132                 }
14133                 else
14134                 {
14135                         // switch back to original channel/bwmode/ch_offset;
14136                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
14137                 }
14138         }
14139         else
14140         {
14141                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
14142         }
14143
14144 }
14145 #endif //CONFIG_CONCURRENT_MODE
14146
14147 int rtw_chk_start_clnt_join(_adapter *padapter, u8 *ch, u8 *bw, u8 *offset)
14148 {
14149         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
14150         unsigned char   cur_ch = pmlmeext->cur_channel;
14151         unsigned char   cur_bw = pmlmeext->cur_bwmode;
14152         unsigned char   cur_ch_offset = pmlmeext->cur_ch_offset;
14153         bool chbw_allow = _TRUE;
14154         bool connect_allow = _TRUE;
14155
14156 #ifdef CONFIG_CONCURRENT_MODE
14157         PADAPTER pbuddy_adapter;
14158         struct mlme_ext_priv *pbuddy_mlmeext;
14159         struct mlme_ext_info    *pbuddy_pmlmeinfo;
14160         struct mlme_priv *pbuddy_mlmepriv;
14161
14162         if (!rtw_buddy_adapter_up(padapter)) {
14163                 goto exit;
14164         }
14165
14166         pbuddy_adapter = padapter->pbuddy_adapter;
14167         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
14168         pbuddy_pmlmeinfo = &(pbuddy_mlmeext->mlmext_info);
14169         pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
14170
14171         if((pbuddy_pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)//for AP MODE
14172         {
14173                 DBG_871X("start_clnt_join: "ADPT_FMT"(ch=%d, bw=%d, ch_offset=%d)"
14174                         ", "ADPT_FMT" AP mode(ch=%d, bw=%d, ch_offset=%d)\n",
14175                         ADPT_ARG(padapter), pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset,
14176                         ADPT_ARG(pbuddy_adapter), pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
14177
14178                 if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel)
14179                 {
14180                         chbw_allow = _FALSE;
14181                 }
14182                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
14183                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
14184                         (pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset))
14185                 {
14186                         chbw_allow = _FALSE;
14187                 }
14188                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_20) &&
14189                         ((pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40)||
14190                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80)) )
14191                 {
14192                         cur_ch = pmlmeext->cur_channel;
14193                         cur_bw = pbuddy_mlmeext->cur_bwmode;
14194                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
14195                 }
14196
14197                 DBG_871X("start_clnt_join: connect_allow:%d, chbw_allow:%d\n", connect_allow, chbw_allow);
14198                 if (chbw_allow == _FALSE) {
14199                         #ifdef CONFIG_SPCT_CH_SWITCH
14200                         if (1) {
14201                                 rtw_ap_inform_ch_switch(pbuddy_adapter, pmlmeext->cur_channel , pmlmeext->cur_ch_offset);
14202                         } else
14203                         #endif
14204                         {
14205                                 //issue deauth to all stas if if2 is at ap mode
14206                                 rtw_sta_flush(pbuddy_adapter);
14207                         }
14208                         rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
14209                 }
14210         }
14211         else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED) == _TRUE &&
14212                 check_fwstate(pbuddy_mlmepriv, WIFI_STATION_STATE) == _TRUE) //for Client Mode/p2p client
14213         {
14214                 DBG_871X("start_clnt_join: "ADPT_FMT"(ch=%d, bw=%d, ch_offset=%d)"
14215                         ", "ADPT_FMT" STA mode(ch=%d, bw=%d, ch_offset=%d)\n",
14216                         ADPT_ARG(padapter), pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset,
14217                         ADPT_ARG(pbuddy_adapter), pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
14218
14219                 if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel)
14220                 {
14221                         chbw_allow = _FALSE;
14222                 }
14223                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_20) &&
14224                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40))
14225                 {
14226                         cur_bw = CHANNEL_WIDTH_40;
14227                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
14228                 }
14229                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_20) &&
14230                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80))
14231                 {
14232                         cur_bw = CHANNEL_WIDTH_80;
14233                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
14234                 }
14235                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
14236                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
14237                         (pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset))
14238                 {
14239                         chbw_allow = _FALSE;
14240                 }
14241                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_40) &&
14242                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80))
14243                 {
14244                         if(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
14245                         {
14246                                 chbw_allow = _FALSE;
14247                         }
14248                         else
14249                         {
14250                                 cur_bw = CHANNEL_WIDTH_80;
14251                         }
14252                 }
14253                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_80) &&
14254                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_40))
14255                 {
14256                         if(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
14257                         {
14258                                 chbw_allow = _FALSE;
14259                         }
14260                 }
14261                 else if((pmlmeext->cur_bwmode == CHANNEL_WIDTH_80) &&
14262                         (pbuddy_mlmeext->cur_bwmode == CHANNEL_WIDTH_80))
14263                 {
14264                         if(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
14265                         {
14266                                 chbw_allow = _FALSE;
14267                         }
14268                 }
14269
14270
14271                 connect_allow = chbw_allow;
14272
14273 #ifdef CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT
14274                 #if defined(CONFIG_P2P) && defined(CONFIG_IOCTL_CFG80211)
14275                 /* wlan0-sta mode has higher priority than p2p0-p2p client */
14276                 if (!rtw_p2p_chk_state(&(pbuddy_adapter->wdinfo), P2P_STATE_NONE)
14277                         && pbuddy_adapter->wdinfo.driver_interface == DRIVER_CFG80211)
14278                 {
14279                         connect_allow = _TRUE;
14280                 }
14281                 #endif /* CONFIG_P2P && CONFIG_IOCTL_CFG80211 */
14282 #else
14283                 connect_allow = _TRUE;
14284 #endif /* CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT */
14285
14286                 DBG_871X("start_clnt_join: connect_allow:%d, chbw_allow:%d\n", connect_allow, chbw_allow);
14287                 if (connect_allow == _TRUE && chbw_allow == _FALSE) {
14288                         /* disconnect buddy's connection */
14289                         rtw_disassoc_cmd(pbuddy_adapter, 500, _FALSE);
14290                         rtw_indicate_disconnect(pbuddy_adapter);
14291                         rtw_free_assoc_resources(pbuddy_adapter, 1);
14292                 }
14293         }
14294
14295 exit:
14296 #endif /* CONFIG_CONCURRENT_MODE */
14297
14298         if (!ch || !bw || !offset) {
14299                 rtw_warn_on(1);
14300                 connect_allow = _FALSE;
14301         }
14302
14303         if (connect_allow == _TRUE) {
14304                 DBG_871X("start_join_set_ch_bw: ch=%d, bwmode=%d, ch_offset=%d\n", cur_ch, cur_bw, cur_ch_offset);
14305                 *ch = cur_ch;
14306                 *bw = cur_bw;
14307                 *offset = cur_ch_offset;
14308         }
14309
14310         return connect_allow == _TRUE ? _SUCCESS : _FAIL;
14311 }
14312
14313 /* Find union about ch, bw, ch_offset of all linked/linking interfaces */
14314 int rtw_get_ch_setting_union(_adapter *adapter, u8 *ch, u8 *bw, u8 *offset)
14315 {
14316         struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
14317         _adapter *iface;
14318         struct mlme_ext_priv *mlmeext;
14319         int i;
14320         u8 ch_ret = 0;
14321         u8 bw_ret = CHANNEL_WIDTH_20;
14322         u8 offset_ret = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
14323         int num = 0;
14324
14325         if (ch) *ch = 0;
14326         if (bw) *bw = CHANNEL_WIDTH_20;
14327         if (offset) *offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
14328
14329         for (i = 0; i<dvobj->iface_nums; i++) {
14330                 iface = dvobj->padapters[i];
14331                 mlmeext = &iface->mlmeextpriv;
14332
14333                 if (!check_fwstate(&iface->mlmepriv, _FW_LINKED|_FW_UNDER_LINKING))
14334                         continue;
14335
14336                 if (num == 0) {
14337                         ch_ret = mlmeext->cur_channel;
14338                         bw_ret = mlmeext->cur_bwmode;
14339                         offset_ret = mlmeext->cur_ch_offset;
14340                         num++;
14341                         continue;
14342                 }
14343
14344                 if (ch_ret != mlmeext->cur_channel) {
14345                         num = 0;
14346                         break;
14347                 }
14348
14349                 if (bw_ret < mlmeext->cur_bwmode) {
14350                         bw_ret = mlmeext->cur_bwmode;
14351                         offset_ret = mlmeext->cur_ch_offset;
14352                 } else if (bw_ret == mlmeext->cur_bwmode && offset_ret != mlmeext->cur_ch_offset) {
14353                         num = 0;
14354                         break;
14355                 }
14356
14357                 num++;
14358         }
14359
14360         if (num) {
14361                 if (ch) *ch = ch_ret;
14362                 if (bw) *bw = bw_ret;
14363                 if (offset) *offset = offset_ret;
14364         }
14365
14366         return num;
14367 }
14368
14369 u8 set_ch_hdl(_adapter *padapter, u8 *pbuf)
14370 {
14371         struct set_ch_parm *set_ch_parm;
14372         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
14373         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
14374
14375         if(!pbuf)
14376                 return H2C_PARAMETERS_ERROR;
14377
14378         set_ch_parm = (struct set_ch_parm *)pbuf;
14379
14380         DBG_871X(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
14381                 FUNC_NDEV_ARG(padapter->pnetdev),
14382                 set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
14383
14384         pmlmeext->cur_channel = set_ch_parm->ch;
14385         pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
14386         pmlmeext->cur_bwmode = set_ch_parm->bw;
14387
14388         set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
14389
14390         return  H2C_SUCCESS;
14391 }
14392
14393 u8 set_chplan_hdl(_adapter *padapter, unsigned char *pbuf)
14394 {
14395         struct SetChannelPlan_param *setChannelPlan_param;
14396         struct mlme_priv *mlme = &padapter->mlmepriv;
14397         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
14398
14399         if(!pbuf)
14400                 return H2C_PARAMETERS_ERROR;
14401
14402         setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
14403
14404         if(!rtw_is_channel_plan_valid(setChannelPlan_param->channel_plan)) {
14405                 return H2C_PARAMETERS_ERROR;
14406         }
14407
14408         mlme->ChannelPlan = setChannelPlan_param->channel_plan;
14409
14410         pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
14411         init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
14412
14413         rtw_hal_set_odm_var(padapter,HAL_ODM_REGULATION,NULL,_TRUE);
14414
14415 #ifdef CONFIG_IOCTL_CFG80211
14416         rtw_reg_notify_by_driver(padapter);
14417 #endif //CONFIG_IOCTL_CFG80211
14418
14419         return  H2C_SUCCESS;
14420 }
14421
14422 u8 led_blink_hdl(_adapter *padapter, unsigned char *pbuf)
14423 {
14424         struct LedBlink_param *ledBlink_param;
14425
14426         if(!pbuf)
14427                 return H2C_PARAMETERS_ERROR;
14428
14429         ledBlink_param = (struct LedBlink_param *)pbuf;
14430
14431         #ifdef CONFIG_LED_HANDLED_BY_CMD_THREAD
14432         BlinkHandler((PLED_DATA)ledBlink_param->pLed);
14433         #endif
14434
14435         return  H2C_SUCCESS;
14436 }
14437
14438 u8 set_csa_hdl(_adapter *padapter, unsigned char *pbuf)
14439 {
14440 #ifdef CONFIG_DFS
14441         struct SetChannelSwitch_param *setChannelSwitch_param;
14442         u8 new_ch_no;
14443         u8 gval8 = 0x00, sval8 = 0xff;
14444
14445         if(!pbuf)
14446                 return H2C_PARAMETERS_ERROR;
14447
14448         setChannelSwitch_param = (struct SetChannelSwitch_param *)pbuf;
14449         new_ch_no = setChannelSwitch_param->new_ch_no;
14450
14451         rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
14452
14453         rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &sval8);
14454
14455         DBG_871X("DFS detected! Swiching channel to %d!\n", new_ch_no);
14456         SelectChannel(padapter, new_ch_no);
14457
14458         rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
14459
14460         rtw_disassoc_cmd(padapter, 0, _FALSE);
14461         rtw_indicate_disconnect(padapter);
14462         rtw_free_assoc_resources(padapter, 1);
14463         rtw_free_network_queue(padapter, _TRUE);
14464
14465         if ( ((new_ch_no >= 52) && (new_ch_no <= 64)) ||((new_ch_no >= 100) && (new_ch_no <= 140)) ) {
14466                 DBG_871X("Switched to DFS band (ch %02x) again!!\n", new_ch_no);
14467         }
14468
14469         return  H2C_SUCCESS;
14470 #else
14471         return  H2C_REJECTED;
14472 #endif //CONFIG_DFS
14473
14474 }
14475
14476 // TDLS_ESTABLISHED     : write RCR DATA BIT
14477 // TDLS_CS_OFF          : go back to the channel linked with AP, terminating channel switch procedure
14478 // TDLS_INIT_CH_SEN     : init channel sensing, receive all data and mgnt frame
14479 // TDLS_DONE_CH_SEN: channel sensing and report candidate channel
14480 // TDLS_OFF_CH          : first time set channel to off channel
14481 // TDLS_BASE_CH         : go back tp the channel linked with AP when set base channel as target channel
14482 // TDLS_P_OFF_CH        : periodically go to off channel
14483 // TDLS_P_BASE_CH       : periodically go back to base channel
14484 // TDLS_RS_RCR          : restore RCR
14485 // TDLS_TEAR_STA        : free tdls sta
14486 u8 tdls_hdl(_adapter *padapter, unsigned char *pbuf)
14487 {
14488 #ifdef CONFIG_TDLS
14489         _irqL irqL;
14490         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
14491         struct TDLSoption_param *TDLSoption;
14492         struct sta_info *ptdls_sta;
14493         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
14494         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
14495         u8 survey_channel, i, min, option;
14496
14497         if(!pbuf)
14498                 return H2C_PARAMETERS_ERROR;
14499
14500         TDLSoption = (struct TDLSoption_param *)pbuf;
14501
14502         ptdls_sta = rtw_get_stainfo( &(padapter->stapriv), TDLSoption->addr );
14503         option = TDLSoption->option;
14504
14505         if( ptdls_sta == NULL )
14506         {
14507                 if( option != TDLS_RS_RCR )
14508                         return H2C_REJECTED;
14509         }
14510
14511         //_enter_critical_bh(&(ptdlsinfo->hdl_lock), &irqL);
14512         DBG_871X("[%s] option:%d\n", __FUNCTION__, option);
14513
14514         switch(option){
14515                 case TDLS_ESTABLISHED:
14516                 {
14517                         u8 sta_band = 0;
14518
14519                         //leave ALL PS when TDLS is established
14520                         rtw_pwr_wakeup(padapter);
14521
14522                         //As long as TDLS handshake success, we should set RCR_CBSSID_DATA bit to 0
14523                         //so we can receive all kinds of data frames.
14524                         rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_WRCR, 0);
14525                         DBG_871X("Created Direct Link with "MAC_FMT"\n", MAC_ARG(ptdls_sta->hwaddr));
14526
14527                         pmlmeinfo->FW_sta_info[ptdls_sta->mac_id].psta = ptdls_sta;
14528                         //set TDLS sta rate.
14529                         //update station supportRate
14530                         rtw_hal_update_sta_rate_mask(padapter, ptdls_sta);
14531                         if(padapter->mlmeextpriv.cur_channel > 14) {
14532                                 if (ptdls_sta->ra_mask & 0xffff000)
14533                                         sta_band |= WIRELESS_11_5N ;
14534
14535                                 if (ptdls_sta->ra_mask & 0xff0)
14536                                         sta_band |= WIRELESS_11A;
14537
14538                                 // 5G band
14539                                 #ifdef CONFIG_80211AC_VHT
14540                                 if (ptdls_sta->vhtpriv.vht_option)  {
14541                                         sta_band = WIRELESS_11_5AC;
14542                                 }
14543                                 #endif
14544
14545                         } else {
14546                                 if (ptdls_sta->ra_mask & 0xffff000)
14547                                         sta_band |= WIRELESS_11_24N;
14548
14549                                 if (ptdls_sta->ra_mask & 0xff0)
14550                                         sta_band |= WIRELESS_11G;
14551
14552                                 if (ptdls_sta->ra_mask & 0x0f)
14553                                         sta_band |= WIRELESS_11B;
14554                         }
14555                         ptdls_sta->wireless_mode = sta_band;
14556                         ptdls_sta->raid = rtw_hal_networktype_to_raid(padapter,ptdls_sta);
14557                         set_sta_rate(padapter, ptdls_sta);
14558                         //sta mode
14559                         rtw_hal_set_odm_var(padapter,HAL_ODM_STA_INFO,ptdls_sta,_TRUE);
14560                         break;
14561                 }
14562                 case TDLS_SD_PTI:
14563                         ptdls_sta->tdls_sta_state |= TDLS_WAIT_PTR_STATE;
14564                         issue_tdls_peer_traffic_indication(padapter, ptdls_sta);
14565                         _set_timer(&ptdls_sta->pti_timer, TDLS_PTI_TIME);
14566                         break;
14567                 case TDLS_CS_OFF:
14568                         _cancel_timer_ex(&ptdls_sta->base_ch_timer);
14569                         _cancel_timer_ex(&ptdls_sta->off_ch_timer);
14570                         SelectChannel(padapter, pmlmeext->cur_channel);
14571                         ptdls_sta->tdls_sta_state &= ~(TDLS_CH_SWITCH_ON_STATE |
14572                                                                 TDLS_PEER_AT_OFF_STATE |
14573                                                                 TDLS_AT_OFF_CH_STATE);
14574                         DBG_871X("go back to base channel\n ");
14575                         issue_nulldata(padapter, NULL, 0, 0, 0);
14576                         break;
14577                 case TDLS_INIT_CH_SEN:
14578                         rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_INIT_CH_SEN, 0);
14579                         pmlmeext->sitesurvey_res.channel_idx = 0;
14580                         ptdls_sta->option = TDLS_DONE_CH_SEN;
14581                         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_DONE_CH_SEN);
14582                         break;
14583                 case TDLS_DONE_CH_SEN:
14584                         survey_channel = pmlmeext->channel_set[pmlmeext->sitesurvey_res.channel_idx].ChannelNum;
14585                         if(survey_channel){
14586                                 SelectChannel(padapter, survey_channel);
14587                                 ptdlsinfo->cur_channel = survey_channel;
14588                                 pmlmeext->sitesurvey_res.channel_idx++;
14589                                 _set_timer(&ptdls_sta->option_timer, SURVEY_TO);
14590                         }else{
14591                                 SelectChannel(padapter, pmlmeext->cur_channel);
14592
14593                                 rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_DONE_CH_SEN, 0);
14594
14595                                 if(ptdlsinfo->ch_sensing==1){
14596                                         ptdlsinfo->ch_sensing=0;
14597                                         ptdlsinfo->cur_channel=1;
14598                                         min=ptdlsinfo->collect_pkt_num[0];
14599                                         for(i=1; i<MAX_CHANNEL_NUM-1; i++){
14600                                                 if(min > ptdlsinfo->collect_pkt_num[i]){
14601                                                         ptdlsinfo->cur_channel=i+1;
14602                                                         min=ptdlsinfo->collect_pkt_num[i];
14603                                                 }
14604                                                 ptdlsinfo->collect_pkt_num[i]=0;
14605                                         }
14606                                         ptdlsinfo->collect_pkt_num[0]=0;
14607                                         ptdlsinfo->candidate_ch=ptdlsinfo->cur_channel;
14608                                         DBG_871X("TDLS channel sensing done, candidate channel: %02x\n", ptdlsinfo->candidate_ch);
14609                                         ptdlsinfo->cur_channel=0;
14610
14611                                 }
14612
14613                                 if(ptdls_sta->tdls_sta_state & TDLS_PEER_SLEEP_STATE){
14614                                         ptdls_sta->tdls_sta_state |= TDLS_APSD_CHSW_STATE;
14615                                 }else{
14616                                         //send null data with pwrbit==1 before send ch_switching_req to peer STA.
14617                                         issue_nulldata(padapter, NULL, 1, 0, 0);
14618
14619                                         ptdls_sta->tdls_sta_state |= TDLS_CH_SW_INITIATOR_STATE;
14620
14621                                         issue_tdls_ch_switch_req(padapter, ptdls_sta->hwaddr);
14622                                         DBG_871X("issue tdls ch switch req\n");
14623                                 }
14624                         }
14625                         break;
14626                 case TDLS_OFF_CH:
14627                         issue_nulldata(padapter, NULL, 1, 0, 0);
14628                         SelectChannel(padapter, ptdls_sta->off_ch);
14629
14630                         DBG_871X("change channel to tar ch:%02x\n", ptdls_sta->off_ch);
14631                         ptdls_sta->tdls_sta_state |= TDLS_AT_OFF_CH_STATE;
14632                         ptdls_sta->tdls_sta_state &= ~(TDLS_PEER_AT_OFF_STATE);
14633                         _set_timer(&ptdls_sta->option_timer, (u32)ptdls_sta->ch_switch_time);
14634                         break;
14635                 case TDLS_BASE_CH:
14636                         _cancel_timer_ex(&ptdls_sta->base_ch_timer);
14637                         _cancel_timer_ex(&ptdls_sta->off_ch_timer);
14638                         SelectChannel(padapter, pmlmeext->cur_channel);
14639                         ptdls_sta->tdls_sta_state &= ~(TDLS_CH_SWITCH_ON_STATE |
14640                                                                 TDLS_PEER_AT_OFF_STATE |
14641                                                                 TDLS_AT_OFF_CH_STATE);
14642                         DBG_871X("go back to base channel\n ");
14643                         issue_nulldata(padapter, NULL, 0, 0, 0);
14644                         _set_timer(&ptdls_sta->option_timer, (u32)ptdls_sta->ch_switch_time);
14645                         break;
14646                 case TDLS_P_OFF_CH:
14647                         SelectChannel(padapter, pmlmeext->cur_channel);
14648                         issue_nulldata(padapter, NULL, 0, 0, 0);
14649                         DBG_871X("change channel to base ch:%02x\n", pmlmeext->cur_channel);
14650                         ptdls_sta->tdls_sta_state &= ~(TDLS_PEER_AT_OFF_STATE| TDLS_AT_OFF_CH_STATE);
14651                         _set_timer(&ptdls_sta->off_ch_timer, TDLS_STAY_TIME);
14652                         break;
14653                 case TDLS_P_BASE_CH:
14654                         issue_nulldata(ptdls_sta->padapter, NULL, 1, 0, 0);
14655                         SelectChannel(padapter, ptdls_sta->off_ch);
14656                         DBG_871X("change channel to off ch:%02x\n", ptdls_sta->off_ch);
14657                         ptdls_sta->tdls_sta_state |= TDLS_AT_OFF_CH_STATE;
14658                         if((ptdls_sta->tdls_sta_state & TDLS_PEER_AT_OFF_STATE) != TDLS_PEER_AT_OFF_STATE){
14659                                 issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta->hwaddr, 0, 3, 300);
14660                         }
14661                         _set_timer(&ptdls_sta->base_ch_timer, TDLS_STAY_TIME);
14662                         break;
14663                 case TDLS_RS_RCR:
14664                         rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_RS_RCR, 0);
14665                         DBG_871X("wirte REG_RCR, set bit6 on\n");
14666                         break;
14667                 case TDLS_TEAR_STA:
14668                         free_tdls_sta(padapter, ptdls_sta);
14669                         break;
14670
14671         }
14672
14673         //_exit_critical_bh(&(ptdlsinfo->hdl_lock), &irqL);
14674
14675         return H2C_SUCCESS;
14676 #else
14677         return H2C_REJECTED;
14678 #endif //CONFIG_TDLS
14679
14680 }
14681
14682 u8 run_in_thread_hdl(_adapter *padapter, u8 *pbuf)
14683 {
14684         struct RunInThread_param *p;
14685
14686
14687         if (NULL == pbuf)
14688                 return H2C_PARAMETERS_ERROR;
14689         p = (struct RunInThread_param*)pbuf;
14690
14691         if (p->func)
14692                 p->func(p->context);
14693
14694         return H2C_SUCCESS;
14695 }
14696