rtl8723bu/core/rtw_tdls.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2011 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_TDLS_C_
  21
  22 #include <drv_types.h>
  23
  24 #ifdef CONFIG_TDLS
  25 #define ONE_SEC         1000 /* 1000 ms */
  26
  27 extern unsigned char MCS_rate_2R[16];
  28 extern unsigned char MCS_rate_1R[16];
  29 extern void process_wmmps_data(_adapter *padapter, union recv_frame *precv_frame);
  30
  31 void rtw_reset_tdls_info(_adapter* padapter)
  32 {
  33         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
  34
  35         ptdlsinfo->ap_prohibited = _FALSE;
  36
  37         /* For TDLS channel switch, currently we only allow it to work in wifi logo test mode */
  38         if (padapter->registrypriv.wifi_spec == 1)
  39         {
  40                 ptdlsinfo->ch_switch_prohibited = _FALSE;
  41         }
  42         else
  43         {
  44                 ptdlsinfo->ch_switch_prohibited = _TRUE;
  45         }
  46
  47         ptdlsinfo->link_established = _FALSE;
  48         ptdlsinfo->sta_cnt = 0;
  49         ptdlsinfo->sta_maximum = _FALSE;
  50
  51 #ifdef CONFIG_TDLS_CH_SW
  52         ptdlsinfo->chsw_info.ch_sw_state = TDLS_STATE_NONE;
  53         ATOMIC_SET(&ptdlsinfo->chsw_info.chsw_on, _FALSE);
  54         ptdlsinfo->chsw_info.off_ch_num = 0;
  55         ptdlsinfo->chsw_info.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
  56         ptdlsinfo->chsw_info.cur_time = 0;
  57         ptdlsinfo->chsw_info.delay_switch_back = _FALSE;
  58         ptdlsinfo->chsw_info.dump_stack = _FALSE;
  59 #endif
  60
  61         ptdlsinfo->ch_sensing = 0;
  62         ptdlsinfo->watchdog_count = 0;
  63         ptdlsinfo->dev_discovered = _FALSE;
  64
  65 #ifdef CONFIG_WFD
  66         ptdlsinfo->wfd_info = &padapter->wfd_info;
  67 #endif
  68 }
  69
  70 int rtw_init_tdls_info(_adapter* padapter)
  71 {
  72         int     res = _SUCCESS;
  73         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
  74
  75         rtw_reset_tdls_info(padapter);
  76
  77         ptdlsinfo->tdls_enable = _TRUE;
  78 #ifdef CONFIG_TDLS_DRIVER_SETUP
  79         ptdlsinfo->driver_setup = _TRUE;
  80 #else
  81         ptdlsinfo->driver_setup = _FALSE;
  82 #endif /* CONFIG_TDLS_DRIVER_SETUP */
  83
  84         _rtw_spinlock_init(&ptdlsinfo->cmd_lock);
  85         _rtw_spinlock_init(&ptdlsinfo->hdl_lock);
  86
  87         return res;
  88
  89 }
  90
  91 void rtw_free_tdls_info(struct tdls_info *ptdlsinfo)
  92 {
  93         _rtw_spinlock_free(&ptdlsinfo->cmd_lock);
  94         _rtw_spinlock_free(&ptdlsinfo->hdl_lock);
  95
  96         _rtw_memset(ptdlsinfo, 0, sizeof(struct tdls_info) );
  97
  98 }
  99
 100 int check_ap_tdls_prohibited(u8 *pframe, u8 pkt_len)
 101 {
 102         u8 tdls_prohibited_bit = 0x40; /* bit(38); TDLS_prohibited */
 103
 104         if (pkt_len < 5) {
 105                 return _FALSE;
 106         }
 107
 108         pframe += 4;
 109         if ((*pframe) & tdls_prohibited_bit)
 110                 return _TRUE;
 111
 112         return _FALSE;
 113 }
 114
 115 int check_ap_tdls_ch_switching_prohibited(u8 *pframe, u8 pkt_len)
 116 {
 117         u8 tdls_ch_swithcing_prohibited_bit = 0x80; /* bit(39); TDLS_channel_switching prohibited */
 118
 119         if (pkt_len < 5) {
 120                 return _FALSE;
 121         }
 122
 123         pframe += 4;
 124         if ((*pframe) & tdls_ch_swithcing_prohibited_bit)
 125                 return _TRUE;
 126
 127         return _FALSE;
 128 }
 129
 130 u8 rtw_tdls_is_setup_allowed(_adapter *padapter)
 131 {
 132         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
 133
 134         if (ptdlsinfo->ap_prohibited == _TRUE)
 135                 return _FALSE;
 136
 137         return _TRUE;
 138 }
 139
 140 #ifdef CONFIG_TDLS_CH_SW
 141 u8 rtw_tdls_is_chsw_allowed(_adapter *padapter)
 142 {
 143         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
 144
 145         if (ptdlsinfo->ch_switch_prohibited == _TRUE)
 146                 return _FALSE;
 147
 148         if (padapter->registrypriv.wifi_spec == 0)
 149                 return _FALSE;
 150
 151         return _TRUE;
 152 }
 153 #endif
 154
 155 int _issue_nulldata_to_TDLS_peer_STA(_adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
 156 {
 157         int ret = _FAIL;
 158         struct xmit_frame                       *pmgntframe;
 159         struct pkt_attrib                       *pattrib;
 160         unsigned char                                   *pframe;
 161         struct rtw_ieee80211_hdr        *pwlanhdr;
 162         unsigned short                          *fctrl, *qc;
 163         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 164         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 165         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 166
 167         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
 168                 goto exit;
 169
 170         pattrib = &pmgntframe->attrib;
 171         update_mgntframe_attrib(padapter, pattrib);
 172
 173         pattrib->hdrlen +=2;
 174         pattrib->qos_en = _TRUE;
 175         pattrib->eosp = 1;
 176         pattrib->ack_policy = 0;
 177         pattrib->mdata = 0;
 178         pattrib->retry_ctrl = _FALSE;
 179
 180         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 181
 182         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 183         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 184
 185         fctrl = &(pwlanhdr->frame_ctl);
 186         *(fctrl) = 0;
 187
 188         if (power_mode)
 189                 SetPwrMgt(fctrl);
 190
 191         qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
 192
 193         SetPriority(qc, 7);     /* Set priority to VO */
 194
 195         SetEOSP(qc, pattrib->eosp);
 196
 197         SetAckpolicy(qc, pattrib->ack_policy);
 198
 199         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
 200         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
 201         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
 202
 203         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 204         pmlmeext->mgnt_seq++;
 205         SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
 206
 207         pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos);
 208         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos);
 209
 210         pattrib->last_txcmdsz = pattrib->pktlen;
 211
 212         if (wait_ack)
 213                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
 214         else {
 215                 dump_mgntframe(padapter, pmgntframe);
 216                 ret = _SUCCESS;
 217         }
 218
 219 exit:
 220         return ret;
 221
 222 }
 223
 224 /*
 225  *wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT
 226  *wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX
 227  *try_cnt means the maximal TX count to try
 228  */
 229 int issue_nulldata_to_TDLS_peer_STA(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
 230 {
 231         int ret;
 232         int i = 0;
 233         u32 start = rtw_get_current_time();
 234         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 235         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 236
 237         #if 0
 238         psta = rtw_get_stainfo(&padapter->stapriv, da);
 239         if (psta) {
 240                 if (power_mode)
 241                         rtw_hal_macid_sleep(padapter, psta->mac_id);
 242                 else
 243                         rtw_hal_macid_wakeup(padapter, psta->mac_id);
 244         } else {
 245                 DBG_871X(FUNC_ADPT_FMT ": Can't find sta info for " MAC_FMT ", skip macid %s!!\n",
 246                         FUNC_ADPT_ARG(padapter), MAC_ARG(da), power_mode?"sleep":"wakeup");
 247                 rtw_warn_on(1);
 248         }
 249         #endif
 250
 251         do {
 252                 ret = _issue_nulldata_to_TDLS_peer_STA(padapter, da, power_mode, wait_ms>0 ? _TRUE : _FALSE);
 253
 254                 i++;
 255
 256                 if (RTW_CANNOT_RUN(padapter))
 257                         break;
 258
 259                 if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
 260                         rtw_msleep_os(wait_ms);
 261
 262         } while ((i < try_cnt) && (ret==_FAIL || wait_ms==0));
 263
 264         if (ret != _FAIL) {
 265                 ret = _SUCCESS;
 266                 #ifndef DBG_XMIT_ACK
 267                 goto exit;
 268                 #endif
 269         }
 270
 271         if (try_cnt && wait_ms) {
 272                 if (da)
 273                         DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
 274                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
 275                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
 276                 else
 277                         DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
 278                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
 279                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
 280         }
 281 exit:
 282         return ret;
 283 }
 284
 285 void free_tdls_sta(_adapter *padapter, struct sta_info *ptdls_sta)
 286 {
 287         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
 288         struct sta_priv *pstapriv = &padapter->stapriv;
 289         _irqL irqL;
 290
 291         /* free peer sta_info */
 292         _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
 293         if (ptdlsinfo->sta_cnt != 0)
 294                 ptdlsinfo->sta_cnt--;
 295         _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
 296         /* -2: AP + BC/MC sta, -4: default key */
 297         if (ptdlsinfo->sta_cnt < MAX_ALLOWED_TDLS_STA_NUM) {
 298                 ptdlsinfo->sta_maximum = _FALSE;
 299                 _rtw_memset( &ptdlsinfo->ss_record, 0x00, sizeof(struct tdls_ss_record) );
 300         }
 301
 302         /* clear cam */
 303         rtw_clearstakey_cmd(padapter, ptdls_sta, _TRUE);
 304
 305         if (ptdlsinfo->sta_cnt == 0) {
 306                 rtw_tdls_cmd(padapter, NULL, TDLS_RS_RCR);
 307                 ptdlsinfo->link_established = _FALSE;
 308         }
 309         else
 310                 DBG_871X("Remain tdls sta:%02x\n", ptdlsinfo->sta_cnt);
 311
 312         rtw_free_stainfo(padapter,  ptdls_sta);
 313
 314 }
 315
 316
 317 /* TDLS encryption(if needed) will always be CCMP */
 318 void rtw_tdls_set_key(_adapter *padapter, struct sta_info *ptdls_sta)
 319 {
 320         ptdls_sta->dot118021XPrivacy=_AES_;
 321         rtw_setstakey_cmd(padapter, ptdls_sta, TDLS_KEY, _TRUE);
 322 }
 323
 324 #ifdef CONFIG_80211N_HT
 325 void rtw_tdls_process_ht_cap(_adapter *padapter, struct sta_info *ptdls_sta, u8 *data, u8 Length)
 326 {
 327         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 328         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 329         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
 330         struct ht_priv                  *phtpriv = &pmlmepriv->htpriv;
 331         u8      max_AMPDU_len, min_MPDU_spacing;
 332         u8      cur_ldpc_cap = 0, cur_stbc_cap = 0, cur_beamform_cap = 0;
 333
 334         /* Save HT capabilities in the sta object */
 335         _rtw_memset(&ptdls_sta->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));
 336         if (data && Length >= sizeof(struct rtw_ieee80211_ht_cap)) {
 337                 ptdls_sta->flags |= WLAN_STA_HT;
 338                 ptdls_sta->flags |= WLAN_STA_WME;
 339
 340                 _rtw_memcpy(&ptdls_sta->htpriv.ht_cap, data, sizeof(struct rtw_ieee80211_ht_cap));
 341         } else
 342                 ptdls_sta->flags &= ~WLAN_STA_HT;
 343
 344         if (ptdls_sta->flags & WLAN_STA_HT) {
 345                 if (padapter->registrypriv.ht_enable == _TRUE) {
 346                         ptdls_sta->htpriv.ht_option = _TRUE;
 347                         ptdls_sta->qos_option = _TRUE;
 348                 } else {
 349                         ptdls_sta->htpriv.ht_option = _FALSE;
 350                         ptdls_sta->qos_option = _FALSE;
 351                 }
 352         }
 353
 354         /* HT related cap */
 355         if (ptdls_sta->htpriv.ht_option) {
 356                 /* Check if sta supports rx ampdu */
 357                 if (padapter->registrypriv.ampdu_enable == 1)
 358                         ptdls_sta->htpriv.ampdu_enable = _TRUE;
 359
 360                 /* AMPDU Parameters field */
 361                 /* Get MIN of MAX AMPDU Length Exp */
 362                 if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (data[2] & 0x3))
 363                         max_AMPDU_len = (data[2] & 0x3);
 364                 else
 365                         max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3);
 366                 /* Get MAX of MIN MPDU Start Spacing */
 367                 if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (data[2] & 0x1c))
 368                         min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c);
 369                 else
 370                         min_MPDU_spacing = (data[2] & 0x1c);
 371                 ptdls_sta->htpriv.rx_ampdu_min_spacing = max_AMPDU_len | min_MPDU_spacing;
 372
 373                 /* Check if sta support s Short GI 20M */
 374                 if (ptdls_sta->htpriv.ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SGI_20))
 375                         ptdls_sta->htpriv.sgi_20m = _TRUE;
 376
 377                 /* Check if sta support s Short GI 40M */
 378                 if (ptdls_sta->htpriv.ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SGI_40))
 379                         ptdls_sta->htpriv.sgi_40m = _TRUE;
 380
 381                 /* Bwmode would still followed AP's setting */
 382                 if (ptdls_sta->htpriv.ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH)) {
 383                         if (padapter->mlmeextpriv.cur_bwmode >= CHANNEL_WIDTH_40)
 384                                 ptdls_sta->bw_mode = CHANNEL_WIDTH_40;
 385                         ptdls_sta->htpriv.ch_offset = padapter->mlmeextpriv.cur_ch_offset;
 386                 }
 387
 388                 /* Config LDPC Coding Capability */
 389                 if (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX) && GET_HT_CAP_ELE_LDPC_CAP(data)) {
 390                         SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX));
 391                         DBG_871X("Enable HT Tx LDPC!\n");
 392                 }
 393                 ptdls_sta->htpriv.ldpc_cap = cur_ldpc_cap;
 394
 395                 /* Config STBC setting */
 396                 if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) && GET_HT_CAP_ELE_RX_STBC(data)) {
 397                         SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX));
 398                         DBG_871X("Enable HT Tx STBC!\n");
 399                 }
 400                 ptdls_sta->htpriv.stbc_cap = cur_stbc_cap;
 401
 402         }
 403
 404 }
 405
 406 u8 *rtw_tdls_set_ht_cap(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
 407 {
 408         rtw_ht_use_default_setting(padapter);
 409
 410         rtw_restructure_ht_ie(padapter, NULL, pframe, 0, &(pattrib->pktlen), padapter->mlmeextpriv.cur_channel);
 411
 412         return pframe + pattrib->pktlen;
 413 }
 414 #endif
 415
 416 u8 *rtw_tdls_set_sup_ch(struct mlme_ext_priv *pmlmeext, u8 *pframe, struct pkt_attrib *pattrib)
 417 {
 418         u8 sup_ch[30 * 2] = {0x00}, ch_set_idx = 0, sup_ch_idx = 2;
 419
 420         do {
 421                 if (pmlmeext->channel_set[ch_set_idx].ChannelNum <= 14) {
 422                         sup_ch[0] = 1;  /* First channel number */
 423                         sup_ch[1] = pmlmeext->channel_set[ch_set_idx].ChannelNum;       /* Number of channel */
 424                 } else {
 425                         sup_ch[sup_ch_idx++] = pmlmeext->channel_set[ch_set_idx].ChannelNum;
 426                         sup_ch[sup_ch_idx++] = 1;
 427                 }
 428                 ch_set_idx++;
 429         } while (pmlmeext->channel_set[ch_set_idx].ChannelNum != 0 && ch_set_idx < MAX_CHANNEL_NUM);
 430
 431         return rtw_set_ie(pframe, _SUPPORTED_CH_IE_, sup_ch_idx, sup_ch, &(pattrib->pktlen));
 432 }
 433
 434 u8 *rtw_tdls_set_rsnie(struct tdls_txmgmt *ptxmgmt, u8 *pframe, struct pkt_attrib *pattrib,  int init, struct sta_info *ptdls_sta)
 435 {
 436         u8 *p = NULL;
 437         int len = 0;
 438
 439         if (ptxmgmt->len > 0)
 440                 p = rtw_get_ie(ptxmgmt->buf, _RSN_IE_2_, &len, ptxmgmt->len);
 441
 442         if (p != NULL)
 443                 return rtw_set_ie(pframe, _RSN_IE_2_, len, p+2, &(pattrib->pktlen));
 444         else
 445                 if (init == _TRUE)
 446                         return rtw_set_ie(pframe, _RSN_IE_2_, sizeof(TDLS_RSNIE), TDLS_RSNIE, &(pattrib->pktlen));
 447                 else
 448                         return rtw_set_ie(pframe, _RSN_IE_2_, sizeof(ptdls_sta->TDLS_RSNIE), ptdls_sta->TDLS_RSNIE, &(pattrib->pktlen));
 449 }
 450
 451 u8 *rtw_tdls_set_ext_cap(u8 *pframe, struct pkt_attrib *pattrib)
 452 {
 453         return rtw_set_ie(pframe, _EXT_CAP_IE_ , sizeof(TDLS_EXT_CAPIE), TDLS_EXT_CAPIE, &(pattrib->pktlen));
 454 }
 455
 456 u8 *rtw_tdls_set_qos_cap(u8 *pframe, struct pkt_attrib *pattrib)
 457 {
 458         return rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, sizeof(TDLS_WMMIE), TDLS_WMMIE,  &(pattrib->pktlen));
 459 }
 460
 461 u8 *rtw_tdls_set_ftie(struct tdls_txmgmt *ptxmgmt, u8 *pframe, struct pkt_attrib *pattrib, u8 *ANonce, u8 *SNonce)
 462 {
 463         struct wpa_tdls_ftie FTIE = {0};
 464         u8 *p = NULL;
 465         int len = 0;
 466
 467         if (ptxmgmt->len > 0)
 468                 p = rtw_get_ie(ptxmgmt->buf, _FTIE_, &len, ptxmgmt->len);
 469
 470         if (p != NULL)
 471                 return rtw_set_ie(pframe, _FTIE_, len, p+2, &(pattrib->pktlen));
 472         else {
 473                 if (ANonce != NULL)
 474                         _rtw_memcpy(FTIE.Anonce, ANonce, WPA_NONCE_LEN);
 475                 if (SNonce != NULL)
 476                         _rtw_memcpy(FTIE.Snonce, SNonce, WPA_NONCE_LEN);
 477                 return rtw_set_ie(pframe, _FTIE_ , 82, (u8 *)FTIE.mic_ctrl, &(pattrib->pktlen));
 478         }
 479 }
 480
 481 u8 *rtw_tdls_set_timeout_interval(struct tdls_txmgmt *ptxmgmt, u8 *pframe, struct pkt_attrib *pattrib, int init, struct sta_info *ptdls_sta)
 482 {
 483         u8 timeout_itvl[5];     /* set timeout interval to maximum value */
 484         u32 timeout_interval = TDLS_TPK_RESEND_COUNT;
 485         u8 *p = NULL;
 486         int len = 0;
 487
 488         if (ptxmgmt->len > 0)
 489                 p = rtw_get_ie(ptxmgmt->buf, _TIMEOUT_ITVL_IE_, &len, ptxmgmt->len);
 490
 491         if (p != NULL)
 492                 return rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, len, p+2, &(pattrib->pktlen));
 493         else {
 494                 /* Timeout interval */
 495                 timeout_itvl[0]=0x02;
 496                 if (init == _TRUE)
 497                         _rtw_memcpy(timeout_itvl+1, &timeout_interval, 4);
 498                 else
 499                         _rtw_memcpy(timeout_itvl+1, (u8 *)(&ptdls_sta->TDLS_PeerKey_Lifetime), 4);
 500
 501                 return rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, 5, timeout_itvl, &(pattrib->pktlen));
 502         }
 503 }
 504
 505 u8 *rtw_tdls_set_bss_coexist(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
 506 {
 507         u8 iedata=0;
 508
 509         if (padapter->mlmepriv.num_FortyMHzIntolerant > 0)
 510                 iedata |= BIT(2);       /* 20 MHz BSS Width Request */
 511
 512         /* Information Bit should be set by TDLS test plan 5.9 */
 513         iedata |= BIT(0);
 514         return rtw_set_ie(pframe, EID_BSSCoexistence, 1, &iedata, &(pattrib->pktlen));
 515 }
 516
 517 u8 *rtw_tdls_set_payload_type(u8 *pframe, struct pkt_attrib *pattrib)
 518 {
 519         u8 payload_type = 0x02;
 520         return rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));
 521 }
 522
 523 u8 *rtw_tdls_set_category(u8 *pframe, struct pkt_attrib *pattrib, u8 category)
 524 {
 525         return rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
 526 }
 527
 528 u8 *rtw_tdls_set_action(u8 *pframe, struct pkt_attrib *pattrib, struct tdls_txmgmt *ptxmgmt)
 529 {
 530         return rtw_set_fixed_ie(pframe, 1, &(ptxmgmt->action_code), &(pattrib->pktlen));
 531 }
 532
 533 u8 *rtw_tdls_set_status_code(u8 *pframe, struct pkt_attrib *pattrib, struct tdls_txmgmt *ptxmgmt)
 534 {
 535         return rtw_set_fixed_ie(pframe, 2, (u8 *)&(ptxmgmt->status_code), &(pattrib->pktlen));
 536 }
 537
 538 u8 *rtw_tdls_set_dialog(u8 *pframe, struct pkt_attrib *pattrib, struct tdls_txmgmt *ptxmgmt)
 539 {
 540         u8 dialogtoken = 1;
 541         if (ptxmgmt->dialog_token)
 542                 return rtw_set_fixed_ie(pframe, 1, &(ptxmgmt->dialog_token), &(pattrib->pktlen));
 543         else
 544                 return rtw_set_fixed_ie(pframe, 1, &(dialogtoken), &(pattrib->pktlen));
 545 }
 546
 547 u8 *rtw_tdls_set_reg_class(u8 *pframe, struct pkt_attrib *pattrib, struct sta_info *ptdls_sta)
 548 {
 549         u8 reg_class = 22;
 550         return rtw_set_fixed_ie(pframe, 1, &(reg_class), &(pattrib->pktlen));
 551 }
 552
 553 u8 *rtw_tdls_set_second_channel_offset(u8 *pframe, struct pkt_attrib *pattrib, u8 ch_offset)
 554 {
 555         return rtw_set_ie(pframe, EID_SecondaryChnlOffset , 1, &ch_offset, &(pattrib->pktlen));
 556 }
 557
 558 u8 *rtw_tdls_set_capability(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
 559 {
 560         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 561         struct mlme_ext_info    *pmlmeinfo = &pmlmeext->mlmext_info;
 562         u8 cap_from_ie[2] = {0};
 563
 564         _rtw_memcpy(cap_from_ie, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
 565
 566         return rtw_set_fixed_ie(pframe, 2, cap_from_ie, &(pattrib->pktlen));
 567 }
 568
 569 u8 *rtw_tdls_set_supported_rate(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
 570 {
 571         u8 bssrate[NDIS_802_11_LENGTH_RATES_EX];
 572         int bssrate_len = 0;
 573         u8 more_supportedrates = 0;
 574
 575         rtw_set_supported_rate(bssrate, (padapter->registrypriv.wireless_mode == WIRELESS_MODE_MAX) ? padapter->mlmeextpriv.cur_wireless_mode : padapter->registrypriv.wireless_mode);
 576         bssrate_len = rtw_get_rateset_len(bssrate);
 577
 578         if (bssrate_len > 8) {
 579                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
 580                 more_supportedrates = 1;
 581         } else {
 582                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
 583         }
 584
 585         /* extended supported rates */
 586         if (more_supportedrates == 1) {
 587                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
 588         }
 589
 590         return pframe;
 591 }
 592
 593 u8 *rtw_tdls_set_sup_reg_class(u8 *pframe, struct pkt_attrib *pattrib)
 594 {
 595         return rtw_set_ie(pframe, _SRC_IE_ , sizeof(TDLS_SRC), TDLS_SRC, &(pattrib->pktlen));
 596 }
 597
 598 u8 *rtw_tdls_set_linkid(u8 *pframe, struct pkt_attrib *pattrib, u8 init)
 599 {
 600         u8 link_id_addr[18] = {0};
 601         if (init == _TRUE) {
 602                 _rtw_memcpy(link_id_addr, pattrib->ra, 6);
 603                 _rtw_memcpy((link_id_addr+6), pattrib->src, 6);
 604                 _rtw_memcpy((link_id_addr+12), pattrib->dst, 6);
 605         } else {
 606                 _rtw_memcpy(link_id_addr, pattrib->ra, 6);
 607                 _rtw_memcpy((link_id_addr+6), pattrib->dst, 6);
 608                 _rtw_memcpy((link_id_addr+12), pattrib->src, 6);
 609         }
 610         return rtw_set_ie(pframe, _LINK_ID_IE_, 18, link_id_addr, &(pattrib->pktlen));
 611 }
 612
 613 #ifdef CONFIG_TDLS_CH_SW
 614 u8 *rtw_tdls_set_target_ch(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
 615 {
 616         u8 target_ch = 1;
 617         if (padapter->tdlsinfo.chsw_info.off_ch_num)
 618                 return rtw_set_fixed_ie(pframe, 1, &(padapter->tdlsinfo.chsw_info.off_ch_num), &(pattrib->pktlen));
 619         else
 620                 return rtw_set_fixed_ie(pframe, 1, &(target_ch), &(pattrib->pktlen));
 621 }
 622
 623 u8 *rtw_tdls_set_ch_sw(u8 *pframe, struct pkt_attrib *pattrib, struct sta_info *ptdls_sta)
 624 {
 625         u8 ch_switch_timing[4] = {0};
 626         u16 switch_time = (ptdls_sta->ch_switch_time >= TDLS_CH_SWITCH_TIME * 1000) ?
 627                 ptdls_sta->ch_switch_time : TDLS_CH_SWITCH_TIME;
 628         u16 switch_timeout = (ptdls_sta->ch_switch_timeout >= TDLS_CH_SWITCH_TIMEOUT * 1000) ?
 629                 ptdls_sta->ch_switch_timeout : TDLS_CH_SWITCH_TIMEOUT;
 630
 631         _rtw_memcpy(ch_switch_timing, &switch_time, 2);
 632         _rtw_memcpy(ch_switch_timing + 2, &switch_timeout, 2);
 633
 634         return rtw_set_ie(pframe, _CH_SWITCH_TIMING_,  4, ch_switch_timing, &(pattrib->pktlen));
 635 }
 636
 637 void rtw_tdls_set_ch_sw_oper_control(_adapter *padapter, u8 enable)
 638 {
 639         if (ATOMIC_READ(&padapter->tdlsinfo.chsw_info.chsw_on) != enable)
 640                 ATOMIC_SET(&padapter->tdlsinfo.chsw_info.chsw_on, enable);
 641
 642         rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_BCN_EARLY_C2H_RPT, &enable);
 643         DBG_871X("[TDLS] %s Bcn Early C2H Report\n", (enable == _TRUE) ? "Start" : "Stop");
 644 }
 645
 646 void rtw_tdls_ch_sw_back_to_base_chnl(_adapter *padapter)
 647 {
 648         struct mlme_priv *pmlmepriv;
 649         struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
 650
 651         pmlmepriv = &padapter->mlmepriv;
 652
 653         if ((ATOMIC_READ(&pchsw_info->chsw_on) == _TRUE) &&
 654         (padapter->mlmeextpriv.cur_channel != rtw_get_oper_ch(padapter)))
 655                 rtw_tdls_cmd(padapter, pchsw_info->addr, TDLS_CH_SW_TO_BASE_CHNL_UNSOLICITED);
 656 }
 657
 658 static void rtw_tdls_chsw_oper_init(_adapter *padapter, u32 timeout_ms)
 659 {
 660         struct submit_ctx       *chsw_sctx = &padapter->tdlsinfo.chsw_info.chsw_sctx;
 661
 662         rtw_sctx_init(chsw_sctx, timeout_ms);
 663 }
 664
 665 static int rtw_tdls_chsw_oper_wait(_adapter *padapter)
 666 {
 667         struct submit_ctx       *chsw_sctx = &padapter->tdlsinfo.chsw_info.chsw_sctx;
 668
 669         return rtw_sctx_wait(chsw_sctx, __func__);
 670 }
 671
 672 void rtw_tdls_chsw_oper_done(_adapter *padapter)
 673 {
 674         struct submit_ctx       *chsw_sctx = &padapter->tdlsinfo.chsw_info.chsw_sctx;
 675
 676         rtw_sctx_done(&chsw_sctx);
 677 }
 678
 679 s32 rtw_tdls_do_ch_sw(_adapter *padapter, u8 chnl_type, u8 channel, u8 channel_offset, u16 bwmode, u16 ch_switch_time)
 680 {
 681         u32 ch_sw_time_start, ch_sw_time_spent, wait_time;
 682         u8 take_care_iqk;
 683         s32 ret = _FAIL;
 684
 685         ch_sw_time_start = rtw_systime_to_ms(rtw_get_current_time());
 686
 687         rtw_tdls_chsw_oper_init(padapter, TDLS_CH_SWITCH_OPER_OFFLOAD_TIMEOUT);
 688
 689         /* channel switch IOs offload to FW */
 690         if (rtw_hal_ch_sw_oper_offload(padapter, channel, channel_offset, bwmode) == _SUCCESS) {
 691                 if (rtw_tdls_chsw_oper_wait(padapter) == _SUCCESS) {
 692                         /* set channel and bw related variables in driver */
 693                         _enter_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL);
 694                         rtw_set_oper_ch(padapter, channel);
 695                         rtw_set_oper_choffset(padapter, channel_offset);
 696                         rtw_set_oper_bw(padapter, bwmode);
 697                         _exit_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL);
 698
 699                         rtw_hal_get_hwreg(padapter, HW_VAR_CH_SW_NEED_TO_TAKE_CARE_IQK_INFO, &take_care_iqk);
 700                         if (take_care_iqk == _TRUE)
 701                                 rtw_hal_ch_sw_iqk_info_restore(padapter, CH_SW_USE_CASE_TDLS);
 702
 703                         ch_sw_time_spent = rtw_systime_to_ms(rtw_get_current_time()) - ch_sw_time_start;
 704
 705                         if (chnl_type == TDLS_CH_SW_OFF_CHNL) {
 706                                 if ((u32)ch_switch_time / 1000 > ch_sw_time_spent)
 707                                         wait_time = (u32)ch_switch_time / 1000 - ch_sw_time_spent;
 708                                 else
 709                                         wait_time = 0;
 710
 711                                 if (wait_time > 0)
 712                                         rtw_msleep_os(wait_time);
 713                         }
 714
 715                         ret = _SUCCESS;
 716                 } else
 717                         DBG_871X("[TDLS] chsw oper wait fail !!\n");
 718         }
 719
 720         return ret;
 721 }
 722 #endif
 723
 724 u8 *rtw_tdls_set_wmm_params(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
 725 {
 726         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 727         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 728         u8 wmm_param_ele[24] = {0};
 729
 730         if (&pmlmeinfo->WMM_param) {
 731                 _rtw_memcpy(wmm_param_ele, WMM_PARA_OUI, 6);
 732                 if (_rtw_memcmp(&pmlmeinfo->WMM_param, &wmm_param_ele[6], 18) == _TRUE)
 733                         /* Use default WMM Param */
 734                         _rtw_memcpy(wmm_param_ele + 6, (u8 *)&TDLS_WMM_PARAM_IE, sizeof(TDLS_WMM_PARAM_IE));
 735                 else
 736                         _rtw_memcpy(wmm_param_ele + 6, (u8 *)&pmlmeinfo->WMM_param, sizeof(pmlmeinfo->WMM_param));
 737                 return rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_,  24, wmm_param_ele, &(pattrib->pktlen));
 738         }
 739         else
 740                 return pframe;
 741 }
 742
 743 #ifdef CONFIG_WFD
 744 void rtw_tdls_process_wfd_ie(struct tdls_info *ptdlsinfo, u8 *ptr, u8 length)
 745 {
 746         u8 *wfd_ie;
 747         u32     wfd_ielen = 0;
 748
 749         if (!hal_chk_wl_func(tdls_info_to_adapter(ptdlsinfo), WL_FUNC_MIRACAST))
 750                 return;
 751
 752         /* Try to get the TCP port information when receiving the negotiation response. */
 753
 754         wfd_ie = rtw_get_wfd_ie(ptr, length, NULL, &wfd_ielen);
 755         while (wfd_ie) {
 756                 u8 *attr_content;
 757                 u32     attr_contentlen = 0;
 758                 int     i;
 759
 760                 DBG_871X( "[%s] WFD IE Found!!\n", __FUNCTION__ );
 761                 attr_content = rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, NULL, &attr_contentlen);
 762                 if (attr_content && attr_contentlen) {
 763                         ptdlsinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16( attr_content + 2 );
 764                         DBG_871X( "[%s] Peer PORT NUM = %d\n", __FUNCTION__, ptdlsinfo->wfd_info->peer_rtsp_ctrlport );
 765                 }
 766
 767                 attr_content = rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_LOCAL_IP_ADDR, NULL, &attr_contentlen);
 768                 if (attr_content && attr_contentlen) {
 769                         _rtw_memcpy(ptdlsinfo->wfd_info->peer_ip_address, ( attr_content + 1 ), 4);
 770                         DBG_871X("[%s] Peer IP = %02u.%02u.%02u.%02u\n", __FUNCTION__,
 771                                 ptdlsinfo->wfd_info->peer_ip_address[0], ptdlsinfo->wfd_info->peer_ip_address[1],
 772                                 ptdlsinfo->wfd_info->peer_ip_address[2], ptdlsinfo->wfd_info->peer_ip_address[3]);
 773                 }
 774
 775                 wfd_ie = rtw_get_wfd_ie(wfd_ie + wfd_ielen, (ptr + length) - (wfd_ie + wfd_ielen), NULL, &wfd_ielen);
 776         }
 777 }
 778
 779 int issue_tunneled_probe_req(_adapter *padapter)
 780 {
 781         struct xmit_frame                       *pmgntframe;
 782         struct pkt_attrib                       *pattrib;
 783         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
 784         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 785         u8 baddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 786         struct tdls_txmgmt txmgmt;
 787         int ret = _FAIL;
 788
 789         DBG_871X("[%s]\n", __FUNCTION__);
 790
 791         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
 792         txmgmt.action_code = TUNNELED_PROBE_REQ;
 793
 794         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
 795                 goto exit;
 796
 797         pattrib = &pmgntframe->attrib;
 798
 799         pmgntframe->frame_tag = DATA_FRAMETAG;
 800         pattrib->ether_type = 0x890d;
 801
 802         _rtw_memcpy(pattrib->dst, baddr, ETH_ALEN);
 803         _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
 804         _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
 805         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
 806
 807         update_tdls_attrib(padapter, pattrib);
 808         pattrib->qsel = pattrib->priority;
 809         if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) != _SUCCESS) {
 810                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
 811                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
 812                 goto exit;
 813         }
 814         dump_mgntframe(padapter, pmgntframe);
 815         ret = _SUCCESS;
 816 exit:
 817
 818         return ret;
 819 }
 820
 821 int issue_tunneled_probe_rsp(_adapter *padapter, union recv_frame *precv_frame)
 822 {
 823         struct xmit_frame                       *pmgntframe;
 824         struct pkt_attrib                       *pattrib;
 825         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
 826         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 827         struct tdls_txmgmt txmgmt;
 828         int ret = _FAIL;
 829
 830         DBG_871X("[%s]\n", __FUNCTION__);
 831
 832         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
 833         txmgmt.action_code = TUNNELED_PROBE_RSP;
 834
 835         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
 836                 goto exit;
 837
 838         pattrib = &pmgntframe->attrib;
 839
 840         pmgntframe->frame_tag = DATA_FRAMETAG;
 841         pattrib->ether_type = 0x890d;
 842
 843         _rtw_memcpy(pattrib->dst, precv_frame->u.hdr.attrib.src, ETH_ALEN);
 844         _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
 845         _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
 846         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
 847
 848         update_tdls_attrib(padapter, pattrib);
 849         pattrib->qsel = pattrib->priority;
 850         if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) != _SUCCESS) {
 851                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
 852                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
 853                 goto exit;
 854         }
 855         dump_mgntframe(padapter, pmgntframe);
 856         ret = _SUCCESS;
 857 exit:
 858
 859         return ret;
 860 }
 861 #endif /* CONFIG_WFD */
 862
 863 int issue_tdls_setup_req(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, int wait_ack)
 864 {
 865         struct tdls_info        *ptdlsinfo = &padapter->tdlsinfo;
 866         struct xmit_frame                       *pmgntframe;
 867         struct pkt_attrib                       *pattrib;
 868         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
 869         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 870         struct sta_priv *pstapriv = &padapter->stapriv;
 871         struct sta_info *ptdls_sta= NULL;
 872         _irqL irqL;
 873         int ret = _FAIL;
 874         /* Retry timer should be set at least 301 sec, using TPK_count counting 301 times. */
 875         u32 timeout_interval = TDLS_TPK_RESEND_COUNT;
 876
 877         DBG_871X("[TDLS] %s\n", __FUNCTION__);
 878
 879         ptxmgmt->action_code = TDLS_SETUP_REQUEST;
 880         if (rtw_tdls_is_setup_allowed(padapter) == _FALSE)
 881                 goto exit;
 882
 883         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
 884                 goto exit;
 885
 886         pattrib = &pmgntframe->attrib;
 887         pmgntframe->frame_tag = DATA_FRAMETAG;
 888         pattrib->ether_type = 0x890d;
 889
 890         _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
 891         _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
 892         _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
 893         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
 894
 895         update_tdls_attrib(padapter, pattrib);
 896
 897         /* init peer sta_info */
 898         ptdls_sta = rtw_get_stainfo(pstapriv, ptxmgmt->peer);
 899         if (ptdls_sta == NULL) {
 900                 ptdls_sta = rtw_alloc_stainfo(pstapriv, ptxmgmt->peer);
 901                 if (ptdls_sta == NULL) {
 902                         DBG_871X("[%s] rtw_alloc_stainfo fail\n", __FUNCTION__);
 903                         rtw_free_xmitbuf(pxmitpriv,pmgntframe->pxmitbuf);
 904                         rtw_free_xmitframe(pxmitpriv, pmgntframe);
 905                         goto exit;
 906                 }
 907         }
 908
 909         if(!(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE))
 910                 ptdlsinfo->sta_cnt++;
 911
 912         if (ptdlsinfo->sta_cnt == MAX_ALLOWED_TDLS_STA_NUM)
 913                 ptdlsinfo->sta_maximum  = _TRUE;
 914
 915         ptdls_sta->tdls_sta_state |= TDLS_RESPONDER_STATE;
 916
 917         if (rtw_tdls_is_driver_setup(padapter) == _TRUE) {
 918                 ptdls_sta->TDLS_PeerKey_Lifetime = timeout_interval;
 919                 _set_timer(&ptdls_sta->handshake_timer, TDLS_HANDSHAKE_TIME);
 920         }
 921
 922         pattrib->qsel = pattrib->priority;
 923
 924         if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) !=_SUCCESS) {
 925                 rtw_free_xmitbuf(pxmitpriv,pmgntframe->pxmitbuf);
 926                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
 927                 goto exit;
 928         }
 929
 930         if (wait_ack) {
 931                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
 932         } else {
 933                 dump_mgntframe(padapter, pmgntframe);
 934                 ret = _SUCCESS;
 935         }
 936
 937 exit:
 938
 939         return ret;
 940 }
 941
 942 int _issue_tdls_teardown(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, u8 wait_ack)
 943 {
 944         struct xmit_frame                       *pmgntframe;
 945         struct pkt_attrib                       *pattrib;
 946         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
 947         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 948         struct sta_priv *pstapriv = &padapter->stapriv;
 949         struct sta_info *ptdls_sta=NULL;
 950         _irqL irqL;
 951         int ret = _FAIL;
 952
 953         DBG_871X("[TDLS] %s\n", __FUNCTION__);
 954
 955         ptxmgmt->action_code = TDLS_TEARDOWN;
 956         ptdls_sta = rtw_get_stainfo(pstapriv, ptxmgmt->peer);
 957         if (ptdls_sta == NULL) {
 958                 DBG_871X("Np tdls_sta for tearing down\n");
 959                 goto exit;
 960         }
 961
 962         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
 963                 goto exit;
 964
 965         rtw_set_scan_deny(padapter, 550);
 966
 967         rtw_scan_abort(padapter);
 968 #ifdef CONFIG_CONCURRENT_MODE
 969         if (rtw_buddy_adapter_up(padapter))
 970                 rtw_scan_abort(padapter->pbuddy_adapter);
 971 #endif /* CONFIG_CONCURRENT_MODE */
 972
 973         pattrib = &pmgntframe->attrib;
 974
 975         pmgntframe->frame_tag = DATA_FRAMETAG;
 976         pattrib->ether_type = 0x890d;
 977
 978         _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
 979         _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
 980         _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
 981         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
 982
 983         update_tdls_attrib(padapter, pattrib);
 984         pattrib->qsel = pattrib->priority;
 985         if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) {
 986                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
 987                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
 988                 goto exit;
 989         }
 990
 991         if (rtw_tdls_is_driver_setup(padapter) == _TRUE)
 992                 if(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)
 993                         if (pattrib->encrypt)
 994                                 _cancel_timer_ex(&ptdls_sta->TPK_timer);
 995
 996         if (wait_ack) {
 997                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
 998         } else {
 999                 dump_mgntframe(padapter, pmgntframe);
1000                 ret = _SUCCESS;
1001         }
1002
1003         if (rtw_tdls_is_driver_setup(padapter))
1004                 rtw_tdls_cmd(padapter, ptxmgmt->peer, TDLS_TEARDOWN_STA_LOCALLY);
1005
1006 exit:
1007
1008         return ret;
1009 }
1010
1011 int issue_tdls_teardown(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, u8 wait_ack)
1012 {
1013         int ret = _FAIL;
1014
1015         ret = _issue_tdls_teardown(padapter, ptxmgmt, wait_ack);
1016         if ((ptxmgmt->status_code == _RSON_TDLS_TEAR_UN_RSN_) && (ret == _FAIL)) {
1017                 /* Change status code and send teardown again via AP */
1018                 ptxmgmt->status_code = _RSON_TDLS_TEAR_TOOFAR_;
1019                 ret = _issue_tdls_teardown(padapter, ptxmgmt, wait_ack);
1020         }
1021
1022         return ret;
1023 }
1024
1025 int issue_tdls_dis_req(_adapter *padapter, struct tdls_txmgmt *ptxmgmt)
1026 {
1027         struct xmit_frame                       *pmgntframe;
1028         struct pkt_attrib                       *pattrib;
1029         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
1030         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
1031         int ret = _FAIL;
1032
1033         DBG_871X("[TDLS] %s\n", __FUNCTION__);
1034
1035         ptxmgmt->action_code = TDLS_DISCOVERY_REQUEST;
1036         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
1037                 goto exit;
1038
1039         pattrib = &pmgntframe->attrib;
1040         pmgntframe->frame_tag = DATA_FRAMETAG;
1041         pattrib->ether_type = 0x890d;
1042
1043         _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
1044         _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
1045         _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
1046         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
1047
1048         update_tdls_attrib(padapter, pattrib);
1049         pattrib->qsel = pattrib->priority;
1050         if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) {
1051                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
1052                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
1053                 goto exit;
1054         }
1055         dump_mgntframe(padapter, pmgntframe);
1056         DBG_871X("issue tdls dis req\n");
1057
1058         ret = _SUCCESS;
1059 exit:
1060
1061         return ret;
1062 }
1063
1064 int issue_tdls_setup_rsp(_adapter *padapter, struct tdls_txmgmt *ptxmgmt)
1065 {
1066         struct xmit_frame                       *pmgntframe;
1067         struct pkt_attrib                       *pattrib;
1068         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
1069         int ret = _FAIL;
1070
1071         DBG_871X("[TDLS] %s\n", __FUNCTION__);
1072
1073         ptxmgmt->action_code = TDLS_SETUP_RESPONSE;
1074         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
1075                 goto exit;
1076
1077         pattrib = &pmgntframe->attrib;
1078         pmgntframe->frame_tag = DATA_FRAMETAG;
1079         pattrib->ether_type = 0x890d;
1080
1081         _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
1082         _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
1083         _rtw_memcpy(pattrib->ra, get_bssid(&(padapter->mlmepriv)), ETH_ALEN);
1084         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
1085
1086         update_tdls_attrib(padapter, pattrib);
1087         pattrib->qsel = pattrib->priority;
1088         if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) {
1089                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
1090                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
1091                 goto exit;
1092         }
1093
1094         dump_mgntframe(padapter, pmgntframe);
1095
1096         ret = _SUCCESS;
1097 exit:
1098
1099         return ret;
1100
1101 }
1102
1103 int issue_tdls_setup_cfm(_adapter *padapter, struct tdls_txmgmt *ptxmgmt)
1104 {
1105         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
1106         struct xmit_frame                       *pmgntframe;
1107         struct pkt_attrib                       *pattrib;
1108         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
1109         int ret = _FAIL;
1110
1111         DBG_871X("[TDLS] %s\n", __FUNCTION__);
1112
1113         ptxmgmt->action_code = TDLS_SETUP_CONFIRM;
1114         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
1115                 goto exit;
1116
1117         pattrib = &pmgntframe->attrib;
1118         pmgntframe->frame_tag = DATA_FRAMETAG;
1119         pattrib->ether_type = 0x890d;
1120
1121         _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
1122         _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
1123         _rtw_memcpy(pattrib->ra, get_bssid(&padapter->mlmepriv), ETH_ALEN);
1124         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
1125
1126         update_tdls_attrib(padapter, pattrib);
1127         pattrib->qsel = pattrib->priority;
1128         if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) {
1129                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
1130                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
1131                 goto exit;
1132         }
1133
1134         dump_mgntframe(padapter, pmgntframe);
1135
1136         ret = _SUCCESS;
1137 exit:
1138
1139         return ret;
1140
1141 }
1142
1143 /* TDLS Discovery Response frame is a management action frame */
1144 int issue_tdls_dis_rsp(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, u8 privacy)
1145 {
1146         struct xmit_frame               *pmgntframe;
1147         struct pkt_attrib               *pattrib;
1148         unsigned char                   *pframe;
1149         struct rtw_ieee80211_hdr        *pwlanhdr;
1150         unsigned short          *fctrl;
1151         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
1152         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
1153         int ret = _FAIL;
1154
1155         DBG_871X("[TDLS] %s\n", __FUNCTION__);
1156
1157         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
1158                 goto exit;
1159
1160         pattrib = &pmgntframe->attrib;
1161         update_mgntframe_attrib(padapter, pattrib);
1162
1163         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
1164
1165         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
1166         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
1167
1168         fctrl = &(pwlanhdr->frame_ctl);
1169         *(fctrl) = 0;
1170
1171         /* unicast probe request frame */
1172         _rtw_memcpy(pwlanhdr->addr1, ptxmgmt->peer, ETH_ALEN);
1173         _rtw_memcpy(pattrib->dst, pwlanhdr->addr1, ETH_ALEN);
1174         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
1175         _rtw_memcpy(pattrib->src, pwlanhdr->addr2, ETH_ALEN);
1176         _rtw_memcpy(pwlanhdr->addr3, get_bssid(&padapter->mlmepriv), ETH_ALEN);
1177         _rtw_memcpy(pattrib->ra, pwlanhdr->addr3, ETH_ALEN);
1178
1179         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
1180         pmlmeext->mgnt_seq++;
1181         SetFrameSubType(pframe, WIFI_ACTION);
1182
1183         pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
1184         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
1185
1186         rtw_build_tdls_dis_rsp_ies(padapter, pmgntframe, pframe, ptxmgmt, privacy);
1187
1188         pattrib->nr_frags = 1;
1189         pattrib->last_txcmdsz = pattrib->pktlen;
1190
1191         dump_mgntframe(padapter, pmgntframe);
1192         ret = _SUCCESS;
1193
1194 exit:
1195         return ret;
1196 }
1197
1198 int issue_tdls_peer_traffic_rsp(_adapter *padapter, struct sta_info *ptdls_sta, struct tdls_txmgmt *ptxmgmt)
1199 {
1200         struct xmit_frame       *pmgntframe;
1201         struct pkt_attrib       *pattrib;
1202         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
1203         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
1204         int ret = _FAIL;
1205
1206         DBG_871X("[TDLS] %s\n", __FUNCTION__);
1207
1208         ptxmgmt->action_code = TDLS_PEER_TRAFFIC_RESPONSE;
1209
1210         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
1211                 goto exit;
1212
1213         pattrib = &pmgntframe->attrib;
1214
1215         pmgntframe->frame_tag = DATA_FRAMETAG;
1216         pattrib->ether_type = 0x890d;
1217
1218         _rtw_memcpy(pattrib->dst, ptdls_sta->hwaddr, ETH_ALEN);
1219         _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
1220         _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
1221         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
1222
1223         update_tdls_attrib(padapter, pattrib);
1224         pattrib->qsel = pattrib->priority;
1225
1226         if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) !=_SUCCESS) {
1227                 rtw_free_xmitbuf(pxmitpriv,pmgntframe->pxmitbuf);
1228                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
1229                 goto exit;
1230         }
1231
1232         dump_mgntframe(padapter, pmgntframe);
1233         ret = _SUCCESS;
1234
1235 exit:
1236
1237         return ret;
1238 }
1239
1240 int issue_tdls_peer_traffic_indication(_adapter *padapter, struct sta_info *ptdls_sta)
1241 {
1242         struct xmit_frame                       *pmgntframe;
1243         struct pkt_attrib                       *pattrib;
1244         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
1245         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
1246         struct tdls_txmgmt txmgmt;
1247         int ret = _FAIL;
1248
1249         DBG_871X("[TDLS] %s\n", __FUNCTION__);
1250
1251         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
1252         txmgmt.action_code = TDLS_PEER_TRAFFIC_INDICATION;
1253
1254         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
1255                 goto exit;
1256
1257         pattrib = &pmgntframe->attrib;
1258
1259         pmgntframe->frame_tag = DATA_FRAMETAG;
1260         pattrib->ether_type = 0x890d;
1261
1262         _rtw_memcpy(pattrib->dst, ptdls_sta->hwaddr, ETH_ALEN);
1263         _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
1264         _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
1265         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
1266
1267         /* PTI frame's priority should be AC_VO */
1268         pattrib->priority = 7;
1269
1270         update_tdls_attrib(padapter, pattrib);
1271         pattrib->qsel = pattrib->priority;
1272         if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) != _SUCCESS) {
1273                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
1274                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
1275                 goto exit;
1276         }
1277
1278         dump_mgntframe(padapter, pmgntframe);
1279         ret = _SUCCESS;
1280
1281 exit:
1282
1283         return ret;
1284 }
1285
1286 #ifdef CONFIG_TDLS_CH_SW
1287 int issue_tdls_ch_switch_req(_adapter *padapter, struct sta_info *ptdls_sta)
1288 {
1289         struct xmit_frame       *pmgntframe;
1290         struct pkt_attrib       *pattrib;
1291         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
1292         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
1293         struct tdls_txmgmt txmgmt;
1294         int ret = _FAIL;
1295
1296         DBG_871X("[TDLS] %s\n", __FUNCTION__);
1297
1298         if (rtw_tdls_is_chsw_allowed(padapter) == _FALSE) {
1299                 DBG_871X("[TDLS] Ignore %s since channel switch is not allowed\n", __func__);
1300                 goto exit;
1301         }
1302
1303         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
1304         txmgmt.action_code = TDLS_CHANNEL_SWITCH_REQUEST;
1305
1306         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
1307                 goto exit;
1308
1309         pattrib = &pmgntframe->attrib;
1310
1311         pmgntframe->frame_tag = DATA_FRAMETAG;
1312         pattrib->ether_type = 0x890d;
1313
1314         _rtw_memcpy(pattrib->dst, ptdls_sta->hwaddr, ETH_ALEN);
1315         _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
1316         _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
1317         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
1318
1319         update_tdls_attrib(padapter, pattrib);
1320         pattrib->qsel = pattrib->priority;
1321         if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) !=_SUCCESS) {
1322                 rtw_free_xmitbuf(pxmitpriv,pmgntframe->pxmitbuf);
1323                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
1324                 goto exit;
1325         }
1326
1327         dump_mgntframe(padapter, pmgntframe);
1328         ret = _SUCCESS;
1329 exit:
1330
1331         return ret;
1332 }
1333
1334 int issue_tdls_ch_switch_rsp(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, int wait_ack)
1335 {
1336         struct xmit_frame       *pmgntframe;
1337         struct pkt_attrib       *pattrib;
1338         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
1339         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
1340         int ret = _FAIL;
1341
1342         DBG_871X("[TDLS] %s\n", __FUNCTION__);
1343
1344         if (rtw_tdls_is_chsw_allowed(padapter) == _FALSE) {
1345                 DBG_871X("[TDLS] Ignore %s since channel switch is not allowed\n", __func__);
1346                 goto exit;
1347         }
1348
1349         ptxmgmt->action_code = TDLS_CHANNEL_SWITCH_RESPONSE;
1350
1351         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
1352                 goto exit;
1353
1354         pattrib = &pmgntframe->attrib;
1355
1356         pmgntframe->frame_tag = DATA_FRAMETAG;
1357         pattrib->ether_type = 0x890d;
1358
1359         _rtw_memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
1360         _rtw_memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
1361         _rtw_memcpy(pattrib->ra, ptxmgmt->peer, ETH_ALEN);
1362         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
1363
1364         update_tdls_attrib(padapter, pattrib);
1365         pattrib->qsel = pattrib->priority;
1366 /*
1367         _enter_critical_bh(&pxmitpriv->lock, &irqL);
1368         if(xmitframe_enqueue_for_tdls_sleeping_sta(padapter, pmgntframe)==_TRUE){
1369                 _exit_critical_bh(&pxmitpriv->lock, &irqL);
1370                 return _FALSE;
1371         }
1372 */
1373         if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) !=_SUCCESS) {
1374                 rtw_free_xmitbuf(pxmitpriv,pmgntframe->pxmitbuf);
1375                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
1376                 goto exit;
1377         }
1378
1379         if (wait_ack) {
1380                 ret = dump_mgntframe_and_wait_ack_timeout(padapter, pmgntframe, 10);
1381         } else {
1382                 dump_mgntframe(padapter, pmgntframe);
1383                 ret = _SUCCESS;
1384         }
1385 exit:
1386
1387         return ret;
1388 }
1389 #endif
1390
1391 int On_TDLS_Dis_Rsp(_adapter *padapter, union recv_frame *precv_frame)
1392 {
1393         struct sta_info *ptdls_sta = NULL, *psta = rtw_get_stainfo(&(padapter->stapriv), get_bssid(&(padapter->mlmepriv)));
1394         struct recv_priv *precvpriv = &(padapter->recvpriv);
1395         u8 *ptr = precv_frame->u.hdr.rx_data, *psa;
1396         struct rx_pkt_attrib *pattrib = &(precv_frame->u.hdr.attrib);
1397         struct tdls_info *ptdlsinfo = &(padapter->tdlsinfo);
1398         u8 empty_addr[ETH_ALEN] = { 0x00 };
1399         int UndecoratedSmoothedPWDB;
1400         struct tdls_txmgmt txmgmt;
1401         int ret = _SUCCESS;
1402
1403         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
1404         /* WFDTDLS: for sigma test, not to setup direct link automatically */
1405         ptdlsinfo->dev_discovered = _TRUE;
1406
1407         psa = get_sa(ptr);
1408         ptdls_sta = rtw_get_stainfo(&(padapter->stapriv), psa);
1409         if (ptdls_sta != NULL)
1410                 ptdls_sta->sta_stats.rx_tdls_disc_rsp_pkts++;
1411
1412 #ifdef CONFIG_TDLS_AUTOSETUP
1413         if (ptdls_sta != NULL) {
1414                 /* Record the tdls sta with lowest signal strength */
1415                 if (ptdlsinfo->sta_maximum == _TRUE && ptdls_sta->alive_count >= 1 ) {
1416                         if (_rtw_memcmp(ptdlsinfo->ss_record.macaddr, empty_addr, ETH_ALEN)) {
1417                                 _rtw_memcpy(ptdlsinfo->ss_record.macaddr, psa, ETH_ALEN);
1418                                 ptdlsinfo->ss_record.RxPWDBAll = pattrib->phy_info.RxPWDBAll;
1419                         } else {
1420                                 if (ptdlsinfo->ss_record.RxPWDBAll < pattrib->phy_info.RxPWDBAll) {
1421                                         _rtw_memcpy(ptdlsinfo->ss_record.macaddr, psa, ETH_ALEN);
1422                                         ptdlsinfo->ss_record.RxPWDBAll = pattrib->phy_info.RxPWDBAll;
1423                                 }
1424                         }
1425                 }
1426         } else {
1427                 if (ptdlsinfo->sta_maximum == _TRUE) {
1428                         if (_rtw_memcmp( ptdlsinfo->ss_record.macaddr, empty_addr, ETH_ALEN)) {
1429                                 /* All traffics are busy, do not set up another direct link. */
1430                                 ret = _FAIL;
1431                                 goto exit;
1432                         } else {
1433                                 if (pattrib->phy_info.RxPWDBAll > ptdlsinfo->ss_record.RxPWDBAll) {
1434                                         _rtw_memcpy(txmgmt.peer, ptdlsinfo->ss_record.macaddr, ETH_ALEN);
1435                                         /* issue_tdls_teardown(padapter, ptdlsinfo->ss_record.macaddr, _FALSE); */
1436                                 } else {
1437                                         ret = _FAIL;
1438                                         goto exit;
1439                                 }
1440                         }
1441                 }
1442
1443                 rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
1444
1445                 if (pattrib->phy_info.RxPWDBAll + TDLS_SIGNAL_THRESH >= UndecoratedSmoothedPWDB) {
1446                         DBG_871X("pattrib->RxPWDBAll=%d, pdmpriv->UndecoratedSmoothedPWDB=%d\n", pattrib->phy_info.RxPWDBAll, UndecoratedSmoothedPWDB);
1447                         _rtw_memcpy(txmgmt.peer, psa, ETH_ALEN);
1448                         issue_tdls_setup_req(padapter, &txmgmt, _FALSE);
1449                 }
1450         }
1451 #endif /* CONFIG_TDLS_AUTOSETUP */
1452
1453 exit:
1454         return ret;
1455
1456 }
1457
1458 sint On_TDLS_Setup_Req(_adapter *padapter, union recv_frame *precv_frame)
1459 {
1460         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
1461         u8 *psa, *pmyid;
1462         struct sta_info *ptdls_sta= NULL;
1463         struct sta_priv *pstapriv = &padapter->stapriv;
1464         u8 *ptr = precv_frame->u.hdr.rx_data;
1465         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
1466         struct security_priv *psecuritypriv = &padapter->securitypriv;
1467         _irqL irqL;
1468         struct rx_pkt_attrib    *prx_pkt_attrib = &precv_frame->u.hdr.attrib;
1469         u8 *prsnie, *ppairwise_cipher;
1470         u8 i, k;
1471         u8 ccmp_included=0, rsnie_included=0;
1472         u16 j, pairwise_count;
1473         u8 SNonce[32];
1474         u32 timeout_interval = TDLS_TPK_RESEND_COUNT;
1475         sint parsing_length;    /* Frame body length, without icv_len */
1476         PNDIS_802_11_VARIABLE_IEs       pIE;
1477         u8 FIXED_IE = 5;
1478         unsigned char           supportRate[16];
1479         int                             supportRateNum = 0;
1480         struct tdls_txmgmt txmgmt;
1481
1482         if (rtw_tdls_is_setup_allowed(padapter) == _FALSE)
1483                 goto exit;
1484
1485         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
1486         psa = get_sa(ptr);
1487         ptdls_sta = rtw_get_stainfo(pstapriv, psa);
1488
1489         pmyid = adapter_mac_addr(padapter);
1490         ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len+LLC_HEADER_SIZE+ETH_TYPE_LEN+PAYLOAD_TYPE_LEN;
1491         parsing_length= ((union recv_frame *)precv_frame)->u.hdr.len
1492                         -prx_pkt_attrib->hdrlen
1493                         -prx_pkt_attrib->iv_len
1494                         -prx_pkt_attrib->icv_len
1495                         -LLC_HEADER_SIZE
1496                         -ETH_TYPE_LEN
1497                         -PAYLOAD_TYPE_LEN
1498                         -FIXED_IE;
1499
1500         if (ptdls_sta == NULL) {
1501                 ptdls_sta = rtw_alloc_stainfo(pstapriv, psa);
1502         } else {
1503                 if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) {
1504                         /* If the direct link is already set up */
1505                         /* Process as re-setup after tear down */
1506                         DBG_871X("re-setup a direct link\n");
1507                 }
1508                 /* Already receiving TDLS setup request */
1509                 else if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE) {
1510                         DBG_871X("receive duplicated TDLS setup request frame in handshaking\n");
1511                         goto exit;
1512                 }
1513                 /* When receiving and sending setup_req to the same link at the same time */
1514                 /* STA with higher MAC_addr would be initiator */
1515                 else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE) {
1516                         DBG_871X("receive setup_req after sending setup_req\n");
1517                         for (i=0;i<6;i++){
1518                                 if(*(pmyid+i)==*(psa+i)){
1519                                 }
1520                                 else if(*(pmyid+i)>*(psa+i)){
1521                                         ptdls_sta->tdls_sta_state = TDLS_INITIATOR_STATE;
1522                                         break;
1523                                 }else if(*(pmyid+i)<*(psa+i)){
1524                                         goto exit;
1525                                 }
1526                         }
1527                 }
1528         }
1529
1530         if (ptdls_sta) {
1531                 txmgmt.dialog_token = *(ptr+2); /* Copy dialog token */
1532                 txmgmt.status_code = _STATS_SUCCESSFUL_;
1533
1534                 /* Parsing information element */
1535                 for (j=FIXED_IE; j<parsing_length;) {
1536
1537                         pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr+ j);
1538
1539                         switch (pIE->ElementID) {
1540                         case _SUPPORTEDRATES_IE_:
1541                                 _rtw_memcpy(supportRate, pIE->data, pIE->Length);
1542                                 supportRateNum = pIE->Length;
1543                                 break;
1544                         case _COUNTRY_IE_:
1545                                 break;
1546                         case _EXT_SUPPORTEDRATES_IE_:
1547                                 if (supportRateNum<=sizeof(supportRate)) {
1548                                         _rtw_memcpy(supportRate+supportRateNum, pIE->data, pIE->Length);
1549                                         supportRateNum += pIE->Length;
1550                                 }
1551                                 break;
1552                         case _SUPPORTED_CH_IE_:
1553                                 break;
1554                         case _RSN_IE_2_:
1555                                 rsnie_included=1;
1556                                 if (prx_pkt_attrib->encrypt) {
1557                                         prsnie=(u8*)pIE;
1558                                         /* Check CCMP pairwise_cipher presence. */
1559                                         ppairwise_cipher=prsnie+10;
1560                                         _rtw_memcpy(ptdls_sta->TDLS_RSNIE, pIE->data, pIE->Length);
1561                                         pairwise_count = *(u16*)(ppairwise_cipher-2);
1562                                         for (k=0; k<pairwise_count; k++) {
1563                                                 if (_rtw_memcmp( ppairwise_cipher+4*k, RSN_CIPHER_SUITE_CCMP, 4)==_TRUE)
1564                                                         ccmp_included=1;
1565                                         }
1566
1567                                         if (ccmp_included == 0)
1568                                                 txmgmt.status_code=_STATS_INVALID_RSNIE_;
1569                                 }
1570                                 break;
1571                         case _EXT_CAP_IE_:
1572                                 break;
1573                         case _VENDOR_SPECIFIC_IE_:
1574                                 break;
1575                         case _FTIE_:
1576                                 if (prx_pkt_attrib->encrypt)
1577                                         _rtw_memcpy(SNonce, (ptr+j+52), 32);
1578                                 break;
1579                         case _TIMEOUT_ITVL_IE_:
1580                                 if (prx_pkt_attrib->encrypt)
1581                                         timeout_interval = cpu_to_le32(*(u32*)(ptr+j+3));
1582                                 break;
1583                         case _RIC_Descriptor_IE_:
1584                                 break;
1585 #ifdef CONFIG_80211N_HT
1586                         case _HT_CAPABILITY_IE_:
1587                                 rtw_tdls_process_ht_cap(padapter, ptdls_sta, pIE->data, pIE->Length);
1588                                 break;
1589 #endif
1590                         case EID_BSSCoexistence:
1591                                 break;
1592                         case _LINK_ID_IE_:
1593                                 if (_rtw_memcmp(get_bssid(pmlmepriv), pIE->data, 6) == _FALSE)
1594                                         txmgmt.status_code=_STATS_NOT_IN_SAME_BSS_;
1595                                 break;
1596                         default:
1597                                 break;
1598                         }
1599
1600                         j += (pIE->Length + 2);
1601
1602                 }
1603
1604                 /* Check status code */
1605                 /* If responder STA has/hasn't security on AP, but request hasn't/has RSNIE, it should reject */
1606                 if (txmgmt.status_code == _STATS_SUCCESSFUL_) {
1607                         if (rsnie_included && prx_pkt_attrib->encrypt == 0)
1608                                 txmgmt.status_code = _STATS_SEC_DISABLED_;
1609                         else if (rsnie_included==0 && prx_pkt_attrib->encrypt)
1610                                 txmgmt.status_code = _STATS_INVALID_PARAMETERS_;
1611
1612 #ifdef CONFIG_WFD
1613                         /* WFD test plan version 0.18.2 test item 5.1.5 */
1614                         /* SoUT does not use TDLS if AP uses weak security */
1615                         if (padapter->wdinfo.wfd_tdls_enable && (rsnie_included && prx_pkt_attrib->encrypt != _AES_))
1616                                         txmgmt.status_code = _STATS_SEC_DISABLED_;
1617 #endif /* CONFIG_WFD */
1618                 }
1619
1620                 ptdls_sta->tdls_sta_state|= TDLS_INITIATOR_STATE;
1621                 if (prx_pkt_attrib->encrypt) {
1622                         _rtw_memcpy(ptdls_sta->SNonce, SNonce, 32);
1623
1624                         if (timeout_interval <= 300)
1625                                 ptdls_sta->TDLS_PeerKey_Lifetime = TDLS_TPK_RESEND_COUNT;
1626                         else
1627                                 ptdls_sta->TDLS_PeerKey_Lifetime = timeout_interval;
1628                 }
1629
1630                 /* Update station supportRate */
1631                 ptdls_sta->bssratelen = supportRateNum;
1632                 _rtw_memcpy(ptdls_sta->bssrateset, supportRate, supportRateNum);
1633
1634                 if (!(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE))
1635                         ptdlsinfo->sta_cnt++;
1636                 /* -2: AP + BC/MC sta, -4: default key */
1637                 if (ptdlsinfo->sta_cnt == MAX_ALLOWED_TDLS_STA_NUM)
1638                         ptdlsinfo->sta_maximum = _TRUE;
1639
1640 #ifdef CONFIG_WFD
1641                 rtw_tdls_process_wfd_ie(ptdlsinfo, ptr + FIXED_IE, parsing_length);
1642 #endif
1643
1644         }else {
1645                 goto exit;
1646         }
1647
1648         _rtw_memcpy(txmgmt.peer, prx_pkt_attrib->src, ETH_ALEN);
1649
1650         if (rtw_tdls_is_driver_setup(padapter)) {
1651                 issue_tdls_setup_rsp(padapter, &txmgmt);
1652
1653                 if (txmgmt.status_code==_STATS_SUCCESSFUL_) {
1654                         _set_timer( &ptdls_sta->handshake_timer, TDLS_HANDSHAKE_TIME);
1655                 }else {
1656                         free_tdls_sta(padapter, ptdls_sta);
1657                 }
1658         }
1659
1660 exit:
1661
1662         return _SUCCESS;
1663 }
1664
1665 int On_TDLS_Setup_Rsp(_adapter *padapter, union recv_frame *precv_frame)
1666 {
1667         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
1668         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
1669         struct sta_info *ptdls_sta= NULL;
1670         struct sta_priv *pstapriv = &padapter->stapriv;
1671         u8 *ptr = precv_frame->u.hdr.rx_data;
1672         _irqL irqL;
1673         struct rx_pkt_attrib    *prx_pkt_attrib = &precv_frame->u.hdr.attrib;
1674         u8 *psa;
1675         u16 status_code=0;
1676         sint parsing_length;    /* Frame body length, without icv_len */
1677         PNDIS_802_11_VARIABLE_IEs       pIE;
1678         u8 FIXED_IE =7;
1679         u8 ANonce[32];
1680         u8  *pftie=NULL, *ptimeout_ie=NULL, *plinkid_ie=NULL, *prsnie=NULL, *pftie_mic=NULL, *ppairwise_cipher=NULL;
1681         u16 pairwise_count, j, k;
1682         u8 verify_ccmp=0;
1683         unsigned char           supportRate[16];
1684         int                             supportRateNum = 0;
1685         struct tdls_txmgmt txmgmt;
1686         int ret = _SUCCESS;
1687         u32 timeout_interval = TDLS_TPK_RESEND_COUNT;
1688
1689         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
1690         psa = get_sa(ptr);
1691         ptdls_sta = rtw_get_stainfo(pstapriv, psa);
1692
1693         if (ptdls_sta == NULL) {
1694                 DBG_871X("[%s] Direct Link Peer = "MAC_FMT" not found\n", __func__, MAC_ARG(psa));
1695                 ret = _FAIL;
1696                 goto exit;
1697         }
1698
1699         ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len+LLC_HEADER_SIZE+ETH_TYPE_LEN+PAYLOAD_TYPE_LEN;
1700         parsing_length= ((union recv_frame *)precv_frame)->u.hdr.len
1701                         -prx_pkt_attrib->hdrlen
1702                         -prx_pkt_attrib->iv_len
1703                         -prx_pkt_attrib->icv_len
1704                         -LLC_HEADER_SIZE
1705                         -ETH_TYPE_LEN
1706                         -PAYLOAD_TYPE_LEN
1707                         -FIXED_IE;
1708
1709         _rtw_memcpy(&status_code, ptr+2, 2);
1710
1711         if (status_code != 0) {
1712                 DBG_871X( "[TDLS] %s status_code = %d, free_tdls_sta\n", __FUNCTION__, status_code );
1713                 free_tdls_sta(padapter, ptdls_sta);
1714                 ret = _FAIL;
1715                 goto exit;
1716         }
1717
1718         status_code = 0;
1719
1720         /* parsing information element */
1721         for (j = FIXED_IE; j<parsing_length;) {
1722                 pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr+ j);
1723
1724                 switch (pIE->ElementID) {
1725                 case _SUPPORTEDRATES_IE_:
1726                         _rtw_memcpy(supportRate, pIE->data, pIE->Length);
1727                         supportRateNum = pIE->Length;
1728                         break;
1729                 case _COUNTRY_IE_:
1730                         break;
1731                 case _EXT_SUPPORTEDRATES_IE_:
1732                         if (supportRateNum<=sizeof(supportRate)) {
1733                                 _rtw_memcpy(supportRate+supportRateNum, pIE->data, pIE->Length);
1734                                 supportRateNum += pIE->Length;
1735                         }
1736                         break;
1737                 case _SUPPORTED_CH_IE_:
1738                         break;
1739                 case _RSN_IE_2_:
1740                         prsnie=(u8*)pIE;
1741                         /* Check CCMP pairwise_cipher presence. */
1742                         ppairwise_cipher=prsnie+10;
1743                         _rtw_memcpy(&pairwise_count, (u16*)(ppairwise_cipher-2), 2);
1744                         for (k=0;k<pairwise_count;k++) {
1745                                 if (_rtw_memcmp( ppairwise_cipher+4*k, RSN_CIPHER_SUITE_CCMP, 4) == _TRUE)
1746                                         verify_ccmp=1;
1747                         }
1748                 case _EXT_CAP_IE_:
1749                         break;
1750                 case _VENDOR_SPECIFIC_IE_:
1751                         if (_rtw_memcmp((u8 *)pIE + 2, WMM_INFO_OUI, 6) == _TRUE) {
1752                                 /* WMM Info ID and OUI */
1753                                 if ((pregistrypriv->wmm_enable == _TRUE) || (padapter->mlmepriv.htpriv.ht_option == _TRUE))
1754                                         ptdls_sta->qos_option = _TRUE;
1755                         }
1756                         break;
1757                 case _FTIE_:
1758                         pftie=(u8*)pIE;
1759                         _rtw_memcpy(ANonce, (ptr+j+20), 32);
1760                         break;
1761                 case _TIMEOUT_ITVL_IE_:
1762                         ptimeout_ie=(u8*)pIE;
1763                         timeout_interval = cpu_to_le32(*(u32*)(ptimeout_ie+3));
1764                         break;
1765                 case _RIC_Descriptor_IE_:
1766                         break;
1767 #ifdef CONFIG_80211N_HT
1768                 case _HT_CAPABILITY_IE_:
1769                         rtw_tdls_process_ht_cap(padapter, ptdls_sta, pIE->data, pIE->Length);
1770                         break;
1771 #endif
1772                 case EID_BSSCoexistence:
1773                         break;
1774                 case _LINK_ID_IE_:
1775                         plinkid_ie=(u8*)pIE;
1776                         break;
1777                 default:
1778                         break;
1779                 }
1780
1781                 j += (pIE->Length + 2);
1782
1783         }
1784
1785         ptdls_sta->bssratelen = supportRateNum;
1786         _rtw_memcpy(ptdls_sta->bssrateset, supportRate, supportRateNum);
1787         _rtw_memcpy(ptdls_sta->ANonce, ANonce, 32);
1788
1789 #ifdef CONFIG_WFD
1790         rtw_tdls_process_wfd_ie(ptdlsinfo, ptr + FIXED_IE, parsing_length);
1791 #endif
1792
1793         if (status_code != _STATS_SUCCESSFUL_) {
1794                 txmgmt.status_code = status_code;
1795         } else {
1796                 if (prx_pkt_attrib->encrypt) {
1797                         if (verify_ccmp == 1) {
1798                                 txmgmt.status_code = _STATS_SUCCESSFUL_;
1799                                 if (rtw_tdls_is_driver_setup(padapter) == _TRUE) {
1800                                         wpa_tdls_generate_tpk(padapter, ptdls_sta);
1801                                         if (tdls_verify_mic(ptdls_sta->tpk.kck, 2, plinkid_ie, prsnie, ptimeout_ie, pftie) == _FAIL) {
1802                                                 DBG_871X( "[TDLS] %s tdls_verify_mic fail, free_tdls_sta\n", __FUNCTION__);
1803                                                 free_tdls_sta(padapter, ptdls_sta);
1804                                                 ret = _FAIL;
1805                                                 goto exit;
1806                                         }
1807                                         ptdls_sta->TDLS_PeerKey_Lifetime = timeout_interval;
1808                                 }
1809                         }
1810                         else
1811                         {
1812                                 txmgmt.status_code = _STATS_INVALID_RSNIE_;
1813                         }
1814
1815                 }else{
1816                         txmgmt.status_code = _STATS_SUCCESSFUL_;
1817                 }
1818         }
1819
1820         if (rtw_tdls_is_driver_setup(padapter) == _TRUE) {
1821                 _rtw_memcpy(txmgmt.peer, prx_pkt_attrib->src, ETH_ALEN);
1822                 issue_tdls_setup_cfm(padapter, &txmgmt);
1823
1824                 if (txmgmt.status_code == _STATS_SUCCESSFUL_) {
1825                         ptdlsinfo->link_established = _TRUE;
1826
1827                         if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE) {
1828                                 ptdls_sta->tdls_sta_state |= TDLS_LINKED_STATE;
1829                                 ptdls_sta->state |= _FW_LINKED;
1830                                 _cancel_timer_ex( &ptdls_sta->handshake_timer);
1831                         }
1832
1833                         if (prx_pkt_attrib->encrypt)
1834                                 rtw_tdls_set_key(padapter, ptdls_sta);
1835
1836                         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_ESTABLISHED);
1837
1838                 }
1839         }
1840
1841 exit:
1842         if (rtw_tdls_is_driver_setup(padapter) == _TRUE)
1843                 return ret;
1844         else
1845                 return _SUCCESS;
1846
1847 }
1848
1849 int On_TDLS_Setup_Cfm(_adapter *padapter, union recv_frame *precv_frame)
1850 {
1851         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
1852         struct sta_info *ptdls_sta= NULL;
1853         struct sta_priv *pstapriv = &padapter->stapriv;
1854         u8 *ptr = precv_frame->u.hdr.rx_data;
1855         _irqL irqL;
1856         struct rx_pkt_attrib    *prx_pkt_attrib = &precv_frame->u.hdr.attrib;
1857         u8 *psa;
1858         u16 status_code=0;
1859         sint parsing_length;
1860         PNDIS_802_11_VARIABLE_IEs       pIE;
1861         u8 FIXED_IE =5;
1862         u8  *pftie=NULL, *ptimeout_ie=NULL, *plinkid_ie=NULL, *prsnie=NULL, *pftie_mic=NULL, *ppairwise_cipher=NULL;
1863         u16 j, pairwise_count;
1864         int ret = _SUCCESS;
1865
1866         psa = get_sa(ptr);
1867         ptdls_sta = rtw_get_stainfo(pstapriv, psa);
1868
1869         if (ptdls_sta == NULL) {
1870                 DBG_871X("[%s] Direct Link Peer = "MAC_FMT" not found\n", __FUNCTION__, MAC_ARG(psa));
1871                 ret = _FAIL;
1872                 goto exit;
1873         }
1874
1875         ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len+LLC_HEADER_SIZE+ETH_TYPE_LEN+PAYLOAD_TYPE_LEN;
1876         parsing_length= ((union recv_frame *)precv_frame)->u.hdr.len
1877                         -prx_pkt_attrib->hdrlen
1878                         -prx_pkt_attrib->iv_len
1879                         -prx_pkt_attrib->icv_len
1880                         -LLC_HEADER_SIZE
1881                         -ETH_TYPE_LEN
1882                         -PAYLOAD_TYPE_LEN
1883                         -FIXED_IE;
1884
1885         _rtw_memcpy(&status_code, ptr+2, 2);
1886
1887         if (status_code!= 0) {
1888                 DBG_871X("[%s] status_code = %d\n, free_tdls_sta", __FUNCTION__, status_code);
1889                 free_tdls_sta(padapter, ptdls_sta);
1890                 ret = _FAIL;
1891                 goto exit;
1892         }
1893
1894         /* Parsing information element */
1895         for (j = FIXED_IE; j < parsing_length;) {
1896
1897                 pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j);
1898
1899                 switch (pIE->ElementID) {
1900                         case _RSN_IE_2_:
1901                                 prsnie = (u8 *)pIE;
1902                                 break;
1903                         case _VENDOR_SPECIFIC_IE_:
1904                                 if (_rtw_memcmp((u8 *)pIE + 2, WMM_PARA_OUI, 6) == _TRUE) {
1905                                         /* WMM Parameter ID and OUI */
1906                                         ptdls_sta->qos_option = _TRUE;
1907                                 }
1908                                 break;
1909                         case _FTIE_:
1910                                 pftie = (u8 *)pIE;
1911                                 break;
1912                         case _TIMEOUT_ITVL_IE_:
1913                                 ptimeout_ie = (u8 *)pIE;
1914                                 break;
1915 #ifdef CONFIG_80211N_HT
1916                         case _HT_EXTRA_INFO_IE_:
1917                                 break;
1918 #endif
1919                         case _LINK_ID_IE_:
1920                                 plinkid_ie = (u8 *)pIE;
1921                                 break;
1922                         default:
1923                                 break;
1924                 }
1925
1926                 j += (pIE->Length + 2);
1927
1928         }
1929
1930         if (prx_pkt_attrib->encrypt) {
1931                 /* Verify mic in FTIE MIC field */
1932                 if (rtw_tdls_is_driver_setup(padapter) &&
1933                         (tdls_verify_mic(ptdls_sta->tpk.kck, 3, plinkid_ie, prsnie, ptimeout_ie, pftie) == _FAIL)) {
1934                         free_tdls_sta(padapter, ptdls_sta);
1935                         ret = _FAIL;
1936                         goto exit;
1937                 }
1938         }
1939
1940         if (rtw_tdls_is_driver_setup(padapter)) {
1941                 ptdlsinfo->link_established = _TRUE;
1942
1943                 if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE) {
1944                         ptdls_sta->tdls_sta_state|=TDLS_LINKED_STATE;
1945                         ptdls_sta->state |= _FW_LINKED;
1946                         _cancel_timer_ex(&ptdls_sta->handshake_timer);
1947                 }
1948
1949                 if (prx_pkt_attrib->encrypt) {
1950                         rtw_tdls_set_key(padapter, ptdls_sta);
1951
1952                         /* Start  TPK timer */
1953                         ptdls_sta->TPK_count = 0;
1954                         _set_timer(&ptdls_sta->TPK_timer, ONE_SEC);
1955                 }
1956
1957                 rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_ESTABLISHED);
1958         }
1959
1960 exit:
1961         return ret;
1962
1963 }
1964
1965 int On_TDLS_Dis_Req(_adapter *padapter, union recv_frame *precv_frame)
1966 {
1967         struct rx_pkt_attrib    *prx_pkt_attrib = &precv_frame->u.hdr.attrib;
1968         struct sta_priv *pstapriv = &padapter->stapriv;
1969         struct sta_info *psta_ap;
1970         u8 *ptr = precv_frame->u.hdr.rx_data;
1971         sint parsing_length;    /* Frame body length, without icv_len */
1972         PNDIS_802_11_VARIABLE_IEs       pIE;
1973         u8 FIXED_IE = 3, *dst;
1974         u16 j;
1975         struct tdls_txmgmt txmgmt;
1976         int ret = _SUCCESS;
1977
1978         if (rtw_tdls_is_driver_setup(padapter) == _FALSE)
1979                 goto exit;
1980
1981         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
1982         ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len+LLC_HEADER_SIZE+ETH_TYPE_LEN+PAYLOAD_TYPE_LEN;
1983         txmgmt.dialog_token = *(ptr+2);
1984         _rtw_memcpy(&txmgmt.peer, precv_frame->u.hdr.attrib.src, ETH_ALEN);
1985         txmgmt.action_code = TDLS_DISCOVERY_RESPONSE;
1986         parsing_length= ((union recv_frame *)precv_frame)->u.hdr.len
1987                         -prx_pkt_attrib->hdrlen
1988                         -prx_pkt_attrib->iv_len
1989                         -prx_pkt_attrib->icv_len
1990                         -LLC_HEADER_SIZE
1991                         -ETH_TYPE_LEN
1992                         -PAYLOAD_TYPE_LEN
1993                         -FIXED_IE;
1994
1995         /* Parsing information element */
1996         for (j=FIXED_IE; j<parsing_length;) {
1997
1998                 pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr+ j);
1999
2000                 switch (pIE->ElementID) {
2001                 case _LINK_ID_IE_:
2002                         psta_ap = rtw_get_stainfo(pstapriv, pIE->data);
2003                         if (psta_ap == NULL)
2004                                 goto exit;
2005                         dst = pIE->data + 12;
2006                         if (MacAddr_isBcst(dst) == _FALSE && (_rtw_memcmp(adapter_mac_addr(padapter), dst, 6) == _FALSE))
2007                                 goto exit;
2008                         break;
2009                 default:
2010                         break;
2011                 }
2012
2013                 j += (pIE->Length + 2);
2014
2015         }
2016
2017         issue_tdls_dis_rsp(padapter, &txmgmt, prx_pkt_attrib->privacy);
2018
2019 exit:
2020         return ret;
2021
2022 }
2023
2024 int On_TDLS_Teardown(_adapter *padapter, union recv_frame *precv_frame)
2025 {
2026         u8 *psa;
2027         u8 *ptr = precv_frame->u.hdr.rx_data;
2028         struct rx_pkt_attrib    *prx_pkt_attrib = &precv_frame->u.hdr.attrib;
2029         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
2030         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2031         struct sta_priv         *pstapriv = &padapter->stapriv;
2032         struct sta_info *ptdls_sta= NULL;
2033         _irqL irqL;
2034         u8 reason;
2035
2036         reason = *(ptr + prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN + 2);
2037         DBG_871X("[TDLS] %s Reason code(%d)\n", __FUNCTION__,reason);
2038
2039         psa = get_sa(ptr);
2040
2041         ptdls_sta = rtw_get_stainfo(pstapriv, psa);
2042         if (ptdls_sta != NULL) {
2043                 if (rtw_tdls_is_driver_setup(padapter))
2044                         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_TEARDOWN_STA_LOCALLY);
2045         }
2046
2047         return _SUCCESS;
2048
2049 }
2050
2051 #if 0
2052 u8 TDLS_check_ch_state(uint state){
2053         if (state & TDLS_CH_SWITCH_ON_STATE &&
2054                 state & TDLS_PEER_AT_OFF_STATE) {
2055                 if (state & TDLS_PEER_SLEEP_STATE)
2056                         return 2;       /* U-APSD + ch. switch */
2057                 else
2058                         return 1;       /* ch. switch */
2059         }else
2060                 return 0;
2061 }
2062 #endif
2063
2064 int On_TDLS_Peer_Traffic_Indication(_adapter *padapter, union recv_frame *precv_frame)
2065 {
2066         struct rx_pkt_attrib    *pattrib = &precv_frame->u.hdr.attrib;
2067         struct sta_info *ptdls_sta = rtw_get_stainfo(&padapter->stapriv, pattrib->src);
2068         u8 *ptr = precv_frame->u.hdr.rx_data;
2069         struct tdls_txmgmt txmgmt;
2070
2071         ptr +=pattrib->hdrlen + pattrib->iv_len+LLC_HEADER_SIZE+ETH_TYPE_LEN+PAYLOAD_TYPE_LEN;
2072         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
2073
2074         if (ptdls_sta != NULL) {
2075                 txmgmt.dialog_token = *(ptr+2);
2076                 issue_tdls_peer_traffic_rsp(padapter, ptdls_sta, &txmgmt);
2077                 //issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta->hwaddr, 0, 0, 0);
2078         } else {
2079                 DBG_871X("from unknown sta:"MAC_FMT"\n", MAC_ARG(pattrib->src));
2080                 return _FAIL;
2081         }
2082
2083         return _SUCCESS;
2084 }
2085
2086 /* We process buffered data for 1. U-APSD, 2. ch. switch, 3. U-APSD + ch. switch here */
2087 int On_TDLS_Peer_Traffic_Rsp(_adapter *padapter, union recv_frame *precv_frame)
2088 {
2089         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
2090         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2091         struct rx_pkt_attrib    *pattrib = & precv_frame->u.hdr.attrib;
2092         struct sta_priv *pstapriv = &padapter->stapriv;
2093         struct sta_info *ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->src);
2094         u8 wmmps_ac=0;
2095         /* u8 state=TDLS_check_ch_state(ptdls_sta->tdls_sta_state); */
2096         int i;
2097
2098         ptdls_sta->sta_stats.rx_data_pkts++;
2099
2100         ptdls_sta->tdls_sta_state &= ~(TDLS_WAIT_PTR_STATE);
2101
2102         /* Check 4-AC queue bit */
2103         if (ptdls_sta->uapsd_vo || ptdls_sta->uapsd_vi || ptdls_sta->uapsd_be || ptdls_sta->uapsd_bk)
2104                 wmmps_ac=1;
2105
2106         /* If it's a direct link and have buffered frame */
2107         if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) {
2108                 if (wmmps_ac) {
2109                         _irqL irqL;
2110                         _list   *xmitframe_plist, *xmitframe_phead;
2111                         struct xmit_frame *pxmitframe=NULL;
2112
2113                         _enter_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);
2114
2115                         xmitframe_phead = get_list_head(&ptdls_sta->sleep_q);
2116                         xmitframe_plist = get_next(xmitframe_phead);
2117
2118                         /* transmit buffered frames */
2119                         while (rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist) == _FALSE) {
2120                                 pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
2121                                 xmitframe_plist = get_next(xmitframe_plist);
2122                                 rtw_list_delete(&pxmitframe->list);
2123
2124                                 ptdls_sta->sleepq_len--;
2125                                 ptdls_sta->sleepq_ac_len--;
2126                                 if (ptdls_sta->sleepq_len>0) {
2127                                         pxmitframe->attrib.mdata = 1;
2128                                         pxmitframe->attrib.eosp = 0;
2129                                 } else {
2130                                         pxmitframe->attrib.mdata = 0;
2131                                         pxmitframe->attrib.eosp = 1;
2132                                 }
2133                                 pxmitframe->attrib.triggered = 1;
2134
2135                                 rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
2136                         }
2137
2138                         if (ptdls_sta->sleepq_len==0)
2139                                 DBG_871X("no buffered packets for tdls to xmit\n");
2140                         else {
2141                                 DBG_871X("error!psta->sleepq_len=%d\n", ptdls_sta->sleepq_len);
2142                                 ptdls_sta->sleepq_len=0;
2143                         }
2144
2145                         _exit_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);
2146
2147                 }
2148
2149         }
2150
2151         return _SUCCESS;
2152 }
2153
2154 #ifdef CONFIG_TDLS_CH_SW
2155 sint On_TDLS_Ch_Switch_Req(_adapter *padapter, union recv_frame *precv_frame)
2156 {
2157         struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
2158         struct sta_info *ptdls_sta= NULL;
2159         struct sta_priv *pstapriv = &padapter->stapriv;
2160         u8 *ptr = precv_frame->u.hdr.rx_data;
2161         struct rx_pkt_attrib    *prx_pkt_attrib = &precv_frame->u.hdr.attrib;
2162         u8 *psa;
2163         sint parsing_length;
2164         PNDIS_802_11_VARIABLE_IEs       pIE;
2165         u8 FIXED_IE = 4;
2166         u16 j;
2167         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2168         struct tdls_txmgmt txmgmt;
2169         u8 zaddr[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
2170         u16 switch_time = TDLS_CH_SWITCH_TIME * 1000, switch_timeout = TDLS_CH_SWITCH_TIMEOUT * 1000;
2171         u8 take_care_iqk;
2172
2173         if (rtw_tdls_is_chsw_allowed(padapter) == _FALSE) {
2174                 DBG_871X("[TDLS] Ignore %s since channel switch is not allowed\n", __func__);
2175                 return _FAIL;
2176         }
2177
2178         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
2179         psa = get_sa(ptr);
2180         ptdls_sta = rtw_get_stainfo(pstapriv, psa);
2181
2182         if (ptdls_sta == NULL) {
2183                 DBG_871X("[%s] Direct Link Peer = "MAC_FMT" not found\n", __func__, MAC_ARG(psa));
2184                 return _FAIL;
2185         }
2186
2187         ptdls_sta->ch_switch_time=switch_time;
2188         ptdls_sta->ch_switch_timeout=switch_timeout;
2189
2190         ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len+LLC_HEADER_SIZE+ETH_TYPE_LEN+PAYLOAD_TYPE_LEN;
2191         parsing_length= ((union recv_frame *)precv_frame)->u.hdr.len
2192                         -prx_pkt_attrib->hdrlen
2193                         -prx_pkt_attrib->iv_len
2194                         -prx_pkt_attrib->icv_len
2195                         -LLC_HEADER_SIZE
2196                         -ETH_TYPE_LEN
2197                         -PAYLOAD_TYPE_LEN
2198                         -FIXED_IE;
2199
2200         pchsw_info->off_ch_num = *(ptr + 2);
2201
2202         if ((*(ptr + 2) == 2) && (hal_is_band_support(padapter, BAND_ON_5G))) {
2203                 pchsw_info->off_ch_num = 44;
2204         }
2205
2206         if (pchsw_info->off_ch_num != pmlmeext->cur_channel) {
2207                 pchsw_info->delay_switch_back = _FALSE;
2208         }
2209
2210         /* Parsing information element */
2211         for (j=FIXED_IE; j<parsing_length;) {
2212                 pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr+ j);
2213
2214                 switch (pIE->ElementID) {
2215                 case EID_SecondaryChnlOffset:
2216                         switch (*(pIE->data)) {
2217                         case EXTCHNL_OFFSET_UPPER:
2218                                 pchsw_info->ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
2219                                 break;
2220
2221                         case EXTCHNL_OFFSET_LOWER:
2222                                 pchsw_info->ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
2223                                 break;
2224
2225                         default:
2226                                 pchsw_info->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
2227                                 break;
2228                         }
2229                         break;
2230                 case _LINK_ID_IE_:
2231                         break;
2232                 case _CH_SWITCH_TIMING_:
2233                         ptdls_sta->ch_switch_time = (RTW_GET_LE16(pIE->data) >= TDLS_CH_SWITCH_TIME * 1000) ?
2234                                 RTW_GET_LE16(pIE->data) : TDLS_CH_SWITCH_TIME * 1000;
2235                         ptdls_sta->ch_switch_timeout = (RTW_GET_LE16(pIE->data + 2) >= TDLS_CH_SWITCH_TIMEOUT * 1000) ?
2236                                 RTW_GET_LE16(pIE->data + 2) : TDLS_CH_SWITCH_TIMEOUT * 1000;
2237                         DBG_871X("[TDLS] %s ch_switch_time:%d, ch_switch_timeout:%d\n"
2238                                 , __FUNCTION__, RTW_GET_LE16(pIE->data), RTW_GET_LE16(pIE->data + 2));
2239                 default:
2240                         break;
2241                 }
2242
2243                 j += (pIE->Length + 2);
2244         }
2245
2246         rtw_hal_get_hwreg(padapter, HW_VAR_CH_SW_NEED_TO_TAKE_CARE_IQK_INFO, &take_care_iqk);
2247         if (take_care_iqk == _TRUE) {
2248                 u8 central_chnl;
2249                 u8 bw_mode;
2250
2251                 bw_mode = (pchsw_info->ch_offset) ? CHANNEL_WIDTH_40 : CHANNEL_WIDTH_20;
2252                 central_chnl = rtw_get_center_ch(pchsw_info->off_ch_num, bw_mode, pchsw_info->ch_offset);
2253                 if (rtw_hal_ch_sw_iqk_info_search(padapter, central_chnl, bw_mode) < 0) {
2254                         if (!(pchsw_info->ch_sw_state & TDLS_CH_SWITCH_PREPARE_STATE))
2255                                 rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_PREPARE);
2256
2257                         return _FAIL;
2258                 }
2259         }
2260
2261         /* cancel ch sw monitor timer for responder */
2262         if (!(pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE))
2263                 _cancel_timer_ex(&ptdls_sta->ch_sw_monitor_timer);
2264
2265         /* Todo: check status */
2266         txmgmt.status_code = 0;
2267         _rtw_memcpy(txmgmt.peer, psa, ETH_ALEN);
2268
2269         if (_rtw_memcmp(pchsw_info->addr, zaddr, ETH_ALEN) == _TRUE)
2270                 _rtw_memcpy(pchsw_info->addr, ptdls_sta->hwaddr, ETH_ALEN);
2271
2272         if (ATOMIC_READ(&pchsw_info->chsw_on) == _FALSE)
2273                 rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_START);
2274
2275         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_RESP);
2276
2277         return _SUCCESS;
2278 }
2279
2280 sint On_TDLS_Ch_Switch_Rsp(_adapter *padapter, union recv_frame *precv_frame)
2281 {
2282         struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
2283         struct sta_info *ptdls_sta= NULL;
2284         struct sta_priv *pstapriv = &padapter->stapriv;
2285         u8 *ptr = precv_frame->u.hdr.rx_data;
2286         struct rx_pkt_attrib    *prx_pkt_attrib = &precv_frame->u.hdr.attrib;
2287         u8 *psa;
2288         sint parsing_length;
2289         PNDIS_802_11_VARIABLE_IEs       pIE;
2290         u8 FIXED_IE = 4;
2291         u16 status_code, j, switch_time, switch_timeout;
2292         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
2293         int ret = _SUCCESS;
2294
2295         if (rtw_tdls_is_chsw_allowed(padapter) == _FALSE) {
2296                 DBG_871X("[TDLS] Ignore %s since channel switch is not allowed\n", __func__);
2297                 return _SUCCESS;
2298         }
2299
2300         psa = get_sa(ptr);
2301         ptdls_sta = rtw_get_stainfo(pstapriv, psa);
2302
2303         if (ptdls_sta == NULL) {
2304                 DBG_871X("[%s] Direct Link Peer = "MAC_FMT" not found\n", __func__, MAC_ARG(psa));
2305                 return _FAIL;
2306         }
2307
2308         /* If we receive Unsolicited TDLS Channel Switch Response when channel switch is running, */
2309         /* we will go back to base channel and terminate this channel switch procedure */
2310         if (ATOMIC_READ(&pchsw_info->chsw_on) == _TRUE) {
2311                 if (pmlmeext->cur_channel != rtw_get_oper_ch(padapter)) {
2312                         DBG_871X("[TDLS] Rx unsolicited channel switch response\n");
2313                         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_TO_BASE_CHNL);
2314                         goto exit;
2315                 }
2316         }
2317
2318         ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE+ETH_TYPE_LEN+PAYLOAD_TYPE_LEN;
2319         parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len
2320                         -prx_pkt_attrib->hdrlen
2321                         -prx_pkt_attrib->iv_len
2322                         -prx_pkt_attrib->icv_len
2323                         -LLC_HEADER_SIZE
2324                         -ETH_TYPE_LEN
2325                         -PAYLOAD_TYPE_LEN
2326                         -FIXED_IE;
2327
2328         _rtw_memcpy(&status_code, ptr+2, 2);
2329
2330         if (status_code != 0) {
2331                 DBG_871X("[TDLS] %s status_code:%d\n", __func__, status_code);
2332                 pchsw_info->ch_sw_state &= ~(TDLS_CH_SW_INITIATOR_STATE);
2333                 rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_END);
2334                 ret = _FAIL;
2335                 goto exit;
2336         }
2337
2338         /* Parsing information element */
2339         for (j = FIXED_IE; j < parsing_length;) {
2340                 pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr+ j);
2341
2342                 switch (pIE->ElementID) {
2343                 case _LINK_ID_IE_:
2344                         break;
2345                 case _CH_SWITCH_TIMING_:
2346                         _rtw_memcpy(&switch_time, pIE->data, 2);
2347                         if (switch_time > ptdls_sta->ch_switch_time)
2348                                 _rtw_memcpy(&ptdls_sta->ch_switch_time, &switch_time, 2);
2349
2350                         _rtw_memcpy(&switch_timeout, pIE->data + 2, 2);
2351                         if (switch_timeout > ptdls_sta->ch_switch_timeout)
2352                                 _rtw_memcpy(&ptdls_sta->ch_switch_timeout, &switch_timeout, 2);
2353                         break;
2354                 default:
2355                         break;
2356                 }
2357
2358                 j += (pIE->Length + 2);
2359         }
2360
2361         if ((pmlmeext->cur_channel == rtw_get_oper_ch(padapter)) &&
2362                 (pchsw_info->ch_sw_state & TDLS_WAIT_CH_RSP_STATE)) {
2363                 if (ATOMIC_READ(&pchsw_info->chsw_on) == _TRUE)
2364                         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_TO_OFF_CHNL);
2365         }
2366
2367 exit:
2368         return ret;
2369 }
2370 #endif /* CONFIG_TDLS_CH_SW */
2371
2372 #ifdef CONFIG_WFD
2373 void wfd_ie_tdls(_adapter * padapter, u8 *pframe, u32 *pktlen )
2374 {
2375         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
2376         struct wifi_display_info        *pwfd_info = padapter->tdlsinfo.wfd_info;
2377         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
2378         u32 wfdielen = 0;
2379
2380         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
2381                 return;
2382
2383         /* WFD OUI */
2384         wfdielen = 0;
2385         wfdie[ wfdielen++ ] = 0x50;
2386         wfdie[ wfdielen++ ] = 0x6F;
2387         wfdie[ wfdielen++ ] = 0x9A;
2388         wfdie[ wfdielen++ ] = 0x0A;     /* WFA WFD v1.0 */
2389
2390         /*
2391          *      Commented by Albert 20110825
2392          *      According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes
2393          *      1. WFD Device Information
2394          *      2. Associated BSSID ( Optional )
2395          *      3. Local IP Adress ( Optional )
2396          */
2397
2398         /* WFD Device Information ATTR */
2399         /* Type: */
2400         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
2401
2402         /* Length: */
2403         /* Note: In the WFD specification, the size of length field is 2. */
2404         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
2405         wfdielen += 2;
2406
2407         /* Value1: */
2408         /* WFD device information */
2409         /* available for WFD session + Preferred TDLS + WSD ( WFD Service Discovery ) */
2410         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL
2411                                                                 | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_WSD);
2412         wfdielen += 2;
2413
2414         /* Value2: */
2415         /* Session Management Control Port */
2416         /* Default TCP port for RTSP messages is 554 */
2417         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->tdls_rtsp_ctrlport);
2418         wfdielen += 2;
2419
2420         /* Value3: */
2421         /* WFD Device Maximum Throughput */
2422         /* 300Mbps is the maximum throughput */
2423         RTW_PUT_BE16(wfdie + wfdielen, 300);
2424         wfdielen += 2;
2425
2426         /* Associated BSSID ATTR */
2427         /* Type: */
2428         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
2429
2430         /* Length: */
2431         /* Note: In the WFD specification, the size of length field is 2. */
2432         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
2433         wfdielen += 2;
2434
2435         /* Value: */
2436         /* Associated BSSID */
2437         if (check_fwstate( pmlmepriv, _FW_LINKED) == _TRUE)
2438                 _rtw_memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
2439         else
2440                 _rtw_memset(wfdie + wfdielen, 0x00, ETH_ALEN);
2441
2442         /* Local IP Address ATTR */
2443         wfdie[ wfdielen++ ] = WFD_ATTR_LOCAL_IP_ADDR;
2444
2445         /* Length: */
2446         /* Note: In the WFD specification, the size of length field is 2. */
2447         RTW_PUT_BE16(wfdie + wfdielen, 0x0005);
2448         wfdielen += 2;
2449
2450         /* Version: */
2451         /* 0x01: Version1;IPv4 */
2452         wfdie[ wfdielen++ ] = 0x01;
2453
2454         /* IPv4 Address */
2455         _rtw_memcpy( wfdie + wfdielen, pwfd_info->ip_address, 4 );
2456         wfdielen += 4;
2457
2458         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, pktlen);
2459
2460 }
2461 #endif /* CONFIG_WFD */
2462
2463 void rtw_build_tdls_setup_req_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
2464 {
2465         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
2466         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
2467         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2468         struct sta_info *ptdls_sta=rtw_get_stainfo( (&padapter->stapriv) , pattrib->dst);
2469
2470         int i = 0 ;
2471         u32 time;
2472         u8 *pframe_head;
2473
2474         /* SNonce */
2475         if (pattrib->encrypt) {
2476                 for (i=0;i<8;i++) {
2477                         time=rtw_get_current_time();
2478                         _rtw_memcpy(&ptdls_sta->SNonce[4*i], (u8 *)&time, 4);
2479                 }
2480         }
2481
2482         pframe_head = pframe;   /* For rtw_tdls_set_ht_cap() */
2483
2484         pframe = rtw_tdls_set_payload_type(pframe, pattrib);
2485         pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
2486         pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
2487         pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
2488
2489         pframe = rtw_tdls_set_capability(padapter, pframe, pattrib);
2490         pframe = rtw_tdls_set_supported_rate(padapter, pframe, pattrib);
2491         pframe = rtw_tdls_set_sup_ch(&(padapter->mlmeextpriv), pframe, pattrib);
2492         pframe = rtw_tdls_set_sup_reg_class(pframe, pattrib);
2493
2494         if (pattrib->encrypt)
2495                 pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib,  _TRUE, ptdls_sta);
2496
2497         pframe = rtw_tdls_set_ext_cap(pframe, pattrib);
2498
2499         if (pattrib->encrypt) {
2500                 pframe = rtw_tdls_set_ftie(ptxmgmt
2501                                                                         , pframe
2502                                                                         , pattrib
2503                                                                         , NULL
2504                                                                         , ptdls_sta->SNonce);
2505
2506                 pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib, _TRUE, ptdls_sta);
2507         }
2508
2509 #ifdef CONFIG_80211N_HT
2510         /* Sup_reg_classes(optional) */
2511         if (pregistrypriv->ht_enable == _TRUE)
2512                 pframe = rtw_tdls_set_ht_cap(padapter, pframe_head, pattrib);
2513 #endif
2514
2515         pframe = rtw_tdls_set_bss_coexist(padapter, pframe, pattrib);
2516
2517         pframe = rtw_tdls_set_linkid(pframe, pattrib, _TRUE);
2518
2519         if ((pregistrypriv->wmm_enable == _TRUE) || (padapter->mlmepriv.htpriv.ht_option == _TRUE))
2520                 pframe = rtw_tdls_set_qos_cap(pframe, pattrib);
2521
2522
2523 #ifdef CONFIG_WFD
2524         if (padapter->wdinfo.wfd_tdls_enable == 1)
2525                 wfd_ie_tdls(padapter, pframe, &(pattrib->pktlen));
2526 #endif
2527
2528 }
2529
2530 void rtw_build_tdls_setup_rsp_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
2531 {
2532         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
2533         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
2534         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2535         struct sta_info *ptdls_sta;
2536         u8 k; /* for random ANonce */
2537         u8  *pftie=NULL, *ptimeout_ie = NULL, *plinkid_ie = NULL, *prsnie = NULL, *pftie_mic = NULL;
2538         u32 time;
2539         u8 *pframe_head;
2540
2541         ptdls_sta = rtw_get_stainfo( &(padapter->stapriv) , pattrib->dst);
2542
2543         if (ptdls_sta == NULL)
2544                 DBG_871X("[%s] %d ptdls_sta is NULL\n", __FUNCTION__, __LINE__);
2545
2546         if (pattrib->encrypt && ptdls_sta != NULL) {
2547                 for (k=0;k<8;k++) {
2548                         time = rtw_get_current_time();
2549                         _rtw_memcpy(&ptdls_sta->ANonce[4*k], (u8*)&time, 4);
2550                 }
2551         }
2552
2553         pframe_head = pframe;
2554
2555         pframe = rtw_tdls_set_payload_type(pframe, pattrib);
2556         pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
2557         pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
2558         pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt);
2559
2560         if (ptxmgmt->status_code != 0) {
2561                 DBG_871X("[%s] status_code:%04x \n", __FUNCTION__, ptxmgmt->status_code);
2562                 return;
2563         }
2564
2565         pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
2566         pframe = rtw_tdls_set_capability(padapter, pframe, pattrib);
2567         pframe = rtw_tdls_set_supported_rate(padapter, pframe, pattrib);
2568         pframe = rtw_tdls_set_sup_ch(&(padapter->mlmeextpriv), pframe, pattrib);
2569         pframe = rtw_tdls_set_sup_reg_class(pframe, pattrib);
2570
2571         if (pattrib->encrypt) {
2572                 prsnie = pframe;
2573                 pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib,  _FALSE, ptdls_sta);
2574         }
2575
2576         pframe = rtw_tdls_set_ext_cap(pframe, pattrib);
2577
2578         if (pattrib->encrypt) {
2579                 if (rtw_tdls_is_driver_setup(padapter) == _TRUE)
2580                         wpa_tdls_generate_tpk(padapter, ptdls_sta);
2581
2582                 pftie = pframe;
2583                 pftie_mic = pframe+4;
2584                 pframe = rtw_tdls_set_ftie(ptxmgmt
2585                                                                         , pframe
2586                                                                         , pattrib
2587                                                                         , ptdls_sta->ANonce
2588                                                                         , ptdls_sta->SNonce);
2589
2590                 ptimeout_ie = pframe;
2591                 pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib, _FALSE, ptdls_sta);
2592         }
2593
2594 #ifdef CONFIG_80211N_HT
2595         /* Sup_reg_classes(optional) */
2596         if (pregistrypriv->ht_enable == _TRUE)
2597                 pframe = rtw_tdls_set_ht_cap(padapter, pframe_head, pattrib);
2598 #endif
2599
2600         pframe = rtw_tdls_set_bss_coexist(padapter, pframe, pattrib);
2601
2602         plinkid_ie = pframe;
2603         pframe = rtw_tdls_set_linkid(pframe, pattrib, _FALSE);
2604
2605         /* Fill FTIE mic */
2606         if (pattrib->encrypt && rtw_tdls_is_driver_setup(padapter) == _TRUE)
2607                 wpa_tdls_ftie_mic(ptdls_sta->tpk.kck, 2, plinkid_ie, prsnie, ptimeout_ie, pftie, pftie_mic);
2608
2609         if ((pregistrypriv->wmm_enable == _TRUE) || (padapter->mlmepriv.htpriv.ht_option == _TRUE))
2610                 pframe = rtw_tdls_set_qos_cap(pframe, pattrib);
2611
2612
2613 #ifdef CONFIG_WFD
2614         if (padapter->wdinfo.wfd_tdls_enable)
2615                 wfd_ie_tdls(padapter, pframe, &(pattrib->pktlen));
2616 #endif
2617
2618 }
2619
2620 void rtw_build_tdls_setup_cfm_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
2621 {
2622         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
2623         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
2624         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2625         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2626         struct sta_info *ptdls_sta=rtw_get_stainfo( (&padapter->stapriv) , pattrib->dst);
2627
2628         unsigned int ie_len;
2629         unsigned char *p;
2630         u8 wmm_param_ele[24] = {0};
2631         u8  *pftie=NULL, *ptimeout_ie=NULL, *plinkid_ie=NULL, *prsnie=NULL, *pftie_mic=NULL;
2632
2633         pframe = rtw_tdls_set_payload_type(pframe, pattrib);
2634         pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
2635         pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
2636         pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt);
2637         pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
2638
2639         if (ptxmgmt->status_code!=0)
2640                 return;
2641
2642         if (pattrib->encrypt) {
2643                 prsnie = pframe;
2644                 pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib, _TRUE, ptdls_sta);
2645         }
2646
2647         if (pattrib->encrypt) {
2648                 pftie = pframe;
2649                 pftie_mic = pframe+4;
2650                 pframe = rtw_tdls_set_ftie(ptxmgmt
2651                                                                         , pframe
2652                                                                         , pattrib
2653                                                                         , ptdls_sta->ANonce
2654                                                                         , ptdls_sta->SNonce);
2655
2656                 ptimeout_ie = pframe;
2657                 pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib, _TRUE, ptdls_sta);
2658
2659                 if (rtw_tdls_is_driver_setup(padapter) == _TRUE) {
2660                         /* Start TPK timer */
2661                         ptdls_sta->TPK_count=0;
2662                         _set_timer(&ptdls_sta->TPK_timer, ONE_SEC);
2663                 }
2664         }
2665
2666         /* HT operation; todo */
2667
2668         plinkid_ie = pframe;
2669         pframe = rtw_tdls_set_linkid(pframe, pattrib, _TRUE);
2670
2671         if (pattrib->encrypt && (rtw_tdls_is_driver_setup(padapter) == _TRUE))
2672                 wpa_tdls_ftie_mic(ptdls_sta->tpk.kck, 3, plinkid_ie, prsnie, ptimeout_ie, pftie, pftie_mic);
2673
2674         if (ptdls_sta->qos_option == _TRUE)
2675                 pframe = rtw_tdls_set_wmm_params(padapter, pframe, pattrib);
2676
2677 }
2678
2679 void rtw_build_tdls_teardown_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
2680 {
2681         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2682         struct sta_info *ptdls_sta = rtw_get_stainfo( &(padapter->stapriv) , pattrib->dst);
2683         u8  *pftie = NULL, *pftie_mic = NULL, *plinkid_ie = NULL;
2684
2685         pframe = rtw_tdls_set_payload_type(pframe, pattrib);
2686         pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
2687         pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
2688         pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt);
2689
2690         if (pattrib->encrypt) {
2691                 pftie = pframe;
2692                 pftie_mic = pframe + 4;
2693                 pframe = rtw_tdls_set_ftie(ptxmgmt
2694                                                                         , pframe
2695                                                                         , pattrib
2696                                                                         , ptdls_sta->ANonce
2697                                                                         , ptdls_sta->SNonce);
2698         }
2699
2700         plinkid_ie = pframe;
2701         if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE)
2702                 pframe = rtw_tdls_set_linkid(pframe, pattrib, _FALSE);
2703         else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE)
2704                 pframe = rtw_tdls_set_linkid(pframe, pattrib, _TRUE);
2705
2706         if (pattrib->encrypt && (rtw_tdls_is_driver_setup(padapter) == _TRUE))
2707                 wpa_tdls_teardown_ftie_mic(ptdls_sta->tpk.kck, plinkid_ie, ptxmgmt->status_code, 1, 4, pftie, pftie_mic);
2708 }
2709
2710 void rtw_build_tdls_dis_req_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
2711 {
2712         struct pkt_attrib *pattrib = &pxmitframe->attrib;
2713
2714         pframe = rtw_tdls_set_payload_type(pframe, pattrib);
2715         pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
2716         pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
2717         pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
2718         pframe = rtw_tdls_set_linkid(pframe, pattrib, _TRUE);
2719
2720 }
2721
2722 void rtw_build_tdls_dis_rsp_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt, u8 privacy)
2723 {
2724         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
2725         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2726         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2727         u8 *pframe_head, pktlen_index;
2728
2729         pktlen_index = pattrib->pktlen;
2730         pframe_head = pframe;
2731
2732         pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_PUBLIC);
2733         pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
2734         pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
2735         pframe = rtw_tdls_set_capability(padapter, pframe, pattrib);
2736
2737         pframe = rtw_tdls_set_supported_rate(padapter, pframe, pattrib);
2738
2739         pframe = rtw_tdls_set_sup_ch(pmlmeext, pframe, pattrib);
2740
2741         if (privacy)
2742                 pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib, _TRUE, NULL);
2743
2744         pframe = rtw_tdls_set_ext_cap(pframe, pattrib);
2745
2746         if (privacy) {
2747                 pframe = rtw_tdls_set_ftie(ptxmgmt, pframe, pattrib, NULL, NULL);
2748                 pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib,  _TRUE, NULL);
2749         }
2750
2751 #ifdef CONFIG_80211N_HT
2752         if (pregistrypriv->ht_enable == _TRUE)
2753                 pframe = rtw_tdls_set_ht_cap(padapter, pframe_head - pktlen_index, pattrib);
2754 #endif
2755
2756         pframe = rtw_tdls_set_bss_coexist(padapter, pframe, pattrib);
2757         pframe = rtw_tdls_set_linkid(pframe, pattrib, _FALSE);
2758
2759 }
2760
2761
2762 void rtw_build_tdls_peer_traffic_indication_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
2763 {
2764
2765         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2766         u8 AC_queue=0;
2767         struct sta_info *ptdls_sta = rtw_get_stainfo(&padapter->stapriv, pattrib->dst);
2768
2769         pframe = rtw_tdls_set_payload_type(pframe, pattrib);
2770         pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
2771         pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
2772         pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
2773
2774         if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE)
2775                 pframe = rtw_tdls_set_linkid(pframe, pattrib, _FALSE);
2776         else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE)
2777                 pframe = rtw_tdls_set_linkid(pframe, pattrib, _TRUE);
2778
2779         /* PTI control */
2780         /* PU buffer status */
2781         if (ptdls_sta->uapsd_bk & BIT(1))
2782                 AC_queue=BIT(0);
2783         if (ptdls_sta->uapsd_be & BIT(1))
2784                 AC_queue=BIT(1);
2785         if (ptdls_sta->uapsd_vi & BIT(1))
2786                 AC_queue=BIT(2);
2787         if (ptdls_sta->uapsd_vo & BIT(1))
2788                 AC_queue=BIT(3);
2789         pframe = rtw_set_ie(pframe, _PTI_BUFFER_STATUS_, 1, &AC_queue, &(pattrib->pktlen));
2790
2791 }
2792
2793 void rtw_build_tdls_peer_traffic_rsp_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
2794 {
2795
2796         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2797         struct sta_info *ptdls_sta = rtw_get_stainfo(&padapter->stapriv, pattrib->dst);
2798
2799         pframe = rtw_tdls_set_payload_type(pframe, pattrib);
2800         pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
2801         pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
2802         pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
2803
2804         if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE)
2805                 pframe = rtw_tdls_set_linkid(pframe, pattrib, _FALSE);
2806         else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE)
2807                 pframe = rtw_tdls_set_linkid(pframe, pattrib, _TRUE);
2808 }
2809
2810 #ifdef CONFIG_TDLS_CH_SW
2811 void rtw_build_tdls_ch_switch_req_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
2812 {
2813         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
2814         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2815         struct sta_priv         *pstapriv = &padapter->stapriv;
2816         struct sta_info *ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst);
2817         u16 switch_time = TDLS_CH_SWITCH_TIME * 1000, switch_timeout = TDLS_CH_SWITCH_TIMEOUT * 1000;
2818
2819         ptdls_sta->ch_switch_time=switch_time;
2820         ptdls_sta->ch_switch_timeout=switch_timeout;
2821
2822         pframe = rtw_tdls_set_payload_type(pframe, pattrib);
2823         pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
2824         pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
2825         pframe = rtw_tdls_set_target_ch(padapter, pframe, pattrib);
2826         pframe = rtw_tdls_set_reg_class(pframe, pattrib, ptdls_sta);
2827
2828         if (ptdlsinfo->chsw_info.ch_offset != HAL_PRIME_CHNL_OFFSET_DONT_CARE) {
2829                 switch (ptdlsinfo->chsw_info.ch_offset) {
2830                 case HAL_PRIME_CHNL_OFFSET_LOWER:
2831                         pframe = rtw_tdls_set_second_channel_offset(pframe, pattrib, SCA);
2832                         break;
2833                 case HAL_PRIME_CHNL_OFFSET_UPPER:
2834                         pframe = rtw_tdls_set_second_channel_offset(pframe, pattrib, SCB);
2835                         break;
2836                 }
2837         }
2838
2839         if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE)
2840                 pframe = rtw_tdls_set_linkid(pframe, pattrib, _FALSE);
2841         else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE)
2842                 pframe = rtw_tdls_set_linkid(pframe, pattrib, _TRUE);
2843
2844         pframe = rtw_tdls_set_ch_sw(pframe, pattrib, ptdls_sta);
2845
2846 }
2847
2848 void rtw_build_tdls_ch_switch_rsp_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
2849 {
2850
2851         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2852         struct sta_priv         *pstapriv = &padapter->stapriv;
2853         struct sta_info *ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst);
2854
2855         pframe = rtw_tdls_set_payload_type(pframe, pattrib);
2856         pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
2857         pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
2858         pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt);
2859
2860         if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE)
2861                 pframe = rtw_tdls_set_linkid(pframe, pattrib, _FALSE);
2862         else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE)
2863                 pframe = rtw_tdls_set_linkid(pframe, pattrib, _TRUE);
2864
2865         pframe = rtw_tdls_set_ch_sw(pframe, pattrib, ptdls_sta);
2866 }
2867 #endif
2868
2869 #ifdef CONFIG_WFD
2870 void rtw_build_tunneled_probe_req_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)
2871 {
2872
2873         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2874         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
2875         struct wifidirect_info *pbuddy_wdinfo = &padapter->pbuddy_adapter->wdinfo;
2876         u8 category = RTW_WLAN_CATEGORY_P2P;
2877         u8 WFA_OUI[3] = { 0x50, 0x6f, 0x9a};
2878         u8 probe_req = 4;
2879         u8 wfdielen = 0;
2880
2881         pframe = rtw_tdls_set_payload_type(pframe, pattrib);
2882         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
2883         pframe = rtw_set_fixed_ie(pframe, 3, WFA_OUI, &(pattrib->pktlen));
2884         pframe = rtw_set_fixed_ie(pframe, 1, &(probe_req), &(pattrib->pktlen));
2885
2886         if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
2887                 wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe);
2888                 pframe += wfdielen;
2889                 pattrib->pktlen += wfdielen;
2890         } else if (!rtw_p2p_chk_state(pbuddy_wdinfo, P2P_STATE_NONE)) {
2891                 wfdielen = build_probe_req_wfd_ie(pbuddy_wdinfo, pframe);
2892                 pframe += wfdielen;
2893                 pattrib->pktlen += wfdielen;
2894         }
2895
2896 }
2897
2898 void rtw_build_tunneled_probe_rsp_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)
2899 {
2900
2901         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2902         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
2903         struct wifidirect_info *pbuddy_wdinfo = &padapter->pbuddy_adapter->wdinfo;
2904         u8 category = RTW_WLAN_CATEGORY_P2P;
2905         u8 WFA_OUI[3] = { 0x50, 0x6f, 0x9a};
2906         u8 probe_rsp = 5;
2907         u8 wfdielen = 0;
2908
2909         pframe = rtw_tdls_set_payload_type(pframe, pattrib);
2910         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
2911         pframe = rtw_set_fixed_ie(pframe, 3, WFA_OUI, &(pattrib->pktlen));
2912         pframe = rtw_set_fixed_ie(pframe, 1, &(probe_rsp), &(pattrib->pktlen));
2913
2914         if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
2915                 wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 1);
2916                 pframe += wfdielen;
2917                 pattrib->pktlen += wfdielen;
2918         } else if (!rtw_p2p_chk_state(pbuddy_wdinfo, P2P_STATE_NONE)) {
2919                 wfdielen = build_probe_resp_wfd_ie(pbuddy_wdinfo, pframe, 1);
2920                 pframe += wfdielen;
2921                 pattrib->pktlen += wfdielen;
2922         }
2923
2924 }
2925 #endif /* CONFIG_WFD */
2926
2927 void _tdls_tpk_timer_hdl(void *FunctionContext)
2928 {
2929         struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
2930         struct tdls_txmgmt txmgmt;
2931
2932         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
2933         ptdls_sta->TPK_count++;
2934         /* TPK_timer expired in a second */
2935         /* Retry timer should set at least 301 sec. */
2936         if (ptdls_sta->TPK_count >= (ptdls_sta->TDLS_PeerKey_Lifetime - 3)) {
2937                 DBG_871X("[TDLS] %s, Re-Setup TDLS link with "MAC_FMT" since TPK lifetime expires!\n", __FUNCTION__, MAC_ARG(ptdls_sta->hwaddr));
2938                 ptdls_sta->TPK_count=0;
2939                 _rtw_memcpy(txmgmt.peer, ptdls_sta->hwaddr, ETH_ALEN);
2940                 issue_tdls_setup_req(ptdls_sta->padapter, &txmgmt, _FALSE);
2941         }
2942
2943         _set_timer(&ptdls_sta->TPK_timer, ONE_SEC);
2944 }
2945
2946 #ifdef CONFIG_TDLS_CH_SW
2947 void _tdls_ch_switch_timer_hdl(void *FunctionContext)
2948 {
2949         struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
2950         _adapter *padapter = ptdls_sta->padapter;
2951         struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
2952
2953         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_END_TO_BASE_CHNL);
2954         DBG_871X("[TDLS] %s, can't get traffic from op_ch:%d\n", __func__, rtw_get_oper_ch(padapter));
2955 }
2956
2957 void _tdls_delay_timer_hdl(void *FunctionContext)
2958 {
2959         struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
2960         _adapter *padapter = ptdls_sta->padapter;
2961         struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
2962
2963         DBG_871X("[TDLS] %s, op_ch:%d, tdls_state:0x%08x\n", __func__, rtw_get_oper_ch(padapter), ptdls_sta->tdls_sta_state);
2964         pchsw_info->delay_switch_back = _TRUE;
2965 }
2966
2967 void _tdls_stay_on_base_chnl_timer_hdl(void *FunctionContext)
2968 {
2969         struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
2970         _adapter *padapter = ptdls_sta->padapter;
2971         struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
2972
2973         if (ptdls_sta != NULL) {
2974                 issue_tdls_ch_switch_req(padapter, ptdls_sta);
2975                 pchsw_info->ch_sw_state |= TDLS_WAIT_CH_RSP_STATE;
2976         }
2977 }
2978
2979 void _tdls_ch_switch_monitor_timer_hdl(void *FunctionContext)
2980 {
2981         struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
2982         _adapter *padapter = ptdls_sta->padapter;
2983         struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
2984
2985         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_END);
2986         DBG_871X("[TDLS] %s, does not receive ch sw req\n", __func__);
2987 }
2988
2989 #endif
2990
2991 void _tdls_handshake_timer_hdl(void *FunctionContext)
2992 {
2993         struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
2994         _adapter *padapter = ptdls_sta->padapter;
2995         struct tdls_txmgmt txmgmt;
2996
2997         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
2998         _rtw_memcpy(txmgmt.peer, ptdls_sta->hwaddr, ETH_ALEN);
2999         txmgmt.status_code = _RSON_TDLS_TEAR_UN_RSN_;
3000
3001         if (ptdls_sta != NULL) {
3002                 DBG_871X("[TDLS] Handshake time out\n");
3003                 if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)
3004                 {
3005                         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_TEARDOWN_STA);
3006                 }
3007                 else
3008                 {
3009                         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_TEARDOWN_STA_LOCALLY);
3010                 }
3011         }
3012 }
3013
3014 void _tdls_pti_timer_hdl(void *FunctionContext)
3015 {
3016         struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
3017         _adapter *padapter = ptdls_sta->padapter;
3018         struct tdls_txmgmt txmgmt;
3019
3020         _rtw_memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
3021         _rtw_memcpy(txmgmt.peer, ptdls_sta->hwaddr, ETH_ALEN);
3022         txmgmt.status_code = _RSON_TDLS_TEAR_TOOFAR_;
3023
3024         if (ptdls_sta != NULL) {
3025                 if (ptdls_sta->tdls_sta_state & TDLS_WAIT_PTR_STATE) {
3026                         DBG_871X("[TDLS] Doesn't receive PTR from peer dev:"MAC_FMT"; "
3027                                 "Send TDLS Tear Down\n", MAC_ARG(ptdls_sta->hwaddr));
3028                         issue_tdls_teardown(padapter, &txmgmt, _FALSE);
3029                 }
3030         }
3031 }
3032
3033 void rtw_init_tdls_timer(_adapter *padapter, struct sta_info *psta)
3034 {
3035         psta->padapter=padapter;
3036         _init_timer(&psta->TPK_timer, padapter->pnetdev, _tdls_tpk_timer_hdl, psta);
3037 #ifdef CONFIG_TDLS_CH_SW
3038         _init_timer(&psta->ch_sw_timer, padapter->pnetdev, _tdls_ch_switch_timer_hdl, psta);
3039         _init_timer(&psta->delay_timer, padapter->pnetdev, _tdls_delay_timer_hdl, psta);
3040         _init_timer(&psta->stay_on_base_chnl_timer, padapter->pnetdev, _tdls_stay_on_base_chnl_timer_hdl, psta);
3041         _init_timer(&psta->ch_sw_monitor_timer, padapter->pnetdev, _tdls_ch_switch_monitor_timer_hdl, psta);
3042 #endif
3043         _init_timer(&psta->handshake_timer, padapter->pnetdev, _tdls_handshake_timer_hdl, psta);
3044         _init_timer(&psta->pti_timer, padapter->pnetdev, _tdls_pti_timer_hdl, psta);
3045 }
3046
3047 void rtw_free_tdls_timer(struct sta_info *psta)
3048 {
3049         _cancel_timer_ex(&psta->TPK_timer);
3050 #ifdef CONFIG_TDLS_CH_SW
3051         _cancel_timer_ex(&psta->ch_sw_timer);
3052         _cancel_timer_ex(&psta->delay_timer);
3053         _cancel_timer_ex(&psta->stay_on_base_chnl_timer);
3054         _cancel_timer_ex(&psta->ch_sw_monitor_timer);
3055 #endif
3056         _cancel_timer_ex(&psta->handshake_timer);
3057         _cancel_timer_ex(&psta->pti_timer);
3058 }
3059
3060 u8      update_sgi_tdls(_adapter *padapter, struct sta_info *psta)
3061 {
3062         return query_ra_short_GI(psta);
3063 }
3064
3065 u32 update_mask_tdls(_adapter *padapter, struct sta_info *psta)
3066 {
3067         unsigned char sta_band = 0;
3068         unsigned int tx_ra_bitmap=0;
3069         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
3070         WLAN_BSSID_EX *pcur_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;
3071
3072         rtw_hal_update_sta_rate_mask(padapter, psta);
3073         tx_ra_bitmap = psta->ra_mask;
3074
3075         if (pcur_network->Configuration.DSConfig > 14) {
3076                 if (tx_ra_bitmap & 0xffff000)
3077                         sta_band |= WIRELESS_11_5N | WIRELESS_11A;
3078                 else
3079                         sta_band |= WIRELESS_11A;
3080         } else {
3081                 if (tx_ra_bitmap & 0xffff000)
3082                         sta_band |= WIRELESS_11_24N | WIRELESS_11G | WIRELESS_11B;
3083                 else if (tx_ra_bitmap & 0xff0)
3084                         sta_band |= WIRELESS_11G |WIRELESS_11B;
3085                 else
3086                         sta_band |= WIRELESS_11B;
3087         }
3088
3089         psta->wireless_mode = sta_band;
3090
3091         psta->raid = rtw_hal_networktype_to_raid(padapter,psta);
3092         tx_ra_bitmap |= ((psta->raid<<28)&0xf0000000);
3093         return tx_ra_bitmap;
3094 }
3095
3096 int rtw_tdls_is_driver_setup(_adapter *padapter)
3097 {
3098         return padapter->tdlsinfo.driver_setup;
3099 }
3100
3101 const char * rtw_tdls_action_txt(enum TDLS_ACTION_FIELD action)
3102 {
3103         switch (action) {
3104         case TDLS_SETUP_REQUEST:
3105                 return "TDLS_SETUP_REQUEST";
3106         case TDLS_SETUP_RESPONSE:
3107                 return "TDLS_SETUP_RESPONSE";
3108         case TDLS_SETUP_CONFIRM:
3109                 return "TDLS_SETUP_CONFIRM";
3110         case TDLS_TEARDOWN:
3111                 return "TDLS_TEARDOWN";
3112         case TDLS_PEER_TRAFFIC_INDICATION:
3113                 return "TDLS_PEER_TRAFFIC_INDICATION";
3114         case TDLS_CHANNEL_SWITCH_REQUEST:
3115                 return "TDLS_CHANNEL_SWITCH_REQUEST";
3116         case TDLS_CHANNEL_SWITCH_RESPONSE:
3117                 return "TDLS_CHANNEL_SWITCH_RESPONSE";
3118         case TDLS_PEER_PSM_REQUEST:
3119                 return "TDLS_PEER_PSM_REQUEST";
3120         case TDLS_PEER_PSM_RESPONSE:
3121                 return "TDLS_PEER_PSM_RESPONSE";
3122         case TDLS_PEER_TRAFFIC_RESPONSE:
3123                 return "TDLS_PEER_TRAFFIC_RESPONSE";
3124         case TDLS_DISCOVERY_REQUEST:
3125                 return "TDLS_DISCOVERY_REQUEST";
3126         case TDLS_DISCOVERY_RESPONSE:
3127                 return "TDLS_DISCOVERY_RESPONSE";
3128         default:
3129                 return "UNKNOWN";
3130         }
3131 }
3132
3133 #endif /* CONFIG_TDLS */