rtl8723bu/core/rtw_p2p.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_P2P_C_
  21
  22 #include <drv_types.h>
  23
  24 #ifdef CONFIG_P2P
  25
  26 int rtw_p2p_is_channel_list_ok( u8 desired_ch, u8* ch_list, u8 ch_cnt )
  27 {
  28         int found = 0, i = 0;
  29
  30         for( i = 0; i < ch_cnt; i++ )
  31         {
  32                 if ( ch_list[ i ] == desired_ch )
  33                 {
  34                         found = 1;
  35                         break;
  36                 }
  37         }
  38         return( found );
  39 }
  40
  41 int is_any_client_associated(_adapter *padapter)
  42 {
  43         return padapter->stapriv.asoc_list_cnt ? _TRUE : _FALSE;
  44 }
  45
  46 static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf)
  47 {
  48         _irqL irqL;
  49         _list   *phead, *plist;
  50         u32 len=0;
  51         u16 attr_len = 0;
  52         u8 tmplen, *pdata_attr, *pstart, *pcur;
  53         struct sta_info *psta = NULL;
  54         _adapter *padapter = pwdinfo->padapter;
  55         struct sta_priv *pstapriv = &padapter->stapriv;
  56
  57         DBG_871X("%s\n", __FUNCTION__);
  58
  59         pdata_attr = rtw_zmalloc(MAX_P2P_IE_LEN);
  60
  61         if(NULL == pdata_attr){
  62                 DBG_871X("%s pdata_attr malloc failed \n", __FUNCTION__);
  63                 goto _exit;
  64         }
  65
  66         pstart = pdata_attr;
  67         pcur = pdata_attr;
  68
  69         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
  70         phead = &pstapriv->asoc_list;
  71         plist = get_next(phead);
  72
  73         //look up sta asoc_queue
  74         while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
  75         {
  76                 psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
  77
  78                 plist = get_next(plist);
  79
  80
  81                 if(psta->is_p2p_device)
  82                 {
  83                         tmplen = 0;
  84
  85                         pcur++;
  86
  87                         //P2P device address
  88                         _rtw_memcpy(pcur, psta->dev_addr, ETH_ALEN);
  89                         pcur += ETH_ALEN;
  90
  91                         //P2P interface address
  92                         _rtw_memcpy(pcur, psta->hwaddr, ETH_ALEN);
  93                         pcur += ETH_ALEN;
  94
  95                         *pcur = psta->dev_cap;
  96                         pcur++;
  97
  98                         //*(u16*)(pcur) = cpu_to_be16(psta->config_methods);
  99                         RTW_PUT_BE16(pcur, psta->config_methods);
 100                         pcur += 2;
 101
 102                         _rtw_memcpy(pcur, psta->primary_dev_type, 8);
 103                         pcur += 8;
 104
 105                         *pcur = psta->num_of_secdev_type;
 106                         pcur++;
 107
 108                         _rtw_memcpy(pcur, psta->secdev_types_list, psta->num_of_secdev_type*8);
 109                         pcur += psta->num_of_secdev_type*8;
 110
 111                         if(psta->dev_name_len>0)
 112                         {
 113                                 //*(u16*)(pcur) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
 114                                 RTW_PUT_BE16(pcur, WPS_ATTR_DEVICE_NAME);
 115                                 pcur += 2;
 116
 117                                 //*(u16*)(pcur) = cpu_to_be16( psta->dev_name_len );
 118                                 RTW_PUT_BE16(pcur, psta->dev_name_len);
 119                                 pcur += 2;
 120
 121                                 _rtw_memcpy(pcur, psta->dev_name, psta->dev_name_len);
 122                                 pcur += psta->dev_name_len;
 123                         }
 124
 125
 126                         tmplen = (u8)(pcur-pstart);
 127
 128                         *pstart = (tmplen-1);
 129
 130                         attr_len += tmplen;
 131
 132                         //pstart += tmplen;
 133                         pstart = pcur;
 134
 135                 }
 136
 137
 138         }
 139         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
 140
 141         if(attr_len>0)
 142         {
 143                 len = rtw_set_p2p_attr_content(pbuf, P2P_ATTR_GROUP_INFO, attr_len, pdata_attr);
 144         }
 145
 146         rtw_mfree(pdata_attr, MAX_P2P_IE_LEN);
 147
 148 _exit:
 149         return len;
 150
 151 }
 152
 153 static void issue_group_disc_req(struct wifidirect_info *pwdinfo, u8 *da)
 154 {
 155         struct xmit_frame                       *pmgntframe;
 156         struct pkt_attrib                       *pattrib;
 157         unsigned char                                   *pframe;
 158         struct rtw_ieee80211_hdr        *pwlanhdr;
 159         unsigned short                          *fctrl;
 160         _adapter *padapter = pwdinfo->padapter;
 161         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 162         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 163         unsigned char category = RTW_WLAN_CATEGORY_P2P;//P2P action frame
 164         u32     p2poui = cpu_to_be32(P2POUI);
 165         u8      oui_subtype = P2P_GO_DISC_REQUEST;
 166         u8      dialogToken=0;
 167
 168         DBG_871X("[%s]\n", __FUNCTION__);
 169
 170         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
 171         {
 172                 return;
 173         }
 174
 175         //update attribute
 176         pattrib = &pmgntframe->attrib;
 177         update_mgntframe_attrib(padapter, pattrib);
 178
 179         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 180
 181         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 182         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 183
 184         fctrl = &(pwlanhdr->frame_ctl);
 185         *(fctrl) = 0;
 186
 187         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
 188         _rtw_memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
 189         _rtw_memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
 190
 191         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 192         pmlmeext->mgnt_seq++;
 193         SetFrameSubType(pframe, WIFI_ACTION);
 194
 195         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
 196         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
 197
 198         //Build P2P action frame header
 199         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
 200         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
 201         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
 202         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
 203
 204         //there is no IE in this P2P action frame
 205
 206         pattrib->last_txcmdsz = pattrib->pktlen;
 207
 208         dump_mgntframe(padapter, pmgntframe);
 209
 210 }
 211
 212 static void issue_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
 213 {
 214         struct xmit_frame                       *pmgntframe;
 215         struct pkt_attrib                       *pattrib;
 216         unsigned char                                   *pframe;
 217         struct rtw_ieee80211_hdr        *pwlanhdr;
 218         unsigned short                          *fctrl;
 219         _adapter *padapter = pwdinfo->padapter;
 220         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 221         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 222         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
 223         u8                      action = P2P_PUB_ACTION_ACTION;
 224         u32                     p2poui = cpu_to_be32(P2POUI);
 225         u8                      oui_subtype = P2P_DEVDISC_RESP;
 226         u8 p2pie[8] = { 0x00 };
 227         u32 p2pielen = 0;
 228
 229         DBG_871X("[%s]\n", __FUNCTION__);
 230
 231         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
 232         {
 233                 return;
 234         }
 235
 236         //update attribute
 237         pattrib = &pmgntframe->attrib;
 238         update_mgntframe_attrib(padapter, pattrib);
 239
 240         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 241
 242         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 243         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 244
 245         fctrl = &(pwlanhdr->frame_ctl);
 246         *(fctrl) = 0;
 247
 248         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
 249         _rtw_memcpy(pwlanhdr->addr2, pwdinfo->device_addr, ETH_ALEN);
 250         _rtw_memcpy(pwlanhdr->addr3, pwdinfo->device_addr, ETH_ALEN);
 251
 252         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 253         pmlmeext->mgnt_seq++;
 254         SetFrameSubType(pframe, WIFI_ACTION);
 255
 256         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
 257         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
 258
 259         //Build P2P public action frame header
 260         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
 261         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
 262         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
 263         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
 264         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
 265
 266
 267         //Build P2P IE
 268         //      P2P OUI
 269         p2pielen = 0;
 270         p2pie[ p2pielen++ ] = 0x50;
 271         p2pie[ p2pielen++ ] = 0x6F;
 272         p2pie[ p2pielen++ ] = 0x9A;
 273         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
 274
 275         // P2P_ATTR_STATUS
 276         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
 277
 278         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen);
 279
 280         pattrib->last_txcmdsz = pattrib->pktlen;
 281
 282         dump_mgntframe(padapter, pmgntframe);
 283
 284 }
 285
 286 static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8* raddr, u8* frame_body, u16 config_method)
 287 {
 288         _adapter *padapter = pwdinfo->padapter;
 289         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
 290         u8                      action = P2P_PUB_ACTION_ACTION;
 291         u8                      dialogToken = frame_body[7];    //      The Dialog Token of provisioning discovery request frame.
 292         u32                     p2poui = cpu_to_be32(P2POUI);
 293         u8                      oui_subtype = P2P_PROVISION_DISC_RESP;
 294         u8                      wpsie[ 100 ] = { 0x00 };
 295         u8                      wpsielen = 0;
 296 #ifdef CONFIG_WFD
 297         u32                                     wfdielen = 0;
 298 #endif
 299
 300         struct xmit_frame                       *pmgntframe;
 301         struct pkt_attrib                       *pattrib;
 302         unsigned char                                   *pframe;
 303         struct rtw_ieee80211_hdr        *pwlanhdr;
 304         unsigned short                          *fctrl;
 305         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 306         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 307         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 308
 309
 310         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
 311         {
 312                 return;
 313         }
 314
 315         //update attribute
 316         pattrib = &pmgntframe->attrib;
 317         update_mgntframe_attrib(padapter, pattrib);
 318
 319         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 320
 321         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 322         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 323
 324         fctrl = &(pwlanhdr->frame_ctl);
 325         *(fctrl) = 0;
 326
 327         _rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
 328         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
 329         _rtw_memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN);
 330
 331         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 332         pmlmeext->mgnt_seq++;
 333         SetFrameSubType(pframe, WIFI_ACTION);
 334
 335         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
 336         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
 337
 338         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
 339         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
 340         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
 341         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
 342         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
 343
 344         wpsielen = 0;
 345         //      WPS OUI
 346         //*(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
 347         RTW_PUT_BE32(wpsie, WPSOUI);
 348         wpsielen += 4;
 349
 350 #if 0
 351         //      WPS version
 352         //      Type:
 353         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
 354         wpsielen += 2;
 355
 356         //      Length:
 357         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
 358         wpsielen += 2;
 359
 360         //      Value:
 361         wpsie[wpsielen++] = WPS_VERSION_1;      //      Version 1.0
 362 #endif
 363
 364         //      Config Method
 365         //      Type:
 366         //*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
 367         RTW_PUT_BE16(wpsie + wpsielen, WPS_ATTR_CONF_METHOD);
 368         wpsielen += 2;
 369
 370         //      Length:
 371         //*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
 372         RTW_PUT_BE16(wpsie + wpsielen, 0x0002);
 373         wpsielen += 2;
 374
 375         //      Value:
 376         //*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( config_method );
 377         RTW_PUT_BE16(wpsie + wpsielen, config_method);
 378         wpsielen += 2;
 379
 380         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
 381
 382 #ifdef CONFIG_WFD
 383         wfdielen = build_provdisc_resp_wfd_ie(pwdinfo, pframe);
 384         pframe += wfdielen;
 385         pattrib->pktlen += wfdielen;
 386 #endif
 387
 388         pattrib->last_txcmdsz = pattrib->pktlen;
 389
 390         dump_mgntframe(padapter, pmgntframe);
 391
 392         return;
 393
 394 }
 395
 396 static void issue_p2p_presence_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
 397 {
 398         struct xmit_frame                       *pmgntframe;
 399         struct pkt_attrib                       *pattrib;
 400         unsigned char                                   *pframe;
 401         struct rtw_ieee80211_hdr        *pwlanhdr;
 402         unsigned short                          *fctrl;
 403         _adapter *padapter = pwdinfo->padapter;
 404         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 405         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 406         unsigned char category = RTW_WLAN_CATEGORY_P2P;//P2P action frame
 407         u32     p2poui = cpu_to_be32(P2POUI);
 408         u8      oui_subtype = P2P_PRESENCE_RESPONSE;
 409         u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
 410         u8 noa_attr_content[32] = { 0x00 };
 411         u32 p2pielen = 0;
 412
 413         DBG_871X("[%s]\n", __FUNCTION__);
 414
 415         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
 416         {
 417                 return;
 418         }
 419
 420         //update attribute
 421         pattrib = &pmgntframe->attrib;
 422         update_mgntframe_attrib(padapter, pattrib);
 423
 424         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 425
 426         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 427         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 428
 429         fctrl = &(pwlanhdr->frame_ctl);
 430         *(fctrl) = 0;
 431
 432         _rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
 433         _rtw_memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
 434         _rtw_memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
 435
 436         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 437         pmlmeext->mgnt_seq++;
 438         SetFrameSubType(pframe, WIFI_ACTION);
 439
 440         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
 441         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
 442
 443         //Build P2P action frame header
 444         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
 445         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
 446         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
 447         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
 448
 449
 450         //Add P2P IE header
 451         //      P2P OUI
 452         p2pielen = 0;
 453         p2pie[ p2pielen++ ] = 0x50;
 454         p2pie[ p2pielen++ ] = 0x6F;
 455         p2pie[ p2pielen++ ] = 0x9A;
 456         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
 457
 458         //Add Status attribute in P2P IE
 459         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
 460
 461         //Add NoA attribute in P2P IE
 462         noa_attr_content[0] = 0x1;//index
 463         noa_attr_content[1] = 0x0;//CTWindow and OppPS Parameters
 464
 465         //todo: Notice of Absence Descriptor(s)
 466
 467         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_NOA, 2, noa_attr_content);
 468
 469
 470
 471         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &(pattrib->pktlen));
 472
 473
 474         pattrib->last_txcmdsz = pattrib->pktlen;
 475
 476         dump_mgntframe(padapter, pmgntframe);
 477
 478 }
 479
 480 u32 build_beacon_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
 481 {
 482         u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
 483         u16 capability=0;
 484         u32 len=0, p2pielen = 0;
 485
 486
 487         //      P2P OUI
 488         p2pielen = 0;
 489         p2pie[ p2pielen++ ] = 0x50;
 490         p2pie[ p2pielen++ ] = 0x6F;
 491         p2pie[ p2pielen++ ] = 0x9A;
 492         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
 493
 494
 495         //      According to the P2P Specification, the beacon frame should contain 3 P2P attributes
 496         //      1. P2P Capability
 497         //      2. P2P Device ID
 498         //      3. Notice of Absence ( NOA )
 499
 500         //      P2P Capability ATTR
 501         //      Type:
 502         //      Length:
 503         //      Value:
 504         //      Device Capability Bitmap, 1 byte
 505         //      Be able to participate in additional P2P Groups and
 506         //      support the P2P Invitation Procedure
 507         //      Group Capability Bitmap, 1 byte
 508         capability = P2P_DEVCAP_INVITATION_PROC|P2P_DEVCAP_CLIENT_DISCOVERABILITY;
 509         capability |=  ((P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS) << 8);
 510         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
 511                 capability |= (P2P_GRPCAP_GROUP_FORMATION<<8);
 512
 513         capability = cpu_to_le16(capability);
 514
 515         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8*)&capability);
 516
 517
 518         // P2P Device ID ATTR
 519         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_DEVICE_ID, ETH_ALEN, pwdinfo->device_addr);
 520
 521
 522         // Notice of Absence ATTR
 523         //      Type:
 524         //      Length:
 525         //      Value:
 526
 527         //go_add_noa_attr(pwdinfo);
 528
 529
 530         pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
 531
 532
 533         return len;
 534
 535 }
 536
 537 #ifdef CONFIG_WFD
 538 u32 build_beacon_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
 539 {
 540         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
 541         u16 val16=0;
 542         u32 len=0, wfdielen = 0;
 543         _adapter *padapter = pwdinfo->padapter;
 544         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
 545         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
 546
 547         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
 548                 goto exit;
 549
 550         //      WFD OUI
 551         wfdielen = 0;
 552         wfdie[ wfdielen++ ] = 0x50;
 553         wfdie[ wfdielen++ ] = 0x6F;
 554         wfdie[ wfdielen++ ] = 0x9A;
 555         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
 556
 557         //      Commented by Albert 20110812
 558         //      According to the WFD Specification, the beacon frame should contain 4 WFD attributes
 559         //      1. WFD Device Information
 560         //      2. Associated BSSID
 561         //      3. Coupled Sink Information
 562
 563
 564         //      WFD Device Information ATTR
 565         //      Type:
 566         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
 567
 568         //      Length:
 569         //      Note: In the WFD specification, the size of length field is 2.
 570         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
 571         wfdielen += 2;
 572
 573         //      Value1:
 574         //      WFD device information
 575
 576         if ( P2P_ROLE_GO == pwdinfo->role )
 577         {
 578                 if ( is_any_client_associated( pwdinfo->padapter ) )
 579                 {
 580                         //      WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery)
 581                         val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD;
 582                         RTW_PUT_BE16(wfdie + wfdielen, val16);
 583                 }
 584                 else
 585                 {
 586                         //      WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery)
 587                         val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
 588                         RTW_PUT_BE16(wfdie + wfdielen, val16);
 589                 }
 590
 591         }
 592         else
 593         {
 594                 //      WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
 595                 val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
 596                 RTW_PUT_BE16(wfdie + wfdielen, val16);
 597         }
 598
 599         wfdielen += 2;
 600
 601         //      Value2:
 602         //      Session Management Control Port
 603         //      Default TCP port for RTSP messages is 554
 604         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
 605         wfdielen += 2;
 606
 607         //      Value3:
 608         //      WFD Device Maximum Throughput
 609         //      300Mbps is the maximum throughput
 610         RTW_PUT_BE16(wfdie + wfdielen, 300);
 611         wfdielen += 2;
 612
 613         //      Associated BSSID ATTR
 614         //      Type:
 615         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
 616
 617         //      Length:
 618         //      Note: In the WFD specification, the size of length field is 2.
 619         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
 620         wfdielen += 2;
 621
 622         //      Value:
 623         //      Associated BSSID
 624         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
 625         {
 626                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
 627         }
 628         else
 629         {
 630                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
 631         }
 632
 633         wfdielen += ETH_ALEN;
 634
 635         //      Coupled Sink Information ATTR
 636         //      Type:
 637         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
 638
 639         //      Length:
 640         //      Note: In the WFD specification, the size of length field is 2.
 641         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
 642         wfdielen += 2;
 643
 644         //      Value:
 645         //      Coupled Sink Status bitmap
 646         //      Not coupled/available for Coupling
 647         wfdie[ wfdielen++ ] = 0;
 648         //  MAC Addr.
 649         wfdie[ wfdielen++ ] = 0;
 650         wfdie[ wfdielen++ ] = 0;
 651         wfdie[ wfdielen++ ] = 0;
 652         wfdie[ wfdielen++ ] = 0;
 653         wfdie[ wfdielen++ ] = 0;
 654         wfdie[ wfdielen++ ] = 0;
 655
 656         rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
 657
 658 exit:
 659         return len;
 660 }
 661
 662 u32 build_probe_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
 663 {
 664         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
 665         u16 val16=0;
 666         u32 len=0, wfdielen = 0;
 667         _adapter *padapter = pwdinfo->padapter;
 668         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
 669         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
 670
 671         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
 672                 goto exit;
 673
 674         //      WFD OUI
 675         wfdielen = 0;
 676         wfdie[ wfdielen++ ] = 0x50;
 677         wfdie[ wfdielen++ ] = 0x6F;
 678         wfdie[ wfdielen++ ] = 0x9A;
 679         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
 680
 681         //      Commented by Albert 20110812
 682         //      According to the WFD Specification, the probe request frame should contain 4 WFD attributes
 683         //      1. WFD Device Information
 684         //      2. Associated BSSID
 685         //      3. Coupled Sink Information
 686
 687
 688         //      WFD Device Information ATTR
 689         //      Type:
 690         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
 691
 692         //      Length:
 693         //      Note: In the WFD specification, the size of length field is 2.
 694         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
 695         wfdielen += 2;
 696
 697         //      Value1:
 698         //      WFD device information
 699
 700         if ( 1 == pwdinfo->wfd_tdls_enable )
 701         {
 702                 //      WFD primary sink + available for WFD session + WiFi TDLS mode + WSC ( WFD Service Discovery )
 703                 val16 = pwfd_info->wfd_device_type |
 704                                                 WFD_DEVINFO_SESSION_AVAIL |
 705                                                 WFD_DEVINFO_WSD |
 706                                                 WFD_DEVINFO_PC_TDLS;
 707                 RTW_PUT_BE16(wfdie + wfdielen, val16 );
 708         }
 709         else
 710         {
 711                 //      WFD primary sink + available for WFD session + WiFi Direct mode + WSC ( WFD Service Discovery )
 712                 val16 = pwfd_info->wfd_device_type |
 713                                                 WFD_DEVINFO_SESSION_AVAIL |
 714                                                 WFD_DEVINFO_WSD;
 715                 RTW_PUT_BE16(wfdie + wfdielen, val16 );
 716         }
 717
 718         wfdielen += 2;
 719
 720         //      Value2:
 721         //      Session Management Control Port
 722         //      Default TCP port for RTSP messages is 554
 723         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
 724         wfdielen += 2;
 725
 726         //      Value3:
 727         //      WFD Device Maximum Throughput
 728         //      300Mbps is the maximum throughput
 729         RTW_PUT_BE16(wfdie + wfdielen, 300);
 730         wfdielen += 2;
 731
 732         //      Associated BSSID ATTR
 733         //      Type:
 734         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
 735
 736         //      Length:
 737         //      Note: In the WFD specification, the size of length field is 2.
 738         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
 739         wfdielen += 2;
 740
 741         //      Value:
 742         //      Associated BSSID
 743         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
 744         {
 745                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
 746         }
 747         else
 748         {
 749                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
 750         }
 751
 752         wfdielen += ETH_ALEN;
 753
 754         //      Coupled Sink Information ATTR
 755         //      Type:
 756         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
 757
 758         //      Length:
 759         //      Note: In the WFD specification, the size of length field is 2.
 760         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
 761         wfdielen += 2;
 762
 763         //      Value:
 764         //      Coupled Sink Status bitmap
 765         //      Not coupled/available for Coupling
 766         wfdie[ wfdielen++ ] = 0;
 767         //  MAC Addr.
 768         wfdie[ wfdielen++ ] = 0;
 769         wfdie[ wfdielen++ ] = 0;
 770         wfdie[ wfdielen++ ] = 0;
 771         wfdie[ wfdielen++ ] = 0;
 772         wfdie[ wfdielen++ ] = 0;
 773         wfdie[ wfdielen++ ] = 0;
 774
 775         rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
 776
 777 exit:
 778         return len;
 779 }
 780
 781 u32 build_probe_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 tunneled)
 782 {
 783         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
 784         u32 len=0, wfdielen = 0;
 785         _adapter *padapter = pwdinfo->padapter;
 786         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
 787         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
 788
 789         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
 790                 goto exit;
 791
 792         //      WFD OUI
 793         wfdielen = 0;
 794         wfdie[ wfdielen++ ] = 0x50;
 795         wfdie[ wfdielen++ ] = 0x6F;
 796         wfdie[ wfdielen++ ] = 0x9A;
 797         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
 798
 799         //      Commented by Albert 20110812
 800         //      According to the WFD Specification, the probe response frame should contain 4 WFD attributes
 801         //      1. WFD Device Information
 802         //      2. Associated BSSID
 803         //      3. Coupled Sink Information
 804         //      4. WFD Session Information
 805
 806
 807         //      WFD Device Information ATTR
 808         //      Type:
 809         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
 810
 811         //      Length:
 812         //      Note: In the WFD specification, the size of length field is 2.
 813         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
 814         wfdielen += 2;
 815
 816         //      Value1:
 817         //      WFD device information
 818         //      WFD primary sink + available for WFD session + WiFi Direct mode
 819
 820         if (  _TRUE == pwdinfo->session_available )
 821         {
 822                 if ( P2P_ROLE_GO == pwdinfo->role )
 823                 {
 824                         if ( is_any_client_associated( pwdinfo->padapter ) )
 825                         {
 826                                 if ( pwdinfo->wfd_tdls_enable )
 827                                 {
 828                                         //      TDLS mode + WSD ( WFD Service Discovery )
 829                                         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
 830                                 }
 831                                 else
 832                                 {
 833                                         //      WiFi Direct mode + WSD ( WFD Service Discovery )
 834                                         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
 835                                 }
 836                         }
 837                         else
 838                         {
 839                                 if ( pwdinfo->wfd_tdls_enable )
 840                                 {
 841                                         //      available for WFD session + TDLS mode + WSD ( WFD Service Discovery )
 842                                         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
 843                                 }
 844                                 else
 845                                 {
 846                                         //      available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
 847                                         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
 848                                 }
 849                         }
 850                 }
 851                 else
 852                 {
 853                         if ( pwdinfo->wfd_tdls_enable )
 854                         {
 855                                 //      available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
 856                                 RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
 857                         }
 858                         else
 859                         {
 860
 861                                 //      available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
 862                                 RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
 863                         }
 864                 }
 865         }
 866         else
 867         {
 868                 if ( pwdinfo->wfd_tdls_enable )
 869                 {
 870                         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD |WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
 871                 }
 872                 else
 873                 {
 874                         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
 875                 }
 876
 877         }
 878
 879         wfdielen += 2;
 880
 881         //      Value2:
 882         //      Session Management Control Port
 883         //      Default TCP port for RTSP messages is 554
 884         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
 885         wfdielen += 2;
 886
 887         //      Value3:
 888         //      WFD Device Maximum Throughput
 889         //      300Mbps is the maximum throughput
 890         RTW_PUT_BE16(wfdie + wfdielen, 300);
 891         wfdielen += 2;
 892
 893         //      Associated BSSID ATTR
 894         //      Type:
 895         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
 896
 897         //      Length:
 898         //      Note: In the WFD specification, the size of length field is 2.
 899         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
 900         wfdielen += 2;
 901
 902         //      Value:
 903         //      Associated BSSID
 904         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
 905         {
 906                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
 907         }
 908         else
 909         {
 910                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
 911         }
 912
 913         wfdielen += ETH_ALEN;
 914
 915         //      Coupled Sink Information ATTR
 916         //      Type:
 917         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
 918
 919         //      Length:
 920         //      Note: In the WFD specification, the size of length field is 2.
 921         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
 922         wfdielen += 2;
 923
 924         //      Value:
 925         //      Coupled Sink Status bitmap
 926         //      Not coupled/available for Coupling
 927         wfdie[ wfdielen++ ] = 0;
 928         //  MAC Addr.
 929         wfdie[ wfdielen++ ] = 0;
 930         wfdie[ wfdielen++ ] = 0;
 931         wfdie[ wfdielen++ ] = 0;
 932         wfdie[ wfdielen++ ] = 0;
 933         wfdie[ wfdielen++ ] = 0;
 934         wfdie[ wfdielen++ ] = 0;
 935
 936         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
 937         {
 938                 //      WFD Session Information ATTR
 939                 //      Type:
 940                 wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
 941
 942                 //      Length:
 943                 //      Note: In the WFD specification, the size of length field is 2.
 944                 RTW_PUT_BE16(wfdie + wfdielen, 0x0000);
 945                 wfdielen += 2;
 946
 947                 //      Todo: to add the list of WFD device info descriptor in WFD group.
 948
 949         }
 950 #ifdef CONFIG_CONCURRENT_MODE
 951 #ifdef CONFIG_TDLS
 952         if ( ( tunneled == 0 ) && ( padapter->pbuddy_adapter->wdinfo.wfd_tdls_enable == 1 ) )
 953         {
 954                 //      Alternative MAC Address ATTR
 955                 //      Type:
 956                 wfdie[ wfdielen++ ] = WFD_ATTR_ALTER_MAC;
 957
 958                 //      Length:
 959                 //      Note: In the WFD specification, the size of length field is 2.
 960                 RTW_PUT_BE16(wfdie + wfdielen,  ETH_ALEN );
 961                 wfdielen += 2;
 962
 963                 //      Value:
 964                 //      Alternative MAC Address
 965                 _rtw_memcpy(wfdie + wfdielen, adapter_mac_addr(padapter->pbuddy_adapter), ETH_ALEN);
 966                 //      This mac address is used to make the WFD session when TDLS is enable.
 967
 968                 wfdielen += ETH_ALEN;
 969         }
 970 #endif // CONFIG_TDLS
 971 #endif // CONFIG_CONCURRENT_MODE
 972
 973         pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
 974
 975 exit:
 976         return len;
 977 }
 978
 979 u32 build_assoc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
 980 {
 981         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
 982         u16 val16=0;
 983         u32 len=0, wfdielen = 0;
 984         _adapter                                        *padapter = NULL;
 985         struct mlme_priv                        *pmlmepriv = NULL;
 986         struct wifi_display_info                *pwfd_info = NULL;
 987
 988         padapter = pwdinfo->padapter;
 989         pmlmepriv = &padapter->mlmepriv;
 990         pwfd_info = padapter->wdinfo.wfd_info;
 991
 992         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
 993                 goto exit;
 994
 995         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
 996                 goto exit;
 997
 998         /* WFD OUI */
 999         wfdielen = 0;
1000         wfdie[ wfdielen++ ] = 0x50;
1001         wfdie[ wfdielen++ ] = 0x6F;
1002         wfdie[ wfdielen++ ] = 0x9A;
1003         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
1004
1005         //      Commented by Albert 20110812
1006         //      According to the WFD Specification, the probe request frame should contain 4 WFD attributes
1007         //      1. WFD Device Information
1008         //      2. Associated BSSID
1009         //      3. Coupled Sink Information
1010
1011
1012         //      WFD Device Information ATTR
1013         //      Type:
1014         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1015
1016         //      Length:
1017         //      Note: In the WFD specification, the size of length field is 2.
1018         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1019         wfdielen += 2;
1020
1021         //      Value1:
1022         //      WFD device information
1023         //      WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
1024         val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
1025         RTW_PUT_BE16(wfdie + wfdielen, val16);
1026         wfdielen += 2;
1027
1028         //      Value2:
1029         //      Session Management Control Port
1030         //      Default TCP port for RTSP messages is 554
1031         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
1032         wfdielen += 2;
1033
1034         //      Value3:
1035         //      WFD Device Maximum Throughput
1036         //      300Mbps is the maximum throughput
1037         RTW_PUT_BE16(wfdie + wfdielen, 300);
1038         wfdielen += 2;
1039
1040         //      Associated BSSID ATTR
1041         //      Type:
1042         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1043
1044         //      Length:
1045         //      Note: In the WFD specification, the size of length field is 2.
1046         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1047         wfdielen += 2;
1048
1049         //      Value:
1050         //      Associated BSSID
1051         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
1052         {
1053                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
1054         }
1055         else
1056         {
1057                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
1058         }
1059
1060         wfdielen += ETH_ALEN;
1061
1062         //      Coupled Sink Information ATTR
1063         //      Type:
1064         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1065
1066         //      Length:
1067         //      Note: In the WFD specification, the size of length field is 2.
1068         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
1069         wfdielen += 2;
1070
1071         //      Value:
1072         //      Coupled Sink Status bitmap
1073         //      Not coupled/available for Coupling
1074         wfdie[ wfdielen++ ] = 0;
1075         //  MAC Addr.
1076         wfdie[ wfdielen++ ] = 0;
1077         wfdie[ wfdielen++ ] = 0;
1078         wfdie[ wfdielen++ ] = 0;
1079         wfdie[ wfdielen++ ] = 0;
1080         wfdie[ wfdielen++ ] = 0;
1081         wfdie[ wfdielen++ ] = 0;
1082
1083         rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1084
1085 exit:
1086         return len;
1087 }
1088
1089 u32 build_assoc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1090 {
1091         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1092         u32 len=0, wfdielen = 0;
1093         u16 val16=0;
1094         _adapter *padapter = pwdinfo->padapter;
1095         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1096         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1097
1098         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
1099                 goto exit;
1100
1101         //      WFD OUI
1102         wfdielen = 0;
1103         wfdie[ wfdielen++ ] = 0x50;
1104         wfdie[ wfdielen++ ] = 0x6F;
1105         wfdie[ wfdielen++ ] = 0x9A;
1106         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
1107
1108         //      Commented by Albert 20110812
1109         //      According to the WFD Specification, the probe request frame should contain 4 WFD attributes
1110         //      1. WFD Device Information
1111         //      2. Associated BSSID
1112         //      3. Coupled Sink Information
1113
1114
1115         //      WFD Device Information ATTR
1116         //      Type:
1117         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1118
1119         //      Length:
1120         //      Note: In the WFD specification, the size of length field is 2.
1121         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1122         wfdielen += 2;
1123
1124         //      Value1:
1125         //      WFD device information
1126         //      WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
1127         val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
1128         RTW_PUT_BE16(wfdie + wfdielen, val16);
1129         wfdielen += 2;
1130
1131         //      Value2:
1132         //      Session Management Control Port
1133         //      Default TCP port for RTSP messages is 554
1134         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
1135         wfdielen += 2;
1136
1137         //      Value3:
1138         //      WFD Device Maximum Throughput
1139         //      300Mbps is the maximum throughput
1140         RTW_PUT_BE16(wfdie + wfdielen, 300);
1141         wfdielen += 2;
1142
1143         //      Associated BSSID ATTR
1144         //      Type:
1145         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1146
1147         //      Length:
1148         //      Note: In the WFD specification, the size of length field is 2.
1149         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1150         wfdielen += 2;
1151
1152         //      Value:
1153         //      Associated BSSID
1154         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
1155         {
1156                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
1157         }
1158         else
1159         {
1160                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
1161         }
1162
1163         wfdielen += ETH_ALEN;
1164
1165         //      Coupled Sink Information ATTR
1166         //      Type:
1167         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1168
1169         //      Length:
1170         //      Note: In the WFD specification, the size of length field is 2.
1171         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
1172         wfdielen += 2;
1173
1174         //      Value:
1175         //      Coupled Sink Status bitmap
1176         //      Not coupled/available for Coupling
1177         wfdie[ wfdielen++ ] = 0;
1178         //  MAC Addr.
1179         wfdie[ wfdielen++ ] = 0;
1180         wfdie[ wfdielen++ ] = 0;
1181         wfdie[ wfdielen++ ] = 0;
1182         wfdie[ wfdielen++ ] = 0;
1183         wfdie[ wfdielen++ ] = 0;
1184         wfdie[ wfdielen++ ] = 0;
1185
1186         rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1187
1188 exit:
1189         return len;
1190 }
1191
1192 u32 build_nego_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1193 {
1194         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1195         u32 len=0, wfdielen = 0;
1196         u16 val16=0;
1197         _adapter *padapter = pwdinfo->padapter;
1198         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1199         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1200
1201         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
1202                 goto exit;
1203
1204         //      WFD OUI
1205         wfdielen = 0;
1206         wfdie[ wfdielen++ ] = 0x50;
1207         wfdie[ wfdielen++ ] = 0x6F;
1208         wfdie[ wfdielen++ ] = 0x9A;
1209         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
1210
1211         //      Commented by Albert 20110825
1212         //      According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes
1213         //      1. WFD Device Information
1214         //      2. Associated BSSID ( Optional )
1215         //      3. Local IP Adress ( Optional )
1216
1217
1218         //      WFD Device Information ATTR
1219         //      Type:
1220         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1221
1222         //      Length:
1223         //      Note: In the WFD specification, the size of length field is 2.
1224         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1225         wfdielen += 2;
1226
1227         //      Value1:
1228         //      WFD device information
1229         //      WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available
1230         val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL;
1231         RTW_PUT_BE16(wfdie + wfdielen, val16);
1232         wfdielen += 2;
1233
1234         //      Value2:
1235         //      Session Management Control Port
1236         //      Default TCP port for RTSP messages is 554
1237         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
1238         wfdielen += 2;
1239
1240         //      Value3:
1241         //      WFD Device Maximum Throughput
1242         //      300Mbps is the maximum throughput
1243         RTW_PUT_BE16(wfdie + wfdielen, 300);
1244         wfdielen += 2;
1245
1246         //      Associated BSSID ATTR
1247         //      Type:
1248         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1249
1250         //      Length:
1251         //      Note: In the WFD specification, the size of length field is 2.
1252         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1253         wfdielen += 2;
1254
1255         //      Value:
1256         //      Associated BSSID
1257         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
1258         {
1259                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
1260         }
1261         else
1262         {
1263                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
1264         }
1265
1266         wfdielen += ETH_ALEN;
1267
1268         //      Coupled Sink Information ATTR
1269         //      Type:
1270         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1271
1272         //      Length:
1273         //      Note: In the WFD specification, the size of length field is 2.
1274         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
1275         wfdielen += 2;
1276
1277         //      Value:
1278         //      Coupled Sink Status bitmap
1279         //      Not coupled/available for Coupling
1280         wfdie[ wfdielen++ ] = 0;
1281         //  MAC Addr.
1282         wfdie[ wfdielen++ ] = 0;
1283         wfdie[ wfdielen++ ] = 0;
1284         wfdie[ wfdielen++ ] = 0;
1285         wfdie[ wfdielen++ ] = 0;
1286         wfdie[ wfdielen++ ] = 0;
1287         wfdie[ wfdielen++ ] = 0;
1288
1289         rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1290
1291 exit:
1292         return len;
1293 }
1294
1295 u32 build_nego_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1296 {
1297         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1298         u32 len=0, wfdielen = 0;
1299         u16 val16=0;
1300         _adapter *padapter = pwdinfo->padapter;
1301         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1302         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1303
1304         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
1305                 goto exit;
1306
1307         //      WFD OUI
1308         wfdielen = 0;
1309         wfdie[ wfdielen++ ] = 0x50;
1310         wfdie[ wfdielen++ ] = 0x6F;
1311         wfdie[ wfdielen++ ] = 0x9A;
1312         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
1313
1314         //      Commented by Albert 20110825
1315         //      According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes
1316         //      1. WFD Device Information
1317         //      2. Associated BSSID ( Optional )
1318         //      3. Local IP Adress ( Optional )
1319
1320
1321         //      WFD Device Information ATTR
1322         //      Type:
1323         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1324
1325         //      Length:
1326         //      Note: In the WFD specification, the size of length field is 2.
1327         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1328         wfdielen += 2;
1329
1330         //      Value1:
1331         //      WFD device information
1332         //      WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available
1333         val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL;
1334         RTW_PUT_BE16(wfdie + wfdielen, val16);
1335         wfdielen += 2;
1336
1337         //      Value2:
1338         //      Session Management Control Port
1339         //      Default TCP port for RTSP messages is 554
1340         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
1341         wfdielen += 2;
1342
1343         //      Value3:
1344         //      WFD Device Maximum Throughput
1345         //      300Mbps is the maximum throughput
1346         RTW_PUT_BE16(wfdie + wfdielen, 300);
1347         wfdielen += 2;
1348
1349         //      Associated BSSID ATTR
1350         //      Type:
1351         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1352
1353         //      Length:
1354         //      Note: In the WFD specification, the size of length field is 2.
1355         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1356         wfdielen += 2;
1357
1358         //      Value:
1359         //      Associated BSSID
1360         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
1361         {
1362                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
1363         }
1364         else
1365         {
1366                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
1367         }
1368
1369         wfdielen += ETH_ALEN;
1370
1371         //      Coupled Sink Information ATTR
1372         //      Type:
1373         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1374
1375         //      Length:
1376         //      Note: In the WFD specification, the size of length field is 2.
1377         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
1378         wfdielen += 2;
1379
1380         //      Value:
1381         //      Coupled Sink Status bitmap
1382         //      Not coupled/available for Coupling
1383         wfdie[ wfdielen++ ] = 0;
1384         //  MAC Addr.
1385         wfdie[ wfdielen++ ] = 0;
1386         wfdie[ wfdielen++ ] = 0;
1387         wfdie[ wfdielen++ ] = 0;
1388         wfdie[ wfdielen++ ] = 0;
1389         wfdie[ wfdielen++ ] = 0;
1390         wfdie[ wfdielen++ ] = 0;
1391
1392
1393         rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1394
1395 exit:
1396         return len;
1397 }
1398
1399 u32 build_nego_confirm_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1400 {
1401         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1402         u32 len=0, wfdielen = 0;
1403         u16 val16=0;
1404         _adapter *padapter = pwdinfo->padapter;
1405         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1406         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1407
1408         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
1409                 goto exit;
1410
1411         //      WFD OUI
1412         wfdielen = 0;
1413         wfdie[ wfdielen++ ] = 0x50;
1414         wfdie[ wfdielen++ ] = 0x6F;
1415         wfdie[ wfdielen++ ] = 0x9A;
1416         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
1417
1418         //      Commented by Albert 20110825
1419         //      According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes
1420         //      1. WFD Device Information
1421         //      2. Associated BSSID ( Optional )
1422         //      3. Local IP Adress ( Optional )
1423
1424
1425         //      WFD Device Information ATTR
1426         //      Type:
1427         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1428
1429         //      Length:
1430         //      Note: In the WFD specification, the size of length field is 2.
1431         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1432         wfdielen += 2;
1433
1434         //      Value1:
1435         //      WFD device information
1436         //      WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available
1437         val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL;
1438         RTW_PUT_BE16(wfdie + wfdielen, val16);
1439         wfdielen += 2;
1440
1441         //      Value2:
1442         //      Session Management Control Port
1443         //      Default TCP port for RTSP messages is 554
1444         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
1445         wfdielen += 2;
1446
1447         //      Value3:
1448         //      WFD Device Maximum Throughput
1449         //      300Mbps is the maximum throughput
1450         RTW_PUT_BE16(wfdie + wfdielen, 300);
1451         wfdielen += 2;
1452
1453         //      Associated BSSID ATTR
1454         //      Type:
1455         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1456
1457         //      Length:
1458         //      Note: In the WFD specification, the size of length field is 2.
1459         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1460         wfdielen += 2;
1461
1462         //      Value:
1463         //      Associated BSSID
1464         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
1465         {
1466                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
1467         }
1468         else
1469         {
1470                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
1471         }
1472
1473         wfdielen += ETH_ALEN;
1474
1475         //      Coupled Sink Information ATTR
1476         //      Type:
1477         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1478
1479         //      Length:
1480         //      Note: In the WFD specification, the size of length field is 2.
1481         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
1482         wfdielen += 2;
1483
1484         //      Value:
1485         //      Coupled Sink Status bitmap
1486         //      Not coupled/available for Coupling
1487         wfdie[ wfdielen++ ] = 0;
1488         //  MAC Addr.
1489         wfdie[ wfdielen++ ] = 0;
1490         wfdie[ wfdielen++ ] = 0;
1491         wfdie[ wfdielen++ ] = 0;
1492         wfdie[ wfdielen++ ] = 0;
1493         wfdie[ wfdielen++ ] = 0;
1494         wfdie[ wfdielen++ ] = 0;
1495
1496
1497         pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1498
1499 exit:
1500         return len;
1501 }
1502
1503 u32 build_invitation_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1504 {
1505         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1506         u32 len=0, wfdielen = 0;
1507         u16 val16=0;
1508         _adapter *padapter = pwdinfo->padapter;
1509         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1510         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1511
1512         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
1513                 goto exit;
1514
1515         //      WFD OUI
1516         wfdielen = 0;
1517         wfdie[ wfdielen++ ] = 0x50;
1518         wfdie[ wfdielen++ ] = 0x6F;
1519         wfdie[ wfdielen++ ] = 0x9A;
1520         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
1521
1522         //      Commented by Albert 20110825
1523         //      According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes
1524         //      1. WFD Device Information
1525         //      2. Associated BSSID ( Optional )
1526         //      3. Local IP Adress ( Optional )
1527
1528
1529         //      WFD Device Information ATTR
1530         //      Type:
1531         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1532
1533         //      Length:
1534         //      Note: In the WFD specification, the size of length field is 2.
1535         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1536         wfdielen += 2;
1537
1538         //      Value1:
1539         //      WFD device information
1540         //      WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
1541         val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
1542         RTW_PUT_BE16(wfdie + wfdielen, val16);
1543         wfdielen += 2;
1544
1545         //      Value2:
1546         //      Session Management Control Port
1547         //      Default TCP port for RTSP messages is 554
1548         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
1549         wfdielen += 2;
1550
1551         //      Value3:
1552         //      WFD Device Maximum Throughput
1553         //      300Mbps is the maximum throughput
1554         RTW_PUT_BE16(wfdie + wfdielen, 300);
1555         wfdielen += 2;
1556
1557         //      Associated BSSID ATTR
1558         //      Type:
1559         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1560
1561         //      Length:
1562         //      Note: In the WFD specification, the size of length field is 2.
1563         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1564         wfdielen += 2;
1565
1566         //      Value:
1567         //      Associated BSSID
1568         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
1569         {
1570                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
1571         }
1572         else
1573         {
1574                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
1575         }
1576
1577         wfdielen += ETH_ALEN;
1578
1579         //      Coupled Sink Information ATTR
1580         //      Type:
1581         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1582
1583         //      Length:
1584         //      Note: In the WFD specification, the size of length field is 2.
1585         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
1586         wfdielen += 2;
1587
1588         //      Value:
1589         //      Coupled Sink Status bitmap
1590         //      Not coupled/available for Coupling
1591         wfdie[ wfdielen++ ] = 0;
1592         //  MAC Addr.
1593         wfdie[ wfdielen++ ] = 0;
1594         wfdie[ wfdielen++ ] = 0;
1595         wfdie[ wfdielen++ ] = 0;
1596         wfdie[ wfdielen++ ] = 0;
1597         wfdie[ wfdielen++ ] = 0;
1598         wfdie[ wfdielen++ ] = 0;
1599
1600         if ( P2P_ROLE_GO == pwdinfo->role )
1601         {
1602                 //      WFD Session Information ATTR
1603                 //      Type:
1604                 wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
1605
1606                 //      Length:
1607                 //      Note: In the WFD specification, the size of length field is 2.
1608                 RTW_PUT_BE16(wfdie + wfdielen, 0x0000);
1609                 wfdielen += 2;
1610
1611                 //      Todo: to add the list of WFD device info descriptor in WFD group.
1612
1613         }
1614
1615         rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1616
1617 exit:
1618         return len;
1619 }
1620
1621 u32 build_invitation_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1622 {
1623         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1624         u16 val16=0;
1625         u32 len=0, wfdielen = 0;
1626         _adapter *padapter = pwdinfo->padapter;
1627         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1628         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1629
1630         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
1631                 goto exit;
1632
1633         //      WFD OUI
1634         wfdielen = 0;
1635         wfdie[ wfdielen++ ] = 0x50;
1636         wfdie[ wfdielen++ ] = 0x6F;
1637         wfdie[ wfdielen++ ] = 0x9A;
1638         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
1639
1640         //      Commented by Albert 20110825
1641         //      According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes
1642         //      1. WFD Device Information
1643         //      2. Associated BSSID ( Optional )
1644         //      3. Local IP Adress ( Optional )
1645
1646
1647         //      WFD Device Information ATTR
1648         //      Type:
1649         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1650
1651         //      Length:
1652         //      Note: In the WFD specification, the size of length field is 2.
1653         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1654         wfdielen += 2;
1655
1656         //      Value1:
1657         //      WFD device information
1658         //      WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
1659         val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
1660         RTW_PUT_BE16(wfdie + wfdielen, val16);
1661         wfdielen += 2;
1662
1663         //      Value2:
1664         //      Session Management Control Port
1665         //      Default TCP port for RTSP messages is 554
1666         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
1667         wfdielen += 2;
1668
1669         //      Value3:
1670         //      WFD Device Maximum Throughput
1671         //      300Mbps is the maximum throughput
1672         RTW_PUT_BE16(wfdie + wfdielen, 300);
1673         wfdielen += 2;
1674
1675         //      Associated BSSID ATTR
1676         //      Type:
1677         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1678
1679         //      Length:
1680         //      Note: In the WFD specification, the size of length field is 2.
1681         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1682         wfdielen += 2;
1683
1684         //      Value:
1685         //      Associated BSSID
1686         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
1687         {
1688                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
1689         }
1690         else
1691         {
1692                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
1693         }
1694
1695         wfdielen += ETH_ALEN;
1696
1697         //      Coupled Sink Information ATTR
1698         //      Type:
1699         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1700
1701         //      Length:
1702         //      Note: In the WFD specification, the size of length field is 2.
1703         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
1704         wfdielen += 2;
1705
1706         //      Value:
1707         //      Coupled Sink Status bitmap
1708         //      Not coupled/available for Coupling
1709         wfdie[ wfdielen++ ] = 0;
1710         //  MAC Addr.
1711         wfdie[ wfdielen++ ] = 0;
1712         wfdie[ wfdielen++ ] = 0;
1713         wfdie[ wfdielen++ ] = 0;
1714         wfdie[ wfdielen++ ] = 0;
1715         wfdie[ wfdielen++ ] = 0;
1716         wfdie[ wfdielen++ ] = 0;
1717
1718         if ( P2P_ROLE_GO == pwdinfo->role )
1719         {
1720                 //      WFD Session Information ATTR
1721                 //      Type:
1722                 wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
1723
1724                 //      Length:
1725                 //      Note: In the WFD specification, the size of length field is 2.
1726                 RTW_PUT_BE16(wfdie + wfdielen, 0x0000);
1727                 wfdielen += 2;
1728
1729                 //      Todo: to add the list of WFD device info descriptor in WFD group.
1730
1731         }
1732
1733         rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1734
1735 exit:
1736         return len;
1737 }
1738
1739 u32 build_provdisc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1740 {
1741         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1742         u32 len=0, wfdielen = 0;
1743         u16 val16=0;
1744         _adapter *padapter = pwdinfo->padapter;
1745         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1746         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1747
1748         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
1749                 goto exit;
1750
1751         //      WFD OUI
1752         wfdielen = 0;
1753         wfdie[ wfdielen++ ] = 0x50;
1754         wfdie[ wfdielen++ ] = 0x6F;
1755         wfdie[ wfdielen++ ] = 0x9A;
1756         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
1757
1758         //      Commented by Albert 20110825
1759         //      According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes
1760         //      1. WFD Device Information
1761         //      2. Associated BSSID ( Optional )
1762         //      3. Local IP Adress ( Optional )
1763
1764
1765         //      WFD Device Information ATTR
1766         //      Type:
1767         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1768
1769         //      Length:
1770         //      Note: In the WFD specification, the size of length field is 2.
1771         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1772         wfdielen += 2;
1773
1774         //      Value1:
1775         //      WFD device information
1776         //      WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
1777         val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
1778         RTW_PUT_BE16(wfdie + wfdielen, val16);
1779         wfdielen += 2;
1780
1781         //      Value2:
1782         //      Session Management Control Port
1783         //      Default TCP port for RTSP messages is 554
1784         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
1785         wfdielen += 2;
1786
1787         //      Value3:
1788         //      WFD Device Maximum Throughput
1789         //      300Mbps is the maximum throughput
1790         RTW_PUT_BE16(wfdie + wfdielen, 300);
1791         wfdielen += 2;
1792
1793         //      Associated BSSID ATTR
1794         //      Type:
1795         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1796
1797         //      Length:
1798         //      Note: In the WFD specification, the size of length field is 2.
1799         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1800         wfdielen += 2;
1801
1802         //      Value:
1803         //      Associated BSSID
1804         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
1805         {
1806                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
1807         }
1808         else
1809         {
1810                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
1811         }
1812
1813         wfdielen += ETH_ALEN;
1814
1815         //      Coupled Sink Information ATTR
1816         //      Type:
1817         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1818
1819         //      Length:
1820         //      Note: In the WFD specification, the size of length field is 2.
1821         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
1822         wfdielen += 2;
1823
1824         //      Value:
1825         //      Coupled Sink Status bitmap
1826         //      Not coupled/available for Coupling
1827         wfdie[ wfdielen++ ] = 0;
1828         //  MAC Addr.
1829         wfdie[ wfdielen++ ] = 0;
1830         wfdie[ wfdielen++ ] = 0;
1831         wfdie[ wfdielen++ ] = 0;
1832         wfdie[ wfdielen++ ] = 0;
1833         wfdie[ wfdielen++ ] = 0;
1834         wfdie[ wfdielen++ ] = 0;
1835
1836
1837         rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1838
1839 exit:
1840         return len;
1841 }
1842
1843 u32 build_provdisc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1844 {
1845         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1846         u32 len=0, wfdielen = 0;
1847         u16 val16=0;
1848         _adapter *padapter = pwdinfo->padapter;
1849         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1850         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1851
1852         if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
1853                 goto exit;
1854
1855         //      WFD OUI
1856         wfdielen = 0;
1857         wfdie[ wfdielen++ ] = 0x50;
1858         wfdie[ wfdielen++ ] = 0x6F;
1859         wfdie[ wfdielen++ ] = 0x9A;
1860         wfdie[ wfdielen++ ] = 0x0A;     //      WFA WFD v1.0
1861
1862         //      Commented by Albert 20110825
1863         //      According to the WFD Specification, the provision discovery response frame should contain 3 WFD attributes
1864         //      1. WFD Device Information
1865         //      2. Associated BSSID ( Optional )
1866         //      3. Local IP Adress ( Optional )
1867
1868
1869         //      WFD Device Information ATTR
1870         //      Type:
1871         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1872
1873         //      Length:
1874         //      Note: In the WFD specification, the size of length field is 2.
1875         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1876         wfdielen += 2;
1877
1878         //      Value1:
1879         //      WFD device information
1880         //      WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
1881         val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
1882         RTW_PUT_BE16(wfdie + wfdielen, val16);
1883         wfdielen += 2;
1884
1885         //      Value2:
1886         //      Session Management Control Port
1887         //      Default TCP port for RTSP messages is 554
1888         RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
1889         wfdielen += 2;
1890
1891         //      Value3:
1892         //      WFD Device Maximum Throughput
1893         //      300Mbps is the maximum throughput
1894         RTW_PUT_BE16(wfdie + wfdielen, 300);
1895         wfdielen += 2;
1896
1897         //      Associated BSSID ATTR
1898         //      Type:
1899         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1900
1901         //      Length:
1902         //      Note: In the WFD specification, the size of length field is 2.
1903         RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
1904         wfdielen += 2;
1905
1906         //      Value:
1907         //      Associated BSSID
1908         if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
1909         {
1910                 _rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
1911         }
1912         else
1913         {
1914                 _rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
1915         }
1916
1917         wfdielen += ETH_ALEN;
1918
1919         //      Coupled Sink Information ATTR
1920         //      Type:
1921         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1922
1923         //      Length:
1924         //      Note: In the WFD specification, the size of length field is 2.
1925         RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
1926         wfdielen += 2;
1927
1928         //      Value:
1929         //      Coupled Sink Status bitmap
1930         //      Not coupled/available for Coupling
1931         wfdie[ wfdielen++ ] = 0;
1932         //  MAC Addr.
1933         wfdie[ wfdielen++ ] = 0;
1934         wfdie[ wfdielen++ ] = 0;
1935         wfdie[ wfdielen++ ] = 0;
1936         wfdie[ wfdielen++ ] = 0;
1937         wfdie[ wfdielen++ ] = 0;
1938         wfdie[ wfdielen++ ] = 0;
1939
1940         rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1941
1942 exit:
1943         return len;
1944 }
1945 #endif /* CONFIG_WFD */
1946
1947 u32 build_probe_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1948 {
1949         u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
1950         u32 len=0, p2pielen = 0;
1951 #ifdef CONFIG_INTEL_WIDI
1952         struct mlme_priv *pmlmepriv = &(pwdinfo->padapter->mlmepriv);
1953         u8 zero_array_check[L2SDTA_SERVICE_VE_LEN] = { 0x00 };
1954         u8 widi_version = 0, i = 0;
1955
1956         if( _rtw_memcmp( pmlmepriv->sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN ) == _FALSE )
1957         {
1958                 widi_version = 35;
1959         }
1960         else if( pmlmepriv->num_p2p_sdt != 0 )
1961         {
1962                 widi_version = 40;
1963         }
1964 #endif //CONFIG_INTEL_WIDI
1965
1966         //      P2P OUI
1967         p2pielen = 0;
1968         p2pie[ p2pielen++ ] = 0x50;
1969         p2pie[ p2pielen++ ] = 0x6F;
1970         p2pie[ p2pielen++ ] = 0x9A;
1971         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
1972
1973         //      Commented by Albert 20100907
1974         //      According to the P2P Specification, the probe response frame should contain 5 P2P attributes
1975         //      1. P2P Capability
1976         //      2. Extended Listen Timing
1977         //      3. Notice of Absence ( NOA )    ( Only GO needs this )
1978         //      4. Device Info
1979         //      5. Group Info   ( Only GO need this )
1980
1981         //      P2P Capability ATTR
1982         //      Type:
1983         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
1984
1985         //      Length:
1986         //*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
1987         RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
1988         p2pielen += 2;
1989
1990         //      Value:
1991         //      Device Capability Bitmap, 1 byte
1992         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
1993
1994         //      Group Capability Bitmap, 1 byte
1995         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
1996         {
1997                 p2pie[ p2pielen ] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS);
1998
1999                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
2000                         p2pie[ p2pielen ] |= P2P_GRPCAP_GROUP_FORMATION;
2001
2002                 p2pielen++;
2003         }
2004         else if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) )
2005         {
2006                 //      Group Capability Bitmap, 1 byte
2007                 if ( pwdinfo->persistent_supported )
2008                         p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
2009                 else
2010                         p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
2011         }
2012
2013         //      Extended Listen Timing ATTR
2014         //      Type:
2015         p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
2016
2017         //      Length:
2018         //*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
2019         RTW_PUT_LE16(p2pie + p2pielen, 0x0004);
2020         p2pielen += 2;
2021
2022         //      Value:
2023         //      Availability Period
2024         //*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
2025         RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
2026         p2pielen += 2;
2027
2028         //      Availability Interval
2029         //*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
2030         RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
2031         p2pielen += 2;
2032
2033
2034         // Notice of Absence ATTR
2035         //      Type:
2036         //      Length:
2037         //      Value:
2038         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2039         {
2040                 //go_add_noa_attr(pwdinfo);
2041         }
2042
2043         //      Device Info ATTR
2044         //      Type:
2045         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
2046
2047         //      Length:
2048         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
2049         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
2050         //*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
2051 #ifdef CONFIG_INTEL_WIDI
2052         if( widi_version == 35 )
2053         {
2054                 RTW_PUT_LE16(p2pie + p2pielen, 21 + 8 + pwdinfo->device_name_len);
2055         }
2056         else if( widi_version == 40 )
2057         {
2058                 RTW_PUT_LE16(p2pie + p2pielen, 21 + 8 * pmlmepriv->num_p2p_sdt + pwdinfo->device_name_len);
2059         }
2060         else
2061 #endif //CONFIG_INTEL_WIDI
2062         RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
2063         p2pielen += 2;
2064
2065         //      Value:
2066         //      P2P Device Address
2067         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );
2068         p2pielen += ETH_ALEN;
2069
2070         //      Config Method
2071         //      This field should be big endian. Noted by P2P specification.
2072         //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
2073         RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->supported_wps_cm);
2074         p2pielen += 2;
2075
2076 #ifdef CONFIG_INTEL_WIDI
2077         if( widi_version == 40 )
2078         {
2079                 //      Primary Device Type
2080                 //      Category ID
2081                 //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
2082                 RTW_PUT_BE16(p2pie + p2pielen, pmlmepriv->p2p_pdt_cid );
2083                 p2pielen += 2;
2084
2085                 //      OUI
2086                 //*(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
2087                 RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
2088                 p2pielen += 4;
2089
2090                 //      Sub Category ID
2091                 //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
2092                 RTW_PUT_BE16(p2pie + p2pielen, pmlmepriv->p2p_pdt_scid);
2093                 p2pielen += 2;
2094         }
2095         else
2096 #endif //CONFIG_INTEL_WIDI
2097         {
2098                 //      Primary Device Type
2099                 //      Category ID
2100                 //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
2101                 RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA);
2102                 p2pielen += 2;
2103
2104                 //      OUI
2105                 //*(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
2106                 RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
2107                 p2pielen += 4;
2108
2109                 //      Sub Category ID
2110                 //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
2111                 RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER);
2112                 p2pielen += 2;
2113         }
2114
2115         //      Number of Secondary Device Types
2116 #ifdef CONFIG_INTEL_WIDI
2117         if( widi_version == 35 )
2118         {
2119                 p2pie[ p2pielen++ ] = 0x01;
2120
2121                 RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_DISPLAYS);
2122                 p2pielen += 2;
2123
2124                 RTW_PUT_BE32(p2pie + p2pielen, INTEL_DEV_TYPE_OUI);
2125                 p2pielen += 4;
2126
2127                 RTW_PUT_BE16(p2pie + p2pielen, P2P_SCID_WIDI_CONSUMER_SINK);
2128                 p2pielen += 2;
2129         }
2130         else if( widi_version == 40 )
2131         {
2132                 p2pie[ p2pielen++ ] = pmlmepriv->num_p2p_sdt;
2133                 for( ; i < pmlmepriv->num_p2p_sdt; i++ )
2134                 {
2135                         RTW_PUT_BE16(p2pie + p2pielen, pmlmepriv->p2p_sdt_cid[i]);
2136                         p2pielen += 2;
2137
2138                         RTW_PUT_BE32(p2pie + p2pielen, INTEL_DEV_TYPE_OUI);
2139                         p2pielen += 4;
2140
2141                         RTW_PUT_BE16(p2pie + p2pielen, pmlmepriv->p2p_sdt_scid[i]);
2142                         p2pielen += 2;
2143                 }
2144         }
2145         else
2146 #endif //CONFIG_INTEL_WIDI
2147         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
2148
2149         //      Device Name
2150         //      Type:
2151         //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
2152         RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
2153         p2pielen += 2;
2154
2155         //      Length:
2156         //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
2157         RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
2158         p2pielen += 2;
2159
2160         //      Value:
2161         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
2162         p2pielen += pwdinfo->device_name_len;
2163
2164         // Group Info ATTR
2165         //      Type:
2166         //      Length:
2167         //      Value:
2168         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2169         {
2170                 p2pielen += go_add_group_info_attr(pwdinfo, p2pie + p2pielen);
2171         }
2172
2173
2174         pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
2175
2176
2177         return len;
2178
2179 }
2180
2181 u32 build_prov_disc_request_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8* pssid, u8 ussidlen, u8* pdev_raddr )
2182 {
2183         u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
2184         u32 len=0, p2pielen = 0;
2185
2186         //      P2P OUI
2187         p2pielen = 0;
2188         p2pie[ p2pielen++ ] = 0x50;
2189         p2pie[ p2pielen++ ] = 0x6F;
2190         p2pie[ p2pielen++ ] = 0x9A;
2191         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
2192
2193         //      Commented by Albert 20110301
2194         //      According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes
2195         //      1. P2P Capability
2196         //      2. Device Info
2197         //      3. Group ID ( When joining an operating P2P Group )
2198
2199         //      P2P Capability ATTR
2200         //      Type:
2201         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
2202
2203         //      Length:
2204         //*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
2205         RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
2206         p2pielen += 2;
2207
2208         //      Value:
2209         //      Device Capability Bitmap, 1 byte
2210         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
2211
2212         //      Group Capability Bitmap, 1 byte
2213         if ( pwdinfo->persistent_supported )
2214                 p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
2215         else
2216                 p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
2217
2218
2219         //      Device Info ATTR
2220         //      Type:
2221         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
2222
2223         //      Length:
2224         //      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
2225         //      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
2226         //*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
2227         RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
2228         p2pielen += 2;
2229
2230         //      Value:
2231         //      P2P Device Address
2232         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );
2233         p2pielen += ETH_ALEN;
2234
2235         //      Config Method
2236         //      This field should be big endian. Noted by P2P specification.
2237         if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC )
2238         {
2239                 //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_PBC );
2240                 RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_PBC);
2241         }
2242         else
2243         {
2244                 //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
2245                 RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_DISPLAY);
2246         }
2247
2248         p2pielen += 2;
2249
2250         //      Primary Device Type
2251         //      Category ID
2252         //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
2253         RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA);
2254         p2pielen += 2;
2255
2256         //      OUI
2257         //*(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
2258         RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
2259         p2pielen += 4;
2260
2261         //      Sub Category ID
2262         //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
2263         RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER);
2264         p2pielen += 2;
2265
2266         //      Number of Secondary Device Types
2267         p2pie[ p2pielen++ ] = 0x00;     //      No Secondary Device Type List
2268
2269         //      Device Name
2270         //      Type:
2271         //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
2272         RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
2273         p2pielen += 2;
2274
2275         //      Length:
2276         //*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
2277         RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
2278         p2pielen += 2;
2279
2280         //      Value:
2281         _rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
2282         p2pielen += pwdinfo->device_name_len;
2283
2284         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
2285         {
2286                 //      Added by Albert 2011/05/19
2287                 //      In this case, the pdev_raddr is the device address of the group owner.
2288
2289                 //      P2P Group ID ATTR
2290                 //      Type:
2291                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
2292
2293                 //      Length:
2294                 //*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + ussidlen );
2295                 RTW_PUT_LE16(p2pie + p2pielen, ETH_ALEN + ussidlen);
2296                 p2pielen += 2;
2297
2298                 //      Value:
2299                 _rtw_memcpy( p2pie + p2pielen, pdev_raddr, ETH_ALEN );
2300                 p2pielen += ETH_ALEN;
2301
2302                 _rtw_memcpy( p2pie + p2pielen, pssid, ussidlen );
2303                 p2pielen += ussidlen;
2304
2305         }
2306
2307         pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
2308
2309
2310         return len;
2311
2312 }
2313
2314
2315 u32 build_assoc_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 status_code)
2316 {
2317         u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
2318         u32 len=0, p2pielen = 0;
2319
2320         //      P2P OUI
2321         p2pielen = 0;
2322         p2pie[ p2pielen++ ] = 0x50;
2323         p2pie[ p2pielen++ ] = 0x6F;
2324         p2pie[ p2pielen++ ] = 0x9A;
2325         p2pie[ p2pielen++ ] = 0x09;     //      WFA P2P v1.0
2326
2327         // According to the P2P Specification, the Association response frame should contain 2 P2P attributes
2328         //      1. Status
2329         //      2. Extended Listen Timing (optional)
2330
2331
2332         //      Status ATTR
2333         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status_code);
2334
2335
2336         // Extended Listen Timing ATTR
2337         //      Type:
2338         //      Length:
2339         //      Value:
2340
2341
2342         pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
2343
2344         return len;
2345
2346 }
2347
2348 u32 build_deauth_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
2349 {
2350         u32 len=0;
2351
2352         return len;
2353 }
2354
2355 u32 process_probe_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
2356 {
2357         u8 *p;
2358         u32 ret=_FALSE;
2359         u8 *p2pie;
2360         u32     p2pielen = 0;
2361         int ssid_len=0, rate_cnt = 0;
2362
2363         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt,
2364                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
2365
2366         if ( rate_cnt <= 4 )
2367         {
2368                 int i, g_rate =0;
2369
2370                 for( i = 0; i < rate_cnt; i++ )
2371                 {
2372                         if ( ( ( *( p + 2 + i ) & 0xff ) != 0x02 ) &&
2373                                 ( ( *( p + 2 + i ) & 0xff ) != 0x04 ) &&
2374                                 ( ( *( p + 2 + i ) & 0xff ) != 0x0B ) &&
2375                                 ( ( *( p + 2 + i ) & 0xff ) != 0x16 ) )
2376                         {
2377                                 g_rate = 1;
2378                         }
2379                 }
2380
2381                 if ( g_rate == 0 )
2382                 {
2383                         //      There is no OFDM rate included in SupportedRates IE of this probe request frame
2384                         //      The driver should response this probe request.
2385                         return ret;
2386                 }
2387         }
2388         else
2389         {
2390                 //      rate_cnt > 4 means the SupportRates IE contains the OFDM rate because the count of CCK rates are 4.
2391                 //      We should proceed the following check for this probe request.
2392         }
2393
2394         //      Added comments by Albert 20100906
2395         //      There are several items we should check here.
2396         //      1. This probe request frame must contain the P2P IE. (Done)
2397         //      2. This probe request frame must contain the wildcard SSID. (Done)
2398         //      3. Wildcard BSSID. (Todo)
2399         //      4. Destination Address. ( Done in mgt_dispatcher function )
2400         //      5. Requested Device Type in WSC IE. (Todo)
2401         //      6. Device ID attribute in P2P IE. (Todo)
2402
2403         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len,
2404                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
2405
2406         ssid_len &= 0xff;       //      Just last 1 byte is valid for ssid len of the probe request
2407         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2408         {
2409                 if((p2pie=rtw_get_p2p_ie( pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_ , len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_ , NULL, &p2pielen)))
2410                 {
2411                         if ( (p != NULL) && _rtw_memcmp( ( void * ) ( p+2 ), ( void * ) pwdinfo->p2p_wildcard_ssid , 7 ))
2412                         {
2413                                 //todo:
2414                                 //Check Requested Device Type attributes in WSC IE.
2415                                 //Check Device ID attribute in P2P IE
2416
2417                                 ret = _TRUE;
2418                         }
2419                         else if ( (p != NULL) && ( ssid_len == 0 ) )
2420                         {
2421                                 ret = _TRUE;
2422                         }
2423                 }
2424                 else
2425                 {
2426                         //non -p2p device
2427                 }
2428
2429         }
2430
2431
2432         return ret;
2433
2434 }
2435
2436 u32 process_assoc_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len, struct sta_info *psta)
2437 {
2438         u8 status_code = P2P_STATUS_SUCCESS;
2439         u8 *pbuf, *pattr_content=NULL;
2440         u32 attr_contentlen = 0;
2441         u16 cap_attr=0;
2442         unsigned short  frame_type, ie_offset=0;
2443         u8 * ies;
2444         u32 ies_len;
2445         u8 * p2p_ie;
2446         u32     p2p_ielen = 0;
2447
2448         if(!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2449                 return P2P_STATUS_FAIL_REQUEST_UNABLE;
2450
2451         frame_type = GetFrameSubType(pframe);
2452         if (frame_type == WIFI_ASSOCREQ)
2453         {
2454                 ie_offset = _ASOCREQ_IE_OFFSET_;
2455         }
2456         else // WIFI_REASSOCREQ
2457         {
2458                 ie_offset = _REASOCREQ_IE_OFFSET_;
2459         }
2460
2461         ies = pframe + WLAN_HDR_A3_LEN + ie_offset;
2462         ies_len = len - WLAN_HDR_A3_LEN - ie_offset;
2463
2464         p2p_ie = rtw_get_p2p_ie(ies , ies_len , NULL, &p2p_ielen);
2465
2466         if ( !p2p_ie )
2467         {
2468                 DBG_8192C( "[%s] P2P IE not Found!!\n", __FUNCTION__ );
2469                 status_code =  P2P_STATUS_FAIL_INVALID_PARAM;
2470         }
2471         else
2472         {
2473                 DBG_8192C( "[%s] P2P IE Found!!\n", __FUNCTION__ );
2474         }
2475
2476         while ( p2p_ie )
2477         {
2478                 //Check P2P Capability ATTR
2479                 if( rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*) &attr_contentlen) )
2480                 {
2481                         DBG_8192C( "[%s] Got P2P Capability Attr!!\n", __FUNCTION__ );
2482                         cap_attr = le16_to_cpu(cap_attr);
2483                         psta->dev_cap = cap_attr&0xff;
2484                 }
2485
2486                 //Check Extended Listen Timing ATTR
2487
2488
2489                 //Check P2P Device Info ATTR
2490                 if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint*)&attr_contentlen))
2491                 {
2492                         DBG_8192C( "[%s] Got P2P DEVICE INFO Attr!!\n", __FUNCTION__ );
2493                         pattr_content = pbuf = rtw_zmalloc(attr_contentlen);
2494                         if(pattr_content)
2495                         {
2496                                 u8 num_of_secdev_type;
2497                                 u16 dev_name_len;
2498
2499
2500                                 rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO , pattr_content, (uint*)&attr_contentlen);
2501
2502                                 _rtw_memcpy(psta->dev_addr,     pattr_content, ETH_ALEN);//P2P Device Address
2503
2504                                 pattr_content += ETH_ALEN;
2505
2506                                 _rtw_memcpy(&psta->config_methods, pattr_content, 2);//Config Methods
2507                                 psta->config_methods = be16_to_cpu(psta->config_methods);
2508
2509                                 pattr_content += 2;
2510
2511                                 _rtw_memcpy(psta->primary_dev_type, pattr_content, 8);
2512
2513                                 pattr_content += 8;
2514
2515                                 num_of_secdev_type = *pattr_content;
2516                                 pattr_content += 1;
2517
2518                                 if(num_of_secdev_type==0)
2519                                 {
2520                                         psta->num_of_secdev_type = 0;
2521                                 }
2522                                 else
2523                                 {
2524                                         u32 len;
2525
2526                                         psta->num_of_secdev_type = num_of_secdev_type;
2527
2528                                         len = (sizeof(psta->secdev_types_list)<(num_of_secdev_type*8)) ? (sizeof(psta->secdev_types_list)) : (num_of_secdev_type*8);
2529
2530                                         _rtw_memcpy(psta->secdev_types_list, pattr_content, len);
2531
2532                                         pattr_content += (num_of_secdev_type*8);
2533                                 }
2534
2535
2536                                 //dev_name_len = attr_contentlen - ETH_ALEN - 2 - 8 - 1 - (num_of_secdev_type*8);
2537                                 psta->dev_name_len=0;
2538                                 if(WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(u16*)pattr_content))
2539                                 {
2540                                         dev_name_len = be16_to_cpu(*(u16*)(pattr_content+2));
2541
2542                                         psta->dev_name_len = (sizeof(psta->dev_name)<dev_name_len) ? sizeof(psta->dev_name):dev_name_len;
2543
2544                                         _rtw_memcpy(psta->dev_name, pattr_content+4, psta->dev_name_len);
2545                                 }
2546
2547                                 rtw_mfree(pbuf, attr_contentlen);
2548
2549                         }
2550
2551                 }
2552
2553                 //Get the next P2P IE
2554                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
2555
2556         }
2557
2558         return status_code;
2559
2560 }
2561
2562 u32 process_p2p_devdisc_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
2563 {
2564         u8 *frame_body;
2565         u8 status, dialogToken;
2566         struct sta_info *psta = NULL;
2567         _adapter *padapter = pwdinfo->padapter;
2568         struct sta_priv *pstapriv = &padapter->stapriv;
2569         u8 *p2p_ie;
2570         u32     p2p_ielen = 0;
2571
2572         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
2573
2574         dialogToken = frame_body[7];
2575         status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
2576
2577         if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
2578         {
2579                 u8 groupid[ 38 ] = { 0x00 };
2580                 u8 dev_addr[ETH_ALEN] = { 0x00 };
2581                 u32     attr_contentlen = 0;
2582
2583                 if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen))
2584                 {
2585                         if(_rtw_memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) &&
2586                                 _rtw_memcmp(pwdinfo->p2p_group_ssid, groupid+ETH_ALEN, pwdinfo->p2p_group_ssid_len))
2587                         {
2588                                 attr_contentlen=0;
2589                                 if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_ID, dev_addr, &attr_contentlen))
2590                                 {
2591                                         _irqL irqL;
2592                                         _list   *phead, *plist;
2593
2594                                         _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2595                                         phead = &pstapriv->asoc_list;
2596                                         plist = get_next(phead);
2597
2598                                         //look up sta asoc_queue
2599                                         while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
2600                                         {
2601                                                 psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
2602
2603                                                 plist = get_next(plist);
2604
2605                                                 if(psta->is_p2p_device && (psta->dev_cap&P2P_DEVCAP_CLIENT_DISCOVERABILITY) &&
2606                                                         _rtw_memcmp(psta->dev_addr, dev_addr, ETH_ALEN))
2607                                                 {
2608
2609                                                         //_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2610                                                         //issue GO Discoverability Request
2611                                                         issue_group_disc_req(pwdinfo, psta->hwaddr);
2612                                                         //_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2613
2614                                                         status = P2P_STATUS_SUCCESS;
2615
2616                                                         break;
2617                                                 }
2618                                                 else
2619                                                 {
2620                                                         status = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
2621                                                 }
2622
2623                                         }
2624                                         _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
2625
2626                                 }
2627                                 else
2628                                 {
2629                                         status = P2P_STATUS_FAIL_INVALID_PARAM;
2630                                 }
2631
2632                         }
2633                         else
2634                         {
2635                                 status = P2P_STATUS_FAIL_INVALID_PARAM;
2636                         }
2637
2638                 }
2639
2640         }
2641
2642
2643         //issue Device Discoverability Response
2644         issue_p2p_devdisc_resp(pwdinfo, GetAddr2Ptr(pframe), status, dialogToken);
2645
2646
2647         return (status==P2P_STATUS_SUCCESS) ? _TRUE:_FALSE;
2648
2649 }
2650
2651 u32 process_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
2652 {
2653         return _TRUE;
2654 }
2655
2656 u8 process_p2p_provdisc_req(struct wifidirect_info *pwdinfo,  u8 *pframe, uint len )
2657 {
2658         u8 *frame_body;
2659         u8 *wpsie;
2660         uint    wps_ielen = 0, attr_contentlen = 0;
2661         u16     uconfig_method = 0;
2662
2663
2664         frame_body = (pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
2665
2666         if ( (wpsie=rtw_get_wps_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen)) )
2667         {
2668                 if ( rtw_get_wps_attr_content( wpsie, wps_ielen, WPS_ATTR_CONF_METHOD , ( u8* ) &uconfig_method, &attr_contentlen) )
2669                 {
2670                         uconfig_method = be16_to_cpu( uconfig_method );
2671                         switch( uconfig_method )
2672                         {
2673                                 case WPS_CM_DISPLYA:
2674                                 {
2675                                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3 );
2676                                         break;
2677                                 }
2678                                 case WPS_CM_LABEL:
2679                                 {
2680                                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "lab", 3 );
2681                                         break;
2682                                 }
2683                                 case WPS_CM_PUSH_BUTTON:
2684                                 {
2685                                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3 );
2686                                         break;
2687                                 }
2688                                 case WPS_CM_KEYPAD:
2689                                 {
2690                                         _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3 );
2691                                         break;
2692                                 }
2693                         }
2694                         issue_p2p_provision_resp( pwdinfo, GetAddr2Ptr(pframe), frame_body, uconfig_method);
2695                 }
2696         }
2697         DBG_871X( "[%s] config method = %s\n", __FUNCTION__, pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req );
2698         return _TRUE;
2699
2700 }
2701
2702 u8 process_p2p_provdisc_resp(struct wifidirect_info *pwdinfo,  u8 *pframe)
2703 {
2704
2705         return _TRUE;
2706 }
2707
2708 u8 rtw_p2p_get_peer_ch_list(struct wifidirect_info *pwdinfo, u8 *ch_content, u8 ch_cnt, u8 *peer_ch_list)
2709 {
2710         u8 i = 0, j = 0;
2711         u8 temp = 0;
2712         u8 ch_no = 0;
2713         ch_content += 3;
2714         ch_cnt -= 3;
2715
2716         while( ch_cnt > 0)
2717         {
2718                 ch_content += 1;
2719                 ch_cnt -= 1;
2720                 temp = *ch_content;
2721                 for( i = 0 ; i < temp ; i++, j++ )
2722                 {
2723                         peer_ch_list[j] = *( ch_content + 1 + i );
2724                 }
2725                 ch_content += (temp + 1);
2726                 ch_cnt -= (temp + 1);
2727                 ch_no += temp ;
2728         }
2729
2730         return ch_no;
2731 }
2732
2733 u8 rtw_p2p_check_peer_oper_ch(struct mlme_ext_priv *pmlmeext, u8 ch)
2734 {
2735         u8 i = 0;
2736
2737         for( i = 0; i < pmlmeext->max_chan_nums; i++ )
2738         {
2739                 if ( pmlmeext->channel_set[ i ].ChannelNum == ch )
2740                 {
2741                         return _SUCCESS;
2742                 }
2743         }
2744
2745         return _FAIL;
2746 }
2747
2748 u8 rtw_p2p_ch_inclusion(struct mlme_ext_priv *pmlmeext, u8 *peer_ch_list, u8 peer_ch_num, u8 *ch_list_inclusioned)
2749 {
2750         int     i = 0, j = 0, temp = 0;
2751         u8 ch_no = 0;
2752
2753         for( i = 0; i < peer_ch_num; i++ )
2754         {
2755                 for( j = temp; j < pmlmeext->max_chan_nums; j++ )
2756                 {
2757                         if( *( peer_ch_list + i ) == pmlmeext->channel_set[ j ].ChannelNum )
2758                         {
2759                                 ch_list_inclusioned[ ch_no++ ] = *( peer_ch_list + i );
2760                                 temp = j;
2761                                 break;
2762                         }
2763                 }
2764         }
2765
2766         return ch_no;
2767 }
2768
2769 u8 process_p2p_group_negotation_req( struct wifidirect_info *pwdinfo, u8 *pframe, uint len )
2770 {
2771         _adapter *padapter = pwdinfo->padapter;
2772         u8      result = P2P_STATUS_SUCCESS;
2773         u32     p2p_ielen = 0, wps_ielen = 0;
2774         u8 * ies;
2775         u32 ies_len;
2776         u8 *p2p_ie;
2777         u8 *wpsie;
2778         u16             wps_devicepassword_id = 0x0000;
2779         uint    wps_devicepassword_id_len = 0;
2780 #ifdef CONFIG_WFD
2781 #ifdef CONFIG_TDLS
2782         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
2783 #endif // CONFIG_TDLS
2784 #endif // CONFIG_WFD
2785 #ifdef CONFIG_CONCURRENT_MODE
2786         _adapter                                *pbuddy_adapter = pwdinfo->padapter->pbuddy_adapter;
2787         struct wifidirect_info  *pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
2788         struct mlme_priv                *pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
2789         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
2790 #endif
2791
2792         if ( (wpsie=rtw_get_wps_ie( pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen)) )
2793         {
2794                 //      Commented by Kurt 20120113
2795                 //      If some device wants to do p2p handshake without sending prov_disc_req
2796                 //      We have to get peer_req_cm from here.
2797                 if(_rtw_memcmp( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3) )
2798                 {
2799                         rtw_get_wps_attr_content( wpsie, wps_ielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len);
2800                         wps_devicepassword_id = be16_to_cpu( wps_devicepassword_id );
2801
2802                         if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
2803                         {
2804                                 _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3 );
2805                         }
2806                         else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
2807                         {
2808                                 _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3 );
2809                         }
2810                         else
2811                         {
2812                                 _rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3 );
2813                         }
2814                 }
2815         }
2816         else
2817         {
2818                 DBG_871X( "[%s] WPS IE not Found!!\n", __FUNCTION__ );
2819                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
2820                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2821                 return( result );
2822         }
2823
2824         ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
2825         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
2826
2827         p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
2828
2829         if ( !p2p_ie )
2830         {
2831                 DBG_871X( "[%s] P2P IE not Found!!\n", __FUNCTION__ );
2832                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
2833                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2834         }
2835
2836         while ( p2p_ie )
2837         {
2838                 u8      attr_content = 0x00;
2839                 u32     attr_contentlen = 0;
2840                 u8      ch_content[100] = { 0x00 };
2841                 uint    ch_cnt = 0;
2842                 u8      peer_ch_list[100] = { 0x00 };
2843                 u8      peer_ch_num = 0;
2844                 u8      ch_list_inclusioned[100] = { 0x00 };
2845                 u8      ch_num_inclusioned = 0;
2846                 u16     cap_attr;
2847                 u8 listen_ch_attr[5] = { 0x00 };
2848
2849                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING);
2850
2851                 //Check P2P Capability ATTR
2852                 if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen) )
2853                 {
2854                         cap_attr = le16_to_cpu(cap_attr);
2855
2856 #if defined(CONFIG_WFD) && defined(CONFIG_TDLS)
2857                         if(!(cap_attr & P2P_GRPCAP_INTRABSS) )
2858                                 ptdlsinfo->ap_prohibited = _TRUE;
2859 #endif //defined(CONFIG_WFD) && defined(CONFIG_TDLS)
2860                 }
2861
2862                 if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen) )
2863                 {
2864                         DBG_871X( "[%s] GO Intent = %d, tie = %d\n", __FUNCTION__, attr_content >> 1, attr_content & 0x01 );
2865                         pwdinfo->peer_intent = attr_content;    //      include both intent and tie breaker values.
2866
2867                         if ( pwdinfo->intent == ( pwdinfo->peer_intent >> 1 ) )
2868                         {
2869                                 //      Try to match the tie breaker value
2870                                 if ( pwdinfo->intent == P2P_MAX_INTENT )
2871                                 {
2872                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
2873                                         result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
2874                                 }
2875                                 else
2876                                 {
2877                                         if ( attr_content & 0x01 )
2878                                         {
2879                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
2880                                         }
2881                                         else
2882                                         {
2883                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
2884                                         }
2885                                 }
2886                         }
2887                         else if ( pwdinfo->intent > ( pwdinfo->peer_intent >> 1 ) )
2888                         {
2889                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
2890                         }
2891                         else
2892                         {
2893                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
2894                         }
2895
2896                         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2897                         {
2898                                 //      Store the group id information.
2899                                 _rtw_memcpy( pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN );
2900                                 _rtw_memcpy( pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
2901                         }
2902                 }
2903
2904                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_LISTEN_CH, (u8 *)listen_ch_attr, (uint *) &attr_contentlen) && attr_contentlen == 5)
2905                         pwdinfo->nego_req_info.peer_ch = listen_ch_attr[4];
2906
2907                 DBG_871X(FUNC_ADPT_FMT" listen channel :%u\n", FUNC_ADPT_ARG(padapter), pwdinfo->nego_req_info.peer_ch);
2908
2909                 attr_contentlen = 0;
2910                 if ( rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen ) )
2911                 {
2912                         if ( attr_contentlen != ETH_ALEN )
2913                         {
2914                                 _rtw_memset( pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN );
2915                         }
2916                 }
2917
2918                 if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, ch_content, &ch_cnt) )
2919                 {
2920                         peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, ch_content, ch_cnt, peer_ch_list);
2921                         ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
2922
2923                         if( ch_num_inclusioned == 0)
2924                         {
2925                                 DBG_871X( "[%s] No common channel in channel list!\n", __FUNCTION__ );
2926                                 result = P2P_STATUS_FAIL_NO_COMMON_CH;
2927                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2928                                 break;
2929                         }
2930
2931                         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2932                         {
2933                                 if ( !rtw_p2p_is_channel_list_ok( pwdinfo->operating_channel,
2934                                                                                                 ch_list_inclusioned, ch_num_inclusioned) )
2935                                 {
2936 #ifdef CONFIG_CONCURRENT_MODE
2937                                         if (check_buddy_fwstate(padapter, _FW_LINKED)
2938                                                 && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
2939                                         {
2940                                                 DBG_871X( "[%s] desired channel NOT Found!\n", __FUNCTION__ );
2941                                                 result = P2P_STATUS_FAIL_NO_COMMON_CH;
2942                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2943                                                 break;
2944                                         }
2945                                         else
2946 #endif //CONFIG_CONCURRENT_MODE
2947                                         {
2948                                                 u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
2949                                                 attr_contentlen = 0;
2950
2951                                                 if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen) )
2952                                                 {
2953                                                         peer_operating_ch = operatingch_info[4];
2954                                                 }
2955
2956                                                 if ( rtw_p2p_is_channel_list_ok( peer_operating_ch,
2957                                                                                                                 ch_list_inclusioned, ch_num_inclusioned) )
2958                                                 {
2959                                                         /**
2960                                                          *      Change our operating channel as peer's for compatibility.
2961                                                          */
2962                                                         pwdinfo->operating_channel = peer_operating_ch;
2963                                                         DBG_871X( "[%s] Change op ch to %02x as peer's\n", __FUNCTION__, pwdinfo->operating_channel);
2964                                                 }
2965                                                 else
2966                                                 {
2967                                                         // Take first channel of ch_list_inclusioned as operating channel
2968                                                         pwdinfo->operating_channel = ch_list_inclusioned[0];
2969                                                         DBG_871X( "[%s] Change op ch to %02x\n", __FUNCTION__, pwdinfo->operating_channel);
2970                                                 }
2971                                         }
2972
2973                                 }
2974                         }
2975                 }
2976
2977                 //Get the next P2P IE
2978                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
2979         }
2980
2981         if (pwdinfo->ui_got_wps_info == P2P_NO_WPSINFO) {
2982                 result = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
2983                 rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_INFOR_NOREADY);
2984                 return result;
2985         }
2986
2987         #ifdef CONFIG_WFD
2988         rtw_process_wfd_ies(padapter, pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, __func__);
2989         #endif
2990
2991         return( result );
2992 }
2993
2994 u8 process_p2p_group_negotation_resp( struct wifidirect_info *pwdinfo, u8 *pframe, uint len )
2995 {
2996         _adapter *padapter = pwdinfo->padapter;
2997         u8      result = P2P_STATUS_SUCCESS;
2998         u32     p2p_ielen, wps_ielen;
2999         u8 * ies;
3000         u32 ies_len;
3001         u8 * p2p_ie;
3002 #ifdef CONFIG_WFD
3003 #ifdef CONFIG_TDLS
3004         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
3005 #endif // CONFIG_TDLS
3006 #endif // CONFIG_WFD
3007
3008         ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
3009         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
3010
3011         //      Be able to know which one is the P2P GO and which one is P2P client.
3012
3013         if ( rtw_get_wps_ie( ies, ies_len, NULL, &wps_ielen) )
3014         {
3015
3016         }
3017         else
3018         {
3019                 DBG_871X( "[%s] WPS IE not Found!!\n", __FUNCTION__ );
3020                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
3021                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
3022         }
3023
3024         p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
3025         if ( !p2p_ie )
3026         {
3027                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
3028                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
3029                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
3030         }
3031         else
3032         {
3033
3034                 u8      attr_content = 0x00;
3035                 u32     attr_contentlen = 0;
3036                 u8      operatingch_info[5] = { 0x00 };
3037                 uint    ch_cnt = 0;
3038                 u8      ch_content[100] = { 0x00 };
3039                 u8      groupid[ 38 ];
3040                 u16     cap_attr;
3041                 u8      peer_ch_list[100] = { 0x00 };
3042                 u8      peer_ch_num = 0;
3043                 u8      ch_list_inclusioned[100] = { 0x00 };
3044                 u8      ch_num_inclusioned = 0;
3045
3046                 while ( p2p_ie )        //      Found the P2P IE.
3047                 {
3048
3049                         //Check P2P Capability ATTR
3050                         if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen) )
3051                         {
3052                                 cap_attr = le16_to_cpu(cap_attr);
3053 #ifdef CONFIG_TDLS
3054                                 if(!(cap_attr & P2P_GRPCAP_INTRABSS) )
3055                                         ptdlsinfo->ap_prohibited = _TRUE;
3056 #endif // CONFIG_TDLS
3057                         }
3058
3059                         rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
3060                         if ( attr_contentlen == 1 )
3061                         {
3062                                 DBG_871X( "[%s] Status = %d\n", __FUNCTION__, attr_content );
3063                                 if ( attr_content == P2P_STATUS_SUCCESS )
3064                                 {
3065                                         //      Do nothing.
3066                                 }
3067                                 else
3068                                 {
3069                                         if ( P2P_STATUS_FAIL_INFO_UNAVAILABLE == attr_content ) {
3070                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INFOR_NOREADY);
3071                                         } else {
3072                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
3073                                         }
3074                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
3075                                         result = attr_content;
3076                                         break;
3077                                 }
3078                         }
3079
3080                         //      Try to get the peer's interface address
3081                         attr_contentlen = 0;
3082                         if ( rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen ) )
3083                         {
3084                                 if ( attr_contentlen != ETH_ALEN )
3085                                 {
3086                                         _rtw_memset( pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN );
3087                                 }
3088                         }
3089
3090                         //      Try to get the peer's intent and tie breaker value.
3091                         attr_content = 0x00;
3092                         attr_contentlen = 0;
3093                         if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen) )
3094                         {
3095                                 DBG_871X( "[%s] GO Intent = %d, tie = %d\n", __FUNCTION__, attr_content >> 1, attr_content & 0x01 );
3096                                 pwdinfo->peer_intent = attr_content;    //      include both intent and tie breaker values.
3097
3098                                 if ( pwdinfo->intent == ( pwdinfo->peer_intent >> 1 ) )
3099                                 {
3100                                         //      Try to match the tie breaker value
3101                                         if ( pwdinfo->intent == P2P_MAX_INTENT )
3102                                         {
3103                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
3104                                                 result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
3105                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
3106                                         }
3107                                         else
3108                                         {
3109                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
3110                                                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
3111                                                 if ( attr_content & 0x01 )
3112                                                 {
3113                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
3114                                                 }
3115                                                 else
3116                                                 {
3117                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
3118                                                 }
3119                                         }
3120                                 }
3121                                 else if ( pwdinfo->intent > ( pwdinfo->peer_intent >> 1 ) )
3122                                 {
3123                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
3124                                         rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
3125                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
3126                                 }
3127                                 else
3128                                 {
3129                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
3130                                         rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
3131                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
3132                                 }
3133
3134                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
3135                                 {
3136                                         //      Store the group id information.
3137                                         _rtw_memcpy( pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN );
3138                                         _rtw_memcpy( pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
3139
3140                                 }
3141                         }
3142
3143                         //      Try to get the operation channel information
3144
3145                         attr_contentlen = 0;
3146                         if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
3147                         {
3148                                 DBG_871X( "[%s] Peer's operating channel = %d\n", __FUNCTION__, operatingch_info[4] );
3149                                 pwdinfo->peer_operating_ch = operatingch_info[4];
3150                         }
3151
3152                         //      Try to get the channel list information
3153                         if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, pwdinfo->channel_list_attr, &pwdinfo->channel_list_attr_len ) )
3154                         {
3155                                 DBG_871X( "[%s] channel list attribute found, len = %d\n", __FUNCTION__,  pwdinfo->channel_list_attr_len );
3156
3157                                 peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len, peer_ch_list);
3158                                 ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
3159
3160                                 if( ch_num_inclusioned == 0)
3161                                 {
3162                                         DBG_871X( "[%s] No common channel in channel list!\n", __FUNCTION__ );
3163                                         result = P2P_STATUS_FAIL_NO_COMMON_CH;
3164                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
3165                                         break;
3166                                 }
3167
3168                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
3169                                 {
3170                                         if ( !rtw_p2p_is_channel_list_ok( pwdinfo->operating_channel,
3171                                                                                                         ch_list_inclusioned, ch_num_inclusioned) )
3172                                         {
3173 #ifdef CONFIG_CONCURRENT_MODE
3174                                                 if (check_buddy_fwstate(padapter, _FW_LINKED)
3175                                                         && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
3176                                                 {
3177                                                         DBG_871X( "[%s] desired channel NOT Found!\n", __FUNCTION__ );
3178                                                         result = P2P_STATUS_FAIL_NO_COMMON_CH;
3179                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
3180                                                         break;
3181                                                 }
3182                                                 else
3183 #endif //CONFIG_CONCURRENT_MODE
3184                                                 {
3185                                                         u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
3186                                                         attr_contentlen = 0;
3187
3188                                                         if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen) )
3189                                                         {
3190                                                                 peer_operating_ch = operatingch_info[4];
3191                                                         }
3192
3193                                                         if ( rtw_p2p_is_channel_list_ok( peer_operating_ch,
3194                                                                                                                         ch_list_inclusioned, ch_num_inclusioned) )
3195                                                         {
3196                                                                 /**
3197                                                                  *      Change our operating channel as peer's for compatibility.
3198                                                                  */
3199                                                                 pwdinfo->operating_channel = peer_operating_ch;
3200                                                                 DBG_871X( "[%s] Change op ch to %02x as peer's\n", __FUNCTION__, pwdinfo->operating_channel);
3201                                                         }
3202                                                         else
3203                                                         {
3204                                                                 // Take first channel of ch_list_inclusioned as operating channel
3205                                                                 pwdinfo->operating_channel = ch_list_inclusioned[0];
3206                                                                 DBG_871X( "[%s] Change op ch to %02x\n", __FUNCTION__, pwdinfo->operating_channel);
3207                                                         }
3208                                                 }
3209
3210                                         }
3211                                 }
3212
3213                         }
3214                         else
3215                         {
3216                                 DBG_871X( "[%s] channel list attribute not found!\n", __FUNCTION__);
3217                         }
3218
3219                         //      Try to get the group id information if peer is GO
3220                         attr_contentlen = 0;
3221                         _rtw_memset( groupid, 0x00, 38 );
3222                         if ( rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen) )
3223                         {
3224                                 _rtw_memcpy( pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN );
3225                                 _rtw_memcpy( pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN );
3226                         }
3227
3228                         //Get the next P2P IE
3229                         p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
3230                 }
3231
3232         }
3233
3234         #ifdef CONFIG_WFD
3235         rtw_process_wfd_ies(padapter, pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, __func__);
3236         #endif
3237
3238         return( result );
3239
3240 }
3241
3242 u8 process_p2p_group_negotation_confirm( struct wifidirect_info *pwdinfo, u8 *pframe, uint len )
3243 {
3244         _adapter *padapter = pwdinfo->padapter;
3245         u8 * ies;
3246         u32 ies_len;
3247         u8 * p2p_ie;
3248         u32     p2p_ielen = 0;
3249         u8      result = P2P_STATUS_SUCCESS;
3250         ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
3251         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
3252
3253         p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
3254         while ( p2p_ie )        //      Found the P2P IE.
3255         {
3256                 u8      attr_content = 0x00, operatingch_info[5] = { 0x00 };
3257                 u8      groupid[ 38 ] = { 0x00 };
3258                 u32     attr_contentlen = 0;
3259
3260                 pwdinfo->negotiation_dialog_token = 1;
3261                 rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
3262                 if ( attr_contentlen == 1 )
3263                 {
3264                         DBG_871X( "[%s] Status = %d\n", __FUNCTION__, attr_content );
3265                         result = attr_content;
3266
3267                         if ( attr_content == P2P_STATUS_SUCCESS )
3268                         {
3269                                 u8      bcancelled = 0;
3270
3271                                 _cancel_timer( &pwdinfo->restore_p2p_state_timer, &bcancelled );
3272
3273                                 //      Commented by Albert 20100911
3274                                 //      Todo: Need to handle the case which both Intents are the same.
3275                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
3276                                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
3277                                 if ( ( pwdinfo->intent ) > ( pwdinfo->peer_intent >> 1 ) )
3278                                 {
3279                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
3280                                 }
3281                                 else if ( ( pwdinfo->intent ) < ( pwdinfo->peer_intent >> 1 ) )
3282                                 {
3283                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
3284                                 }
3285                                 else
3286                                 {
3287                                         //      Have to compare the Tie Breaker
3288                                         if ( pwdinfo->peer_intent & 0x01 )
3289                                         {
3290                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
3291                                         }
3292                                         else
3293                                         {
3294                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
3295                                         }
3296                                 }
3297
3298 #ifdef CONFIG_CONCURRENT_MODE
3299                                 if (check_buddy_fwstate(padapter , _FW_LINKED)
3300                                         && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
3301                                 {
3302                                         //      Switch back to the AP channel soon.
3303                                         _set_timer( &pwdinfo->ap_p2p_switch_timer, 100 );
3304                                 }
3305 #endif
3306                         }
3307                         else
3308                         {
3309                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
3310                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
3311                                 break;
3312                         }
3313                 }
3314
3315                 //      Try to get the group id information
3316                 attr_contentlen = 0;
3317                 _rtw_memset( groupid, 0x00, 38 );
3318                 if ( rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen) )
3319                 {
3320                         DBG_871X( "[%s] Ssid = %s, ssidlen = %zu\n", __FUNCTION__, &groupid[ETH_ALEN], strlen(&groupid[ETH_ALEN]) );
3321                         _rtw_memcpy( pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN );
3322                         _rtw_memcpy( pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN );
3323                 }
3324
3325                 attr_contentlen = 0;
3326                 if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen) )
3327                 {
3328                         DBG_871X( "[%s] Peer's operating channel = %d\n", __FUNCTION__, operatingch_info[4] );
3329                         pwdinfo->peer_operating_ch = operatingch_info[4];
3330                 }
3331
3332                 //Get the next P2P IE
3333                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
3334
3335         }
3336
3337         return( result );
3338 }
3339
3340 u8 process_p2p_presence_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
3341 {
3342         u8 *frame_body;
3343         u8 dialogToken=0;
3344         u8 status = P2P_STATUS_SUCCESS;
3345
3346         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
3347
3348         dialogToken = frame_body[6];
3349
3350         //todo: check NoA attribute
3351
3352         issue_p2p_presence_resp(pwdinfo, GetAddr2Ptr(pframe), status, dialogToken);
3353
3354         return _TRUE;
3355 }
3356
3357 void find_phase_handler( _adapter*      padapter )
3358 {
3359         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3360         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
3361         NDIS_802_11_SSID        ssid;
3362         _irqL                           irqL;
3363         u8                                      _status = 0;
3364
3365 _func_enter_;
3366
3367         _rtw_memset((unsigned char*)&ssid, 0, sizeof(NDIS_802_11_SSID));
3368         _rtw_memcpy(ssid.Ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN );
3369         ssid.SsidLength = P2P_WILDCARD_SSID_LEN;
3370
3371         rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
3372
3373         _enter_critical_bh(&pmlmepriv->lock, &irqL);
3374         _status = rtw_sitesurvey_cmd(padapter, &ssid, 1, NULL, 0);
3375         _exit_critical_bh(&pmlmepriv->lock, &irqL);
3376
3377
3378 _func_exit_;
3379 }
3380
3381 void p2p_concurrent_handler(  _adapter* padapter );
3382
3383 void restore_p2p_state_handler( _adapter*       padapter )
3384 {
3385         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3386         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
3387
3388 _func_enter_;
3389
3390         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
3391         {
3392                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
3393         }
3394
3395 #ifdef CONFIG_CONCURRENT_MODE
3396         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3397         {
3398                 _adapter                                *pbuddy_adapter = padapter->pbuddy_adapter;
3399                 struct mlme_priv                *pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
3400                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3401
3402                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP))
3403                 {
3404                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
3405
3406                         issue_nulldata(pbuddy_adapter, NULL, 0, 3, 500);
3407                 }
3408         }
3409 #endif
3410
3411         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
3412
3413         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE))
3414         {
3415 #ifdef CONFIG_CONCURRENT_MODE
3416                 p2p_concurrent_handler( padapter );
3417 #else
3418                 //      In the P2P client mode, the driver should not switch back to its listen channel
3419                 //      because this P2P client should stay at the operating channel of P2P GO.
3420                 set_channel_bwmode( padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
3421 #endif
3422         }
3423 _func_exit_;
3424 }
3425
3426 void pre_tx_invitereq_handler( _adapter*        padapter )
3427 {
3428         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3429         u8      val8 = 1;
3430 _func_enter_;
3431
3432         set_channel_bwmode(padapter, pwdinfo->invitereq_info.peer_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
3433         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3434         issue_probereq_p2p(padapter, NULL);
3435         _set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
3436
3437 _func_exit_;
3438 }
3439
3440 void pre_tx_provdisc_handler( _adapter* padapter )
3441 {
3442         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3443         u8      val8 = 1;
3444 _func_enter_;
3445
3446         set_channel_bwmode(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
3447         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3448         issue_probereq_p2p(padapter, NULL);
3449         _set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
3450
3451 _func_exit_;
3452 }
3453
3454 void pre_tx_negoreq_handler( _adapter*  padapter )
3455 {
3456         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3457         u8      val8 = 1;
3458 _func_enter_;
3459
3460         set_channel_bwmode(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
3461         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3462         issue_probereq_p2p(padapter , NULL);
3463         /* WIN Phone only accept unicast probe request when nego back */
3464         issue_probereq_p2p(padapter , pwdinfo->nego_req_info.peerDevAddr);
3465         _set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
3466
3467 _func_exit_;
3468 }
3469
3470 #ifdef CONFIG_CONCURRENT_MODE
3471 void p2p_concurrent_handler( _adapter*  padapter )
3472 {
3473         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3474         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3475         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3476         //_adapter                              *pbuddy_adapter = padapter->pbuddy_adapter;
3477         //struct wifidirect_info        *pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
3478         //struct mlme_priv              *pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
3479         //struct mlme_ext_priv  *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3480         u8                                      val8;
3481 _func_enter_;
3482
3483         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3484         {
3485                 PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
3486                 struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3487
3488                 pwdinfo->operating_channel = pbuddy_mlmeext->cur_channel;
3489
3490                 if( pwdinfo->driver_interface == DRIVER_CFG80211 )
3491                 {
3492                         DBG_871X("%s, switch ch back to buddy's cur_channel=%d\n", __func__, pbuddy_mlmeext->cur_channel);
3493
3494                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
3495
3496                         if (check_buddy_fwstate(padapter, WIFI_FW_STATION_STATE))
3497                                 issue_nulldata(pbuddy_adapter, NULL, 0, 3, 500);
3498                 }
3499                 else if( pwdinfo->driver_interface == DRIVER_WEXT )
3500                 {
3501                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
3502                         {
3503                                 //      Now, the driver stays on the AP's channel.
3504                                 //      If the pwdinfo->ext_listen_period = 0, that means the P2P listen state is not available on listen channel.
3505                                 if ( pwdinfo->ext_listen_period > 0 )
3506                                 {
3507                                         DBG_8192C( "[%s] P2P_STATE_IDLE, ext_listen_period = %d\n", __FUNCTION__, pwdinfo->ext_listen_period );
3508
3509                                         if ( pbuddy_mlmeext->cur_channel != pwdinfo->listen_channel )
3510                                         {
3511                                                 //      Will switch to listen channel so that need to send the NULL data with PW bit to AP.
3512                                                 issue_nulldata(pbuddy_adapter, NULL, 1, 3, 500);
3513                                                 set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
3514                                         }
3515
3516                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
3517                                         if(!check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) &&
3518                                         ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE))
3519                                         {
3520                                                 val8 = 1;
3521                                                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3522                                         }
3523                                         //      Todo: To check the value of pwdinfo->ext_listen_period is equal to 0 or not.
3524                                         _set_timer( &pwdinfo->ap_p2p_switch_timer, pwdinfo->ext_listen_period );
3525                                 }
3526                         }
3527                         else if ( rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN) ||
3528                                         rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL) ||
3529                                         ( rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) && pwdinfo->nego_req_info.benable == _FALSE ) ||
3530                                         rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ) )
3531                         {
3532                                 //      Now, the driver is in the listen state of P2P mode.
3533                                 DBG_8192C( "[%s] P2P_STATE_IDLE, ext_listen_interval = %d\n", __FUNCTION__, pwdinfo->ext_listen_interval );
3534
3535                                 //      Commented by Albert 2012/11/01
3536                                 //      If the AP's channel is the same as the listen channel, we should still be in the listen state
3537                                 //      Other P2P device is still able to find this device out even this device is in the AP's channel.
3538                                 //      So, configure this device to be able to receive the probe request frame and set it to listen state.
3539                                 if ( pbuddy_mlmeext->cur_channel != pwdinfo->listen_channel )
3540                                 {
3541                                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
3542                                         if (!check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) && ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE))
3543                                         {
3544                                                 val8 = 0;
3545                                                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3546                                         }
3547                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_IDLE);
3548                                         issue_nulldata(pbuddy_adapter, NULL, 0, 3, 500);
3549                                 }
3550
3551                                 //      Todo: To check the value of pwdinfo->ext_listen_interval is equal to 0 or not.
3552                                 _set_timer( &pwdinfo->ap_p2p_switch_timer, pwdinfo->ext_listen_interval );
3553                         }
3554                         else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_OK))
3555                         {
3556                                 //      The driver had finished the P2P handshake successfully.
3557                                 val8 = 0;
3558                                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3559                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
3560                                 issue_nulldata(pbuddy_adapter, NULL, 0, 3, 500);
3561                         }
3562                         else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
3563                         {
3564                                 val8 = 1;
3565                                 set_channel_bwmode(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
3566                                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3567                                 issue_probereq_p2p(padapter, NULL);
3568                                 _set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
3569                         }
3570                         else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) && pwdinfo->nego_req_info.benable == _TRUE)
3571                         {
3572                                 val8 = 1;
3573                                 set_channel_bwmode(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
3574                                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3575                                 issue_probereq_p2p(padapter, NULL);
3576                                 _set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
3577                         }
3578                         else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ ) && pwdinfo->invitereq_info.benable == _TRUE)
3579                         {
3580                                 /*
3581                                 val8 = 1;
3582                         set_channel_bwmode(padapter, , HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
3583                                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3584                                 issue_probereq_p2p(padapter, NULL);
3585                                 _set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
3586                                 */
3587                         }
3588                 }
3589         }
3590         else
3591         {
3592                 /* In p2p+softap. When in P2P_STATE_GONEGO_OK, not back to listen channel.*/
3593                 if (!rtw_p2p_chk_state(pwdinfo , P2P_STATE_GONEGO_OK) || padapter->registrypriv.full_ch_in_p2p_handshake == 0)
3594                         set_channel_bwmode(padapter , pwdinfo->listen_channel , HAL_PRIME_CHNL_OFFSET_DONT_CARE , CHANNEL_WIDTH_20);
3595                 else
3596                         DBG_871X("%s, buddy not linked, go nego ok, not back to listen channel\n", __func__);
3597         }
3598
3599 _func_exit_;
3600 }
3601 #endif
3602
3603 #ifdef CONFIG_IOCTL_CFG80211
3604 static void ro_ch_handler(_adapter *padapter)
3605 {
3606         struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
3607         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
3608         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3609         u8 ch, bw, offset;
3610 _func_enter_;
3611
3612         if (rtw_get_ch_setting_union(padapter, &ch, &bw, &offset) != 0) {
3613                 if (0)
3614                         DBG_871X(FUNC_ADPT_FMT" back to linked/linking union - ch:%u, bw:%u, offset:%u\n",
3615                                 FUNC_ADPT_ARG(padapter), ch, bw, offset);
3616         }
3617         else if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->listen_channel) {
3618                 ch = pwdinfo->listen_channel;
3619                 bw = CHANNEL_WIDTH_20;
3620                 offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
3621                 if (0)
3622                         DBG_871X(FUNC_ADPT_FMT" back to listen ch - ch:%u, bw:%u, offset:%u\n",
3623                                 FUNC_ADPT_ARG(padapter), ch, bw, offset);
3624         }
3625         else {
3626                 ch = pcfg80211_wdinfo->restore_channel;
3627                 bw = CHANNEL_WIDTH_20;
3628                 offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
3629                 if (0)
3630                         DBG_871X(FUNC_ADPT_FMT" back to restore ch - ch:%u, bw:%u, offset:%u\n",
3631                                 FUNC_ADPT_ARG(padapter), ch, bw, offset);
3632         }
3633
3634         set_channel_bwmode(padapter, ch, offset, bw);
3635
3636         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
3637 #ifdef CONFIG_DEBUG_CFG80211
3638         DBG_871X("%s, role=%d, p2p_state=%d\n", __func__, rtw_p2p_role(pwdinfo), rtw_p2p_state(pwdinfo));
3639 #endif
3640
3641         pcfg80211_wdinfo->is_ro_ch = _FALSE;
3642         pcfg80211_wdinfo->last_ro_ch_time = rtw_get_current_time();
3643
3644         DBG_871X("cfg80211_remain_on_channel_expired cookie:0x%llx, ch=%d, bw=%d, offset=%d\n"
3645                 , pcfg80211_wdinfo->remain_on_ch_cookie
3646                 , rtw_get_oper_ch(padapter), rtw_get_oper_bw(padapter), rtw_get_oper_choffset(padapter));
3647
3648         rtw_cfg80211_remain_on_channel_expired(padapter,
3649                 pcfg80211_wdinfo->remain_on_ch_cookie,
3650                 &pcfg80211_wdinfo->remain_on_ch_channel,
3651                 pcfg80211_wdinfo->remain_on_ch_type, GFP_KERNEL);
3652
3653 _func_exit_;
3654 }
3655
3656 static void ro_ch_timer_process (void *FunctionContext)
3657 {
3658         _adapter *adapter = (_adapter *)FunctionContext;
3659         struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(adapter);
3660
3661         //printk("%s \n", __FUNCTION__);
3662
3663 #ifdef  CONFIG_CONCURRENT_MODE
3664         ATOMIC_SET(&pwdev_priv->ro_ch_to, 1);
3665 #endif
3666
3667         p2p_protocol_wk_cmd( adapter, P2P_RO_CH_WK);
3668 }
3669
3670 static void rtw_change_p2pie_op_ch(_adapter *padapter, const u8 *frame_body, u32 len, u8 ch)
3671 {
3672         u8 *ies, *p2p_ie;
3673         u32 ies_len, p2p_ielen;
3674         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
3675         struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3676
3677         ies = (u8*)(frame_body + _PUBLIC_ACTION_IE_OFFSET_);
3678         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
3679
3680         p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
3681
3682         while ( p2p_ie ) {
3683                 u32     attr_contentlen = 0;
3684                 u8 *pattr = NULL;
3685
3686                 //Check P2P_ATTR_OPERATING_CH
3687                 attr_contentlen = 0;
3688                 pattr = NULL;
3689                 if((pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, (uint*)&attr_contentlen))!=NULL)
3690                 {
3691                         *(pattr+4) = ch;
3692                 }
3693
3694                 //Get the next P2P IE
3695                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
3696         }
3697 }
3698
3699 static void rtw_change_p2pie_ch_list(_adapter *padapter, const u8 *frame_body, u32 len, u8 ch)
3700 {
3701         u8 *ies, *p2p_ie;
3702         u32 ies_len, p2p_ielen;
3703         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
3704         struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3705
3706         ies = (u8*)(frame_body + _PUBLIC_ACTION_IE_OFFSET_);
3707         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
3708
3709         p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
3710
3711         while (p2p_ie) {
3712                 u32     attr_contentlen = 0;
3713                 u8 *pattr = NULL;
3714
3715                 //Check P2P_ATTR_CH_LIST
3716                 if ((pattr=rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, (uint*)&attr_contentlen))!=NULL) {
3717                         int i;
3718                         u32 num_of_ch;
3719                         u8 *pattr_temp = pattr + 3 ;
3720
3721                         attr_contentlen -= 3;
3722
3723                         while (attr_contentlen>0) {
3724                                 num_of_ch = *(pattr_temp+1);
3725
3726                                 for(i=0; i<num_of_ch; i++)
3727                                         *(pattr_temp+2+i) = ch;
3728
3729                                 pattr_temp += (2+num_of_ch);
3730                                 attr_contentlen -= (2+num_of_ch);
3731                         }
3732                 }
3733
3734                 //Get the next P2P IE
3735                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
3736         }
3737 }
3738
3739 static bool rtw_chk_p2pie_ch_list_with_buddy(_adapter *padapter, const u8 *frame_body, u32 len)
3740 {
3741         bool fit = _FALSE;
3742 #ifdef CONFIG_CONCURRENT_MODE
3743         u8 *ies, *p2p_ie;
3744         u32 ies_len, p2p_ielen;
3745         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
3746         struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3747         u8 buddy_ch = pbuddy_mlmeext->cur_channel;
3748
3749         ies = (u8*)(frame_body + _PUBLIC_ACTION_IE_OFFSET_);
3750         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
3751
3752         p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
3753
3754         while (p2p_ie) {
3755                 u32     attr_contentlen = 0;
3756                 u8 *pattr = NULL;
3757
3758                 //Check P2P_ATTR_CH_LIST
3759                 if ((pattr=rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, (uint*)&attr_contentlen))!=NULL) {
3760                         int i;
3761                         u32 num_of_ch;
3762                         u8 *pattr_temp = pattr + 3 ;
3763
3764                         attr_contentlen -= 3;
3765
3766                         while (attr_contentlen>0) {
3767                                 num_of_ch = *(pattr_temp+1);
3768
3769                                 for(i=0; i<num_of_ch; i++) {
3770                                         if (*(pattr_temp+2+i) == buddy_ch) {
3771                                                 DBG_871X(FUNC_ADPT_FMT" ch_list fit buddy_ch:%u\n", FUNC_ADPT_ARG(padapter), buddy_ch);
3772                                                 fit = _TRUE;
3773                                                 break;
3774                                         }
3775                                 }
3776
3777                                 pattr_temp += (2+num_of_ch);
3778                                 attr_contentlen -= (2+num_of_ch);
3779                         }
3780                 }
3781
3782                 //Get the next P2P IE
3783                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
3784         }
3785 #endif
3786         return fit;
3787 }
3788
3789 static bool rtw_chk_p2pie_op_ch_with_buddy(_adapter *padapter, const u8 *frame_body, u32 len)
3790 {
3791         bool fit = _FALSE;
3792 #ifdef CONFIG_CONCURRENT_MODE
3793         u8 *ies, *p2p_ie;
3794         u32 ies_len, p2p_ielen;
3795         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
3796         struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3797         u8 buddy_ch = pbuddy_mlmeext->cur_channel;
3798
3799         ies = (u8*)(frame_body + _PUBLIC_ACTION_IE_OFFSET_);
3800         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
3801
3802         p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
3803
3804         while (p2p_ie) {
3805                 u32     attr_contentlen = 0;
3806                 u8 *pattr = NULL;
3807
3808                 //Check P2P_ATTR_OPERATING_CH
3809                 attr_contentlen = 0;
3810                 pattr = NULL;
3811                 if((pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, (uint*)&attr_contentlen))!=NULL) {
3812                         if (*(pattr+4) == buddy_ch) {
3813                                 DBG_871X(FUNC_ADPT_FMT" op_ch fit buddy_ch:%u\n", FUNC_ADPT_ARG(padapter), buddy_ch);
3814                                 fit = _TRUE;
3815                                 break;
3816                         }
3817                 }
3818
3819                 //Get the next P2P IE
3820                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
3821         }
3822 #endif
3823         return fit;
3824 }
3825
3826 static void rtw_cfg80211_adjust_p2pie_channel(_adapter *padapter, const u8 *frame_body, u32 len)
3827 {
3828 #ifdef CONFIG_CONCURRENT_MODE
3829         u8 *ies, *p2p_ie;
3830         u32 ies_len, p2p_ielen;
3831         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
3832         struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3833
3834         ies = (u8*)(frame_body + _PUBLIC_ACTION_IE_OFFSET_);
3835         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
3836
3837         p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
3838
3839         while ( p2p_ie )
3840         {
3841                 u32     attr_contentlen = 0;
3842                 u8 *pattr = NULL;
3843
3844                 //Check P2P_ATTR_CH_LIST
3845                 if((pattr=rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, (uint*)&attr_contentlen))!=NULL)
3846                 {
3847                         int i;
3848                         u32 num_of_ch;
3849                         u8 *pattr_temp = pattr + 3 ;
3850
3851                         attr_contentlen -= 3;
3852
3853                         while(attr_contentlen>0)
3854                         {
3855                                 num_of_ch = *(pattr_temp+1);
3856
3857                                 for(i=0; i<num_of_ch; i++)
3858                                         *(pattr_temp+2+i) = pbuddy_mlmeext->cur_channel;//forcing to the same channel
3859
3860                                 pattr_temp += (2+num_of_ch);
3861                                 attr_contentlen -= (2+num_of_ch);
3862                         }
3863                 }
3864
3865                 //Check P2P_ATTR_OPERATING_CH
3866                 attr_contentlen = 0;
3867                 pattr = NULL;
3868                 if((pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, (uint*)&attr_contentlen))!=NULL)
3869                 {
3870                         *(pattr+4) = pbuddy_mlmeext->cur_channel;//forcing to the same channel
3871                 }
3872
3873                 //Get the next P2P IE
3874                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
3875
3876         }
3877
3878 #endif
3879 }
3880
3881 #ifdef CONFIG_WFD
3882 u32 rtw_xframe_build_wfd_ie(struct xmit_frame *xframe)
3883 {
3884         _adapter *adapter = xframe->padapter;
3885         struct wifidirect_info *wdinfo = &adapter->wdinfo;
3886         u8 *frame = xframe->buf_addr + TXDESC_OFFSET;
3887         u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
3888         u8 *frame_tail = frame + xframe->attrib.pktlen;
3889         u8 category, action, OUI_Subtype, dialogToken = 0;
3890         u32     wfdielen = 0;
3891
3892         category = frame_body[0];
3893         if (category == RTW_WLAN_CATEGORY_PUBLIC) {
3894                 action = frame_body[1];
3895                 if (action == ACT_PUBLIC_VENDOR
3896                         && _rtw_memcmp(frame_body + 2, P2P_OUI, 4) == _TRUE
3897                 ) {
3898                         OUI_Subtype = frame_body[6];
3899                         dialogToken = frame_body[7];
3900
3901                         switch (OUI_Subtype) {
3902                         case P2P_GO_NEGO_REQ:
3903                                 wfdielen = build_nego_req_wfd_ie(wdinfo, frame_tail);
3904                                 break;
3905                         case P2P_GO_NEGO_RESP:
3906                                 wfdielen = build_nego_resp_wfd_ie(wdinfo, frame_tail);
3907                                 break;
3908                         case P2P_GO_NEGO_CONF:
3909                                 wfdielen = build_nego_confirm_wfd_ie(wdinfo, frame_tail);
3910                                 break;
3911                         case P2P_INVIT_REQ:
3912                                 wfdielen = build_invitation_req_wfd_ie(wdinfo, frame_tail);
3913                                 break;
3914                         case P2P_INVIT_RESP:
3915                                 wfdielen = build_invitation_resp_wfd_ie(wdinfo, frame_tail);
3916                                 break;
3917                         case P2P_PROVISION_DISC_REQ:
3918                                 wfdielen = build_provdisc_req_wfd_ie(wdinfo, frame_tail);
3919                                 break;
3920                         case P2P_PROVISION_DISC_RESP:
3921                                 wfdielen = build_provdisc_resp_wfd_ie(wdinfo, frame_tail);
3922                                 break;
3923                         case P2P_DEVDISC_REQ:
3924                         case P2P_DEVDISC_RESP:
3925                         default:
3926                                 break;
3927                         }
3928
3929                 }
3930         } else if (category == RTW_WLAN_CATEGORY_P2P) {
3931                 OUI_Subtype = frame_body[5];
3932                 dialogToken = frame_body[6];
3933
3934                 #ifdef CONFIG_DEBUG_CFG80211
3935                 DBG_871X("ACTION_CATEGORY_P2P: OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n"
3936                         , cpu_to_be32(*((u32 *)(frame_body + 1))), OUI_Subtype, dialogToken);
3937                 #endif
3938
3939                 switch (OUI_Subtype) {
3940                 case P2P_NOTICE_OF_ABSENCE:
3941                         break;
3942                 case P2P_PRESENCE_REQUEST:
3943                         break;
3944                 case P2P_PRESENCE_RESPONSE:
3945                         break;
3946                 case P2P_GO_DISC_REQUEST:
3947                         break;
3948                 default:
3949                         break;
3950                 }
3951         } else {
3952                 DBG_871X("%s, action frame category=%d\n", __func__, category);
3953         }
3954
3955         xframe->attrib.pktlen += wfdielen;
3956
3957         return wfdielen;
3958 }
3959 #endif /* CONFIG_WFD */
3960
3961 bool rtw_xframe_del_wfd_ie(struct xmit_frame *xframe)
3962 {
3963 #define DBG_XFRAME_DEL_WFD_IE 0
3964
3965         _adapter *adapter = xframe->padapter;
3966         u8 *frame = xframe->buf_addr + TXDESC_OFFSET;
3967         u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
3968         u8 *frame_tail = frame + xframe->attrib.pktlen;
3969         u8 category, action, OUI_Subtype;
3970         u8 *ies = NULL;
3971         uint ies_len_ori = 0;
3972         uint ies_len = 0;
3973
3974         category = frame_body[0];
3975         if (category == RTW_WLAN_CATEGORY_PUBLIC) {
3976                 action = frame_body[1];
3977                 if (action == ACT_PUBLIC_VENDOR
3978                         && _rtw_memcmp(frame_body + 2, P2P_OUI, 4) == _TRUE
3979                 ) {
3980                         OUI_Subtype = frame_body[6];
3981
3982                         switch (OUI_Subtype) {
3983                         case P2P_GO_NEGO_REQ:
3984                         case P2P_GO_NEGO_RESP:
3985                         case P2P_GO_NEGO_CONF:
3986                         case P2P_INVIT_REQ:
3987                         case P2P_INVIT_RESP:
3988                         case P2P_PROVISION_DISC_REQ:
3989                         case P2P_PROVISION_DISC_RESP:
3990                                 ies = frame_body + 8;
3991                                 ies_len_ori = frame_tail - (frame_body + 8);
3992                                 break;
3993                         }
3994                 }
3995         }
3996
3997         if (ies && ies_len_ori) {
3998                 ies_len = rtw_del_wfd_ie(ies, ies_len_ori, DBG_XFRAME_DEL_WFD_IE ? __func__ : NULL);
3999                 xframe->attrib.pktlen -= (ies_len_ori - ies_len);
4000         }
4001
4002         return ies_len_ori != ies_len;
4003 }
4004
4005 /*
4006 * rtw_xframe_chk_wfd_ie -
4007 *
4008 */
4009 void rtw_xframe_chk_wfd_ie(struct xmit_frame *xframe)
4010 {
4011         _adapter *adapter = xframe->padapter;
4012         u8 *frame = xframe->buf_addr + TXDESC_OFFSET;
4013         u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
4014         u8 *frame_tail = frame + xframe->attrib.pktlen;
4015
4016         struct wifidirect_info *wdinfo = &adapter->wdinfo;
4017         struct mlme_priv *mlme = &adapter->mlmepriv;
4018         u8 build = 0;
4019         u8 del = 0;
4020
4021         if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
4022                 del = 1;
4023
4024 #ifdef CONFIG_IOCTL_CFG80211
4025         if (_TRUE == wdinfo->wfd_info->wfd_enable)
4026 #endif
4027                 del = build = 1;
4028
4029         if (del)
4030                 rtw_xframe_del_wfd_ie(xframe);
4031
4032 #ifdef CONFIG_WFD
4033         if (build)
4034                 rtw_xframe_build_wfd_ie(xframe);
4035 #endif
4036 }
4037
4038 u8 *dump_p2p_attr_ch_list(u8 *p2p_ie, uint p2p_ielen, u8 *buf, u32 buf_len)
4039 {
4040         uint attr_contentlen = 0;
4041         u8 *pattr = NULL;
4042         int w_sz = 0;
4043         u8 ch_cnt = 0;
4044         u8 ch_list[40];
4045         bool continuous = _FALSE;
4046
4047         if ((pattr=rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, &attr_contentlen))!=NULL) {
4048                 int i, j;
4049                 u32 num_of_ch;
4050                 u8 *pattr_temp = pattr + 3 ;
4051
4052                 attr_contentlen -= 3;
4053
4054                 _rtw_memset(ch_list, 0, 40);
4055
4056                 while (attr_contentlen>0) {
4057                         num_of_ch = *(pattr_temp+1);
4058
4059                         for(i=0; i<num_of_ch; i++) {
4060                                 for (j=0;j<ch_cnt;j++) {
4061                                         if (ch_list[j] == *(pattr_temp+2+i))
4062                                                 break;
4063                                 }
4064                                 if (j>=ch_cnt)
4065                                         ch_list[ch_cnt++] = *(pattr_temp+2+i);
4066
4067                         }
4068
4069                         pattr_temp += (2+num_of_ch);
4070                         attr_contentlen -= (2+num_of_ch);
4071                 }
4072
4073                 for (j=0;j<ch_cnt;j++) {
4074                         if (j == 0) {
4075                                 w_sz += snprintf(buf+w_sz, buf_len-w_sz, "%u", ch_list[j]);
4076                         } else if (ch_list[j] - ch_list[j-1] != 1) {
4077                                 w_sz += snprintf(buf+w_sz, buf_len-w_sz, ", %u", ch_list[j]);
4078                         } else if (j != ch_cnt-1 && ch_list[j+1] - ch_list[j] == 1) {
4079                                 /* empty */
4080                         } else {
4081                                 w_sz += snprintf(buf+w_sz, buf_len-w_sz, "-%u", ch_list[j]);
4082                         }
4083                 }
4084         }
4085         return buf;
4086 }
4087
4088 /*
4089  * return _TRUE if requester is GO, _FALSE if responder is GO
4090  */
4091 bool rtw_p2p_nego_intent_compare(u8 req, u8 resp)
4092 {
4093         if (req>>1 == resp >>1)
4094                 return  req&0x01 ? _TRUE : _FALSE;
4095         else if (req>>1 > resp>>1)
4096                 return _TRUE;
4097         else
4098                 return _FALSE;
4099 }
4100
4101 int rtw_p2p_check_frames(_adapter *padapter, const u8 *buf, u32 len, u8 tx)
4102 {
4103         int is_p2p_frame = (-1);
4104         unsigned char   *frame_body;
4105         u8 category, action, OUI_Subtype, dialogToken=0;
4106         u8 *p2p_ie = NULL;
4107         uint p2p_ielen = 0;
4108         struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(padapter);
4109         int status = -1;
4110         u8 ch_list_buf[128] = {'\0'};
4111         int op_ch = -1;
4112         int listen_ch = -1;
4113         u8 intent = 0;
4114
4115         frame_body = (unsigned char *)(buf + sizeof(struct rtw_ieee80211_hdr_3addr));
4116         category = frame_body[0];
4117         //just for check
4118         if(category == RTW_WLAN_CATEGORY_PUBLIC)
4119         {
4120                 action = frame_body[1];
4121                 if (action == ACT_PUBLIC_VENDOR
4122                         && _rtw_memcmp(frame_body+2, P2P_OUI, 4) == _TRUE
4123                 )
4124                 {
4125                         OUI_Subtype = frame_body[6];
4126                         dialogToken = frame_body[7];
4127                         is_p2p_frame = OUI_Subtype;
4128                         #ifdef CONFIG_DEBUG_CFG80211
4129                         DBG_871X("ACTION_CATEGORY_PUBLIC: ACT_PUBLIC_VENDOR, OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n",
4130                                 cpu_to_be32( *( ( u32* ) ( frame_body + 2 ) ) ), OUI_Subtype, dialogToken);
4131                         #endif
4132
4133                         p2p_ie = rtw_get_p2p_ie(
4134                                 (u8 *)buf+sizeof(struct rtw_ieee80211_hdr_3addr)+_PUBLIC_ACTION_IE_OFFSET_,
4135                                 len-sizeof(struct rtw_ieee80211_hdr_3addr)-_PUBLIC_ACTION_IE_OFFSET_,
4136                                 NULL, &p2p_ielen);
4137
4138                         switch( OUI_Subtype )//OUI Subtype
4139                         {
4140                                 u8 *cont;
4141                                 uint cont_len;
4142                                 case P2P_GO_NEGO_REQ:
4143                                 {
4144                                         struct rtw_wdev_nego_info* nego_info = &pwdev_priv->nego_info;
4145
4146                                         if (tx) {
4147                                                 #ifdef CONFIG_DRV_ISSUE_PROV_REQ // IOT FOR S2
4148                                                 if(pwdev_priv->provdisc_req_issued == _FALSE)
4149                                                         rtw_cfg80211_issue_p2p_provision_request(padapter, buf, len);
4150                                                 #endif //CONFIG_DRV_ISSUE_PROV_REQ
4151
4152                                                 //pwdev_priv->provdisc_req_issued = _FALSE;
4153
4154                                                 #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
4155                                                 if (check_buddy_fwstate(padapter , _FW_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
4156                                                         rtw_cfg80211_adjust_p2pie_channel(padapter , frame_body , len-sizeof(struct rtw_ieee80211_hdr_3addr));
4157                                                 #endif
4158                                         }
4159
4160                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
4161                                                 op_ch = *(cont+4);
4162                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_LISTEN_CH, NULL, &cont_len)))
4163                                                 listen_ch = *(cont+4);
4164                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, NULL, &cont_len)))
4165                                                 intent = *cont;
4166
4167                                         if (nego_info->token != dialogToken)
4168                                                 rtw_wdev_nego_info_init(nego_info);
4169
4170                                         _rtw_memcpy(nego_info->peer_mac, tx ? GetAddr1Ptr(buf) : GetAddr2Ptr(buf), ETH_ALEN);
4171                                         nego_info->active = tx ? 1 : 0;
4172                                         nego_info->token = dialogToken;
4173                                         nego_info->req_op_ch = op_ch;
4174                                         nego_info->req_listen_ch = listen_ch;
4175                                         nego_info->req_intent = intent;
4176                                         nego_info->state = 0;
4177
4178                                         dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128);
4179                                         DBG_871X("RTW_%s:P2P_GO_NEGO_REQ, dialogToken=%d, intent:%u%s, listen_ch:%d, op_ch:%d, ch_list:%s, full_ch_in_p2p_handshake:%d\n" ,
4180                                                         (tx == _TRUE)?"Tx":"Rx" , dialogToken , (intent>>1) , intent&0x1 ? "+" : "-" , listen_ch , op_ch , ch_list_buf , padapter->registrypriv.full_ch_in_p2p_handshake);
4181
4182                                         if (!tx) {
4183                                                 #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
4184                                                 if (check_buddy_fwstate(padapter, _FW_LINKED)
4185                                                         && rtw_chk_p2pie_ch_list_with_buddy(padapter , frame_body , len-sizeof(struct rtw_ieee80211_hdr_3addr)) == _FALSE
4186                                                         && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
4187                                                 {
4188                                                         DBG_871X(FUNC_ADPT_FMT" ch_list has no intersect with buddy\n", FUNC_ADPT_ARG(padapter));
4189                                                         rtw_change_p2pie_ch_list(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr), 0);
4190                                                 }
4191                                                 #endif
4192                                         }
4193
4194                                         break;
4195                                 }
4196                                 case P2P_GO_NEGO_RESP:
4197                                 {
4198                                         struct rtw_wdev_nego_info* nego_info = &pwdev_priv->nego_info;
4199
4200                                         if (tx) {
4201                                                 #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
4202                                                 if (check_buddy_fwstate(padapter , _FW_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
4203                                                         rtw_cfg80211_adjust_p2pie_channel(padapter , frame_body , len-sizeof(struct rtw_ieee80211_hdr_3addr));
4204                                                 #endif
4205                                         }
4206
4207                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
4208                                                 op_ch = *(cont+4);
4209                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, NULL, &cont_len)))
4210                                                 intent = *cont;
4211                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len)))
4212                                                 status = *cont;
4213
4214                                         if (nego_info->token == dialogToken && nego_info->state == 0
4215                                                 && _rtw_memcmp(nego_info->peer_mac, tx ? GetAddr1Ptr(buf) : GetAddr2Ptr(buf), ETH_ALEN) == _TRUE
4216                                         ) {
4217                                                 nego_info->status = (status==-1) ? 0xff : status;
4218                                                 nego_info->rsp_op_ch= op_ch;
4219                                                 nego_info->rsp_intent = intent;
4220                                                 nego_info->state = 1;
4221                                                 if (status != 0)
4222                                                         nego_info->token = 0; /* init */
4223                                         }
4224
4225                                         dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128);
4226                                         DBG_871X("RTW_%s:P2P_GO_NEGO_RESP, dialogToken=%d, intent:%u%s, status:%d, op_ch:%d, ch_list:%s\n",
4227                                                         (tx==_TRUE)?"Tx":"Rx", dialogToken, (intent>>1), intent&0x1 ? "+" : "-", status, op_ch, ch_list_buf);
4228
4229                                         if (!tx) {
4230                                                 pwdev_priv->provdisc_req_issued = _FALSE;
4231                                                 #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
4232                                                 if (check_buddy_fwstate(padapter, _FW_LINKED)
4233                                                         && rtw_chk_p2pie_ch_list_with_buddy(padapter , frame_body , len-sizeof(struct rtw_ieee80211_hdr_3addr)) == _FALSE
4234                                                         && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
4235                                                 {
4236                                                         DBG_871X(FUNC_ADPT_FMT" ch_list has no intersect with buddy\n", FUNC_ADPT_ARG(padapter));
4237                                                         rtw_change_p2pie_ch_list(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr), 0);
4238                                                 }
4239                                                 #endif
4240                                         }
4241
4242                                         break;
4243                                 }
4244                                 case P2P_GO_NEGO_CONF:
4245                                 {
4246                                         struct rtw_wdev_nego_info* nego_info = &pwdev_priv->nego_info;
4247                                         bool is_go = _FALSE;
4248
4249                                         if (tx) {
4250                                                 #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
4251                                                 if (check_buddy_fwstate(padapter, _FW_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
4252                                                         rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr));
4253                                                 #endif
4254                                         }
4255
4256                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
4257                                                 op_ch = *(cont+4);
4258                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len)))
4259                                                 status = *cont;
4260
4261                                         if (nego_info->token == dialogToken && nego_info->state == 1
4262                                                 && _rtw_memcmp(nego_info->peer_mac, tx ? GetAddr1Ptr(buf) : GetAddr2Ptr(buf), ETH_ALEN) == _TRUE
4263                                         ) {
4264                                                 nego_info->status = (status==-1) ? 0xff : status;
4265                                                 nego_info->conf_op_ch = (op_ch==-1) ? 0 : op_ch;
4266                                                 nego_info->state = 2;
4267
4268                                                 if (status == 0) {
4269                                                         if (rtw_p2p_nego_intent_compare(nego_info->req_intent, nego_info->rsp_intent) ^ !tx)
4270                                                                 is_go = _TRUE;
4271                                                 }
4272
4273                                                 nego_info->token = 0; /* init */
4274                                         }
4275
4276                                         dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128);
4277                                         DBG_871X("RTW_%s:P2P_GO_NEGO_CONF, dialogToken=%d, status:%d, op_ch:%d, ch_list:%s\n",
4278                                                 (tx==_TRUE)?"Tx":"Rx", dialogToken, status, op_ch, ch_list_buf);
4279
4280                                         if (!tx) {
4281                                         }
4282
4283                                         break;
4284                                 }
4285                                 case P2P_INVIT_REQ:
4286                                 {
4287                                         struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info;
4288                                         int flags = -1;
4289
4290                                         if (tx) {
4291                                                 #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
4292                                                 if (check_buddy_fwstate(padapter, _FW_LINKED)
4293                                                         && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
4294                                                         rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr));
4295                                                 #endif
4296                                         }
4297
4298                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, NULL, &cont_len)))
4299                                                 flags = *cont;
4300                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
4301                                                 op_ch = *(cont+4);
4302
4303                                         if (invit_info->token != dialogToken)
4304                                                 rtw_wdev_invit_info_init(invit_info);
4305
4306                                         _rtw_memcpy(invit_info->peer_mac, tx ? GetAddr1Ptr(buf) : GetAddr2Ptr(buf), ETH_ALEN);
4307                                         invit_info->active = tx ? 1 : 0;
4308                                         invit_info->token = dialogToken;
4309                                         invit_info->flags = (flags==-1) ? 0x0 : flags;
4310                                         invit_info->req_op_ch= op_ch;
4311                                         invit_info->state = 0;
4312
4313                                         dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128);
4314                                         DBG_871X("RTW_%s:P2P_INVIT_REQ, dialogToken=%d, flags:0x%02x, op_ch:%d, ch_list:%s\n",
4315                                                         (tx==_TRUE)?"Tx":"Rx", dialogToken, flags, op_ch, ch_list_buf);
4316
4317                                         if (!tx) {
4318                                                 #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
4319                                                 if (check_buddy_fwstate(padapter , _FW_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) {
4320                                                         if (op_ch != -1 && rtw_chk_p2pie_op_ch_with_buddy(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr)) == _FALSE) {
4321                                                                 DBG_871X(FUNC_ADPT_FMT" op_ch:%u has no intersect with buddy\n", FUNC_ADPT_ARG(padapter), op_ch);
4322                                                                 rtw_change_p2pie_ch_list(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr), 0);
4323                                                         } else if (rtw_chk_p2pie_ch_list_with_buddy(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr)) == _FALSE) {
4324                                                                 DBG_871X(FUNC_ADPT_FMT" ch_list has no intersect with buddy\n", FUNC_ADPT_ARG(padapter));
4325                                                                 rtw_change_p2pie_ch_list(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr), 0);
4326                                                         }
4327                                                 }
4328                                                 #endif
4329                                         }
4330
4331                                         break;
4332                                 }
4333                                 case P2P_INVIT_RESP:
4334                                 {
4335                                         struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info;
4336
4337                                         if (tx) {
4338                                                 #if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
4339                                                 if (check_buddy_fwstate(padapter , _FW_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
4340                                                         rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr));
4341                                                 #endif
4342                                         }
4343
4344                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len)))
4345                                         {
4346 #ifdef CONFIG_P2P_INVITE_IOT
4347                                                 if(tx && *cont==7)
4348                                                 {
4349                                                         DBG_871X("TX_P2P_INVITE_RESP, status is no common channel, change to unknown group\n");
4350                                                         *cont = 8; //unknow group status
4351                                                 }
4352 #endif //CONFIG_P2P_INVITE_IOT
4353                                                 status = *cont;
4354                                         }
4355                                         if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
4356                                                 op_ch = *(cont+4);
4357
4358                                         if (invit_info->token == dialogToken && invit_info->state == 0
4359                                                 && _rtw_memcmp(invit_info->peer_mac, tx ? GetAddr1Ptr(buf) : GetAddr2Ptr(buf), ETH_ALEN) == _TRUE
4360                                         ) {
4361                                                 invit_info->status = (status==-1) ? 0xff : status;
4362                                                 invit_info->rsp_op_ch= op_ch;
4363                                                 invit_info->state = 1;
4364                                                 invit_info->token = 0; /* init */
4365                                         }
4366
4367                                         dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128);
4368                                         DBG_871X("RTW_%s:P2P_INVIT_RESP, dialogToken=%d, status:%d, op_ch:%d, ch_list:%s\n",
4369                                                         (tx==_TRUE)?"Tx":"Rx", dialogToken, status, op_ch, ch_list_buf);
4370
4371                                         if (!tx) {
4372                                         }
4373
4374                                         break;
4375                                 }
4376                                 case P2P_DEVDISC_REQ:
4377                                         DBG_871X("RTW_%s:P2P_DEVDISC_REQ, dialogToken=%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken);
4378                                         break;
4379                                 case P2P_DEVDISC_RESP:
4380                                         cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
4381                                         DBG_871X("RTW_%s:P2P_DEVDISC_RESP, dialogToken=%d, status:%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken, cont?*cont:-1);
4382                                         break;
4383                                 case P2P_PROVISION_DISC_REQ:
4384                                 {
4385                                         size_t frame_body_len = len - sizeof(struct rtw_ieee80211_hdr_3addr);
4386                                         u8 *p2p_ie;
4387                                         uint p2p_ielen = 0;
4388                                         uint contentlen = 0;
4389
4390                                         DBG_871X("RTW_%s:P2P_PROVISION_DISC_REQ, dialogToken=%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken);
4391
4392                                         //if(tx)
4393                                         {
4394                                                 pwdev_priv->provdisc_req_issued = _FALSE;
4395
4396                                                 if( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, frame_body_len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)))
4397                                                 {
4398
4399                                                         if(rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, NULL, &contentlen))
4400                                                         {
4401                                                                 pwdev_priv->provdisc_req_issued = _FALSE;//case: p2p_client join p2p GO
4402                                                         }
4403                                                         else
4404                                                         {
4405                                                                 #ifdef CONFIG_DEBUG_CFG80211
4406                                                                 DBG_871X("provdisc_req_issued is _TRUE\n");
4407                                                                 #endif //CONFIG_DEBUG_CFG80211
4408                                                                 pwdev_priv->provdisc_req_issued = _TRUE;//case: p2p_devices connection before Nego req.
4409                                                         }
4410
4411                                                 }
4412                                         }
4413                                 }
4414                                         break;
4415                                 case P2P_PROVISION_DISC_RESP:
4416                                         DBG_871X("RTW_%s:P2P_PROVISION_DISC_RESP, dialogToken=%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken);
4417                                         break;
4418                                 default:
4419                                         DBG_871X("RTW_%s:OUI_Subtype=%d, dialogToken=%d\n", (tx==_TRUE)?"Tx":"Rx", OUI_Subtype, dialogToken);
4420                                         break;
4421                         }
4422
4423                 }
4424
4425         }
4426         else if(category == RTW_WLAN_CATEGORY_P2P)
4427         {
4428                 OUI_Subtype = frame_body[5];
4429                 dialogToken = frame_body[6];
4430
4431                 #ifdef CONFIG_DEBUG_CFG80211
4432                 DBG_871X("ACTION_CATEGORY_P2P: OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n",
4433                         cpu_to_be32( *( ( u32* ) ( frame_body + 1 ) ) ), OUI_Subtype, dialogToken);
4434                 #endif
4435
4436                 is_p2p_frame = OUI_Subtype;
4437
4438                 switch(OUI_Subtype)
4439                 {
4440                         case P2P_NOTICE_OF_ABSENCE:
4441                                 DBG_871X("RTW_%s:P2P_NOTICE_OF_ABSENCE, dialogToken=%d\n", (tx==_TRUE)?"TX":"RX", dialogToken);
4442                                 break;
4443                         case P2P_PRESENCE_REQUEST:
4444                                 DBG_871X("RTW_%s:P2P_PRESENCE_REQUEST, dialogToken=%d\n", (tx==_TRUE)?"TX":"RX", dialogToken);
4445                                 break;
4446                         case P2P_PRESENCE_RESPONSE:
4447                                 DBG_871X("RTW_%s:P2P_PRESENCE_RESPONSE, dialogToken=%d\n", (tx==_TRUE)?"TX":"RX", dialogToken);
4448                                 break;
4449                         case P2P_GO_DISC_REQUEST:
4450                                 DBG_871X("RTW_%s:P2P_GO_DISC_REQUEST, dialogToken=%d\n", (tx==_TRUE)?"TX":"RX", dialogToken);
4451                                 break;
4452                         default:
4453                                 DBG_871X("RTW_%s:OUI_Subtype=%d, dialogToken=%d\n", (tx==_TRUE)?"TX":"RX", OUI_Subtype, dialogToken);
4454                                 break;
4455                 }
4456
4457         }
4458         else
4459         {
4460                 DBG_871X("RTW_%s:action frame category=%d\n", (tx==_TRUE)?"TX":"RX", category);
4461         }
4462
4463         return is_p2p_frame;
4464 }
4465
4466 void rtw_init_cfg80211_wifidirect_info( _adapter*       padapter)
4467 {
4468         struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
4469
4470         _rtw_memset(pcfg80211_wdinfo, 0x00, sizeof(struct cfg80211_wifidirect_info) );
4471
4472         _init_timer( &pcfg80211_wdinfo->remain_on_ch_timer, padapter->pnetdev, ro_ch_timer_process, padapter );
4473 }
4474 #endif //CONFIG_IOCTL_CFG80211
4475
4476 void p2p_protocol_wk_hdl(_adapter *padapter, int intCmdType)
4477 {
4478         struct wifidirect_info  *pwdinfo= &(padapter->wdinfo);
4479
4480 _func_enter_;
4481
4482         switch(intCmdType)
4483         {
4484                 case P2P_FIND_PHASE_WK:
4485                 {
4486                         find_phase_handler( padapter );
4487                         break;
4488                 }
4489                 case P2P_RESTORE_STATE_WK:
4490                 {
4491                         restore_p2p_state_handler( padapter );
4492                         break;
4493                 }
4494                 case P2P_PRE_TX_PROVDISC_PROCESS_WK:
4495                 {
4496 #ifdef CONFIG_CONCURRENT_MODE
4497                         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4498                         {
4499                                 p2p_concurrent_handler( padapter );
4500                         }
4501                         else
4502                         {
4503                                 pre_tx_provdisc_handler( padapter );
4504                         }
4505 #else
4506                         pre_tx_provdisc_handler( padapter );
4507 #endif
4508                         break;
4509                 }
4510                 case P2P_PRE_TX_INVITEREQ_PROCESS_WK:
4511                 {
4512 #ifdef CONFIG_CONCURRENT_MODE
4513                         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4514                         {
4515                                 p2p_concurrent_handler( padapter );
4516                         }
4517                         else
4518                         {
4519                                 pre_tx_invitereq_handler( padapter );
4520                         }
4521 #else
4522                         pre_tx_invitereq_handler( padapter );
4523 #endif
4524                         break;
4525                 }
4526                 case P2P_PRE_TX_NEGOREQ_PROCESS_WK:
4527                 {
4528 #ifdef CONFIG_CONCURRENT_MODE
4529                         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4530                         {
4531                                 p2p_concurrent_handler( padapter );
4532                         }
4533                         else
4534                         {
4535                                 pre_tx_negoreq_handler( padapter );
4536                         }
4537 #else
4538                         pre_tx_negoreq_handler( padapter );
4539 #endif
4540                         break;
4541                 }
4542 #ifdef CONFIG_P2P
4543 #ifdef CONFIG_CONCURRENT_MODE
4544                 case P2P_AP_P2P_CH_SWITCH_PROCESS_WK:
4545                 {
4546                         p2p_concurrent_handler( padapter );
4547                         break;
4548                 }
4549 #endif
4550 #endif
4551 #ifdef CONFIG_IOCTL_CFG80211
4552                 case P2P_RO_CH_WK:
4553                 {
4554                         ro_ch_handler( padapter );
4555                         break;
4556                 }
4557 #endif //CONFIG_IOCTL_CFG80211
4558
4559         }
4560
4561 _func_exit_;
4562 }
4563
4564 int process_p2p_cross_connect_ie(PADAPTER padapter, u8 *IEs, u32 IELength)
4565 {
4566         int ret = _TRUE;
4567         u8 * ies;
4568         u32 ies_len;
4569         u8 * p2p_ie;
4570         u32     p2p_ielen = 0;
4571         u8      p2p_attr[MAX_P2P_IE_LEN] = { 0x00 };// NoA length should be n*(13) + 2
4572         u32     attr_contentlen = 0;
4573
4574         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo );
4575
4576 _func_enter_;
4577
4578         if(IELength <= _BEACON_IE_OFFSET_)
4579                 return ret;
4580
4581         ies = IEs + _BEACON_IE_OFFSET_;
4582         ies_len = IELength - _BEACON_IE_OFFSET_;
4583
4584         p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen);
4585
4586         while(p2p_ie)
4587         {
4588                 // Get P2P Manageability IE.
4589                 if(rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_MANAGEABILITY, p2p_attr, &attr_contentlen))
4590                 {
4591                         if ((p2p_attr[0]&(BIT(0)|BIT(1))) == 0x01) {
4592                                 ret = _FALSE;
4593                         }
4594                         break;
4595                 }
4596                 //Get the next P2P IE
4597                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
4598         }
4599
4600 _func_exit_;
4601         return ret;
4602 }
4603
4604 #ifdef CONFIG_P2P_PS
4605 void process_p2p_ps_ie(PADAPTER padapter, u8 *IEs, u32 IELength)
4606 {
4607         u8 * ies;
4608         u32 ies_len;
4609         u8 * p2p_ie;
4610         u32     p2p_ielen = 0;
4611         u8      noa_attr[MAX_P2P_IE_LEN] = { 0x00 };// NoA length should be n*(13) + 2
4612         u32     attr_contentlen = 0;
4613
4614         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo );
4615         u8      find_p2p = _FALSE, find_p2p_ps = _FALSE;
4616         u8      noa_offset, noa_num, noa_index;
4617
4618 _func_enter_;
4619
4620         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
4621         {
4622                 return;
4623         }
4624 #ifdef CONFIG_CONCURRENT_MODE
4625         if(padapter->iface_type != IFACE_PORT0)
4626                 return;
4627 #endif
4628         if(IELength <= _BEACON_IE_OFFSET_)
4629                 return;
4630
4631         ies = IEs + _BEACON_IE_OFFSET_;
4632         ies_len = IELength - _BEACON_IE_OFFSET_;
4633
4634         p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen);
4635
4636         while(p2p_ie)
4637         {
4638                 find_p2p = _TRUE;
4639                 // Get Notice of Absence IE.
4640                 if(rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_NOA, noa_attr, &attr_contentlen))
4641                 {
4642                         find_p2p_ps = _TRUE;
4643                         noa_index = noa_attr[0];
4644
4645                         if( (pwdinfo->p2p_ps_mode == P2P_PS_NONE) ||
4646                                 (noa_index != pwdinfo->noa_index) )// if index change, driver should reconfigure related setting.
4647                         {
4648                                 pwdinfo->noa_index = noa_index;
4649                                 pwdinfo->opp_ps = noa_attr[1] >> 7;
4650                                 pwdinfo->ctwindow = noa_attr[1] & 0x7F;
4651
4652                                 noa_offset = 2;
4653                                 noa_num = 0;
4654                                 // NoA length should be n*(13) + 2
4655                                 if(attr_contentlen > 2)
4656                                 {
4657                                         while(noa_offset < attr_contentlen)
4658                                         {
4659                                                 //_rtw_memcpy(&wifidirect_info->noa_count[noa_num], &noa_attr[noa_offset], 1);
4660                                                 pwdinfo->noa_count[noa_num] = noa_attr[noa_offset];
4661                                                 noa_offset += 1;
4662
4663                                                 _rtw_memcpy(&pwdinfo->noa_duration[noa_num], &noa_attr[noa_offset], 4);
4664                                                 noa_offset += 4;
4665
4666                                                 _rtw_memcpy(&pwdinfo->noa_interval[noa_num], &noa_attr[noa_offset], 4);
4667                                                 noa_offset += 4;
4668
4669                                                 _rtw_memcpy(&pwdinfo->noa_start_time[noa_num], &noa_attr[noa_offset], 4);
4670                                                 noa_offset += 4;
4671
4672                                                 noa_num++;
4673                                         }
4674                                 }
4675                                 pwdinfo->noa_num = noa_num;
4676
4677                                 if( pwdinfo->opp_ps == 1 )
4678                                 {
4679                                         pwdinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
4680                                         // driver should wait LPS for entering CTWindow
4681                                         if(adapter_to_pwrctl(padapter)->bFwCurrentInPSMode == _TRUE)
4682                                         {
4683                                                 p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1);
4684                                         }
4685                                 }
4686                                 else if( pwdinfo->noa_num > 0 )
4687                                 {
4688                                         pwdinfo->p2p_ps_mode = P2P_PS_NOA;
4689                                         p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1);
4690                                 }
4691                                 else if( pwdinfo->p2p_ps_mode > P2P_PS_NONE)
4692                                 {
4693                                         p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
4694                                 }
4695                         }
4696
4697                         break; // find target, just break.
4698                 }
4699
4700                 //Get the next P2P IE
4701                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
4702
4703         }
4704
4705         if(find_p2p == _TRUE)
4706         {
4707                 if( (pwdinfo->p2p_ps_mode > P2P_PS_NONE) && (find_p2p_ps == _FALSE) )
4708                 {
4709                         p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
4710                 }
4711         }
4712
4713 _func_exit_;
4714 }
4715
4716 void p2p_ps_wk_hdl(_adapter *padapter, u8 p2p_ps_state)
4717 {
4718         struct pwrctrl_priv             *pwrpriv = adapter_to_pwrctl(padapter);
4719         struct wifidirect_info  *pwdinfo= &(padapter->wdinfo);
4720
4721 _func_enter_;
4722
4723         // Pre action for p2p state
4724         switch(p2p_ps_state)
4725         {
4726                 case P2P_PS_DISABLE:
4727                         pwdinfo->p2p_ps_state = p2p_ps_state;
4728
4729                         rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
4730
4731                         pwdinfo->noa_index = 0;
4732                         pwdinfo->ctwindow = 0;
4733                         pwdinfo->opp_ps = 0;
4734                         pwdinfo->noa_num = 0;
4735                         pwdinfo->p2p_ps_mode = P2P_PS_NONE;
4736                         if(pwrpriv->bFwCurrentInPSMode == _TRUE)
4737                         {
4738                                 if(pwrpriv->smart_ps == 0)
4739                                 {
4740                                         pwrpriv->smart_ps = 2;
4741                                         rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&(pwrpriv->pwr_mode)));
4742                                 }
4743                         }
4744                         break;
4745                 case P2P_PS_ENABLE:
4746                         if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
4747                                 pwdinfo->p2p_ps_state = p2p_ps_state;
4748
4749                                 if( pwdinfo->ctwindow > 0 )
4750                                 {
4751                                         if(pwrpriv->smart_ps != 0)
4752                                         {
4753                                                 pwrpriv->smart_ps = 0;
4754                                                 DBG_871X("%s(): Enter CTW, change SmartPS\n", __FUNCTION__);
4755                                                 rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&(pwrpriv->pwr_mode)));
4756                                         }
4757                                 }
4758                                 rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
4759                         }
4760                         break;
4761                 case P2P_PS_SCAN:
4762                 case P2P_PS_SCAN_DONE:
4763                 case P2P_PS_ALLSTASLEEP:
4764                         if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
4765                                 pwdinfo->p2p_ps_state = p2p_ps_state;
4766                                 rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
4767                         }
4768                         break;
4769                 default:
4770                         break;
4771         }
4772
4773 _func_exit_;
4774 }
4775
4776 u8 p2p_ps_wk_cmd(_adapter*padapter, u8 p2p_ps_state, u8 enqueue)
4777 {
4778         struct cmd_obj  *ph2c;
4779         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
4780         struct wifidirect_info  *pwdinfo= &(padapter->wdinfo);
4781         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
4782         u8      res = _SUCCESS;
4783
4784 _func_enter_;
4785
4786         if ( rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)
4787 #ifdef CONFIG_CONCURRENT_MODE
4788                 || (padapter->iface_type != IFACE_PORT0)
4789 #endif
4790                 )
4791         {
4792                 return res;
4793         }
4794
4795         if(enqueue)
4796         {
4797                 ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
4798                 if(ph2c==NULL){
4799                         res= _FAIL;
4800                         goto exit;
4801                 }
4802
4803                 pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
4804                 if(pdrvextra_cmd_parm==NULL){
4805                         rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
4806                         res= _FAIL;
4807                         goto exit;
4808                 }
4809
4810                 pdrvextra_cmd_parm->ec_id = P2P_PS_WK_CID;
4811                 pdrvextra_cmd_parm->type = p2p_ps_state;
4812                 pdrvextra_cmd_parm->size = 0;
4813                 pdrvextra_cmd_parm->pbuf = NULL;
4814
4815                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
4816
4817                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
4818         }
4819         else
4820         {
4821                 p2p_ps_wk_hdl(padapter, p2p_ps_state);
4822         }
4823
4824 exit:
4825
4826 _func_exit_;
4827
4828         return res;
4829
4830 }
4831 #endif // CONFIG_P2P_PS
4832
4833 static void reset_ch_sitesurvey_timer_process (void *FunctionContext)
4834 {
4835         _adapter *adapter = (_adapter *)FunctionContext;
4836         struct  wifidirect_info         *pwdinfo = &adapter->wdinfo;
4837
4838         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
4839                 return;
4840
4841         DBG_871X( "[%s] In\n", __FUNCTION__ );
4842         //      Reset the operation channel information
4843         pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
4844 #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
4845         pwdinfo->rx_invitereq_info.operation_ch[1] = 0;
4846         pwdinfo->rx_invitereq_info.operation_ch[2] = 0;
4847         pwdinfo->rx_invitereq_info.operation_ch[3] = 0;
4848 #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
4849         pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
4850 }
4851
4852 static void reset_ch_sitesurvey_timer_process2 (void *FunctionContext)
4853 {
4854         _adapter *adapter = (_adapter *)FunctionContext;
4855         struct  wifidirect_info         *pwdinfo = &adapter->wdinfo;
4856
4857         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
4858                 return;
4859
4860         DBG_871X( "[%s] In\n", __FUNCTION__ );
4861         //      Reset the operation channel information
4862         pwdinfo->p2p_info.operation_ch[0] = 0;
4863 #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
4864         pwdinfo->p2p_info.operation_ch[1] = 0;
4865         pwdinfo->p2p_info.operation_ch[2] = 0;
4866         pwdinfo->p2p_info.operation_ch[3] = 0;
4867 #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
4868         pwdinfo->p2p_info.scan_op_ch_only = 0;
4869 }
4870
4871 static void restore_p2p_state_timer_process (void *FunctionContext)
4872 {
4873         _adapter *adapter = (_adapter *)FunctionContext;
4874         struct  wifidirect_info         *pwdinfo = &adapter->wdinfo;
4875
4876         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
4877                 return;
4878
4879         p2p_protocol_wk_cmd( adapter, P2P_RESTORE_STATE_WK );
4880 }
4881
4882 static void pre_tx_scan_timer_process (void *FunctionContext)
4883 {
4884         _adapter                                                        *adapter = (_adapter *) FunctionContext;
4885         struct  wifidirect_info                         *pwdinfo = &adapter->wdinfo;
4886         _irqL                                                   irqL;
4887         struct mlme_priv                                        *pmlmepriv = &adapter->mlmepriv;
4888         u8                                                              _status = 0;
4889
4890         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
4891                 return;
4892
4893         _enter_critical_bh(&pmlmepriv->lock, &irqL);
4894
4895
4896         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
4897         {
4898                 if ( _TRUE == pwdinfo->tx_prov_disc_info.benable )      //      the provision discovery request frame is trigger to send or not
4899                 {
4900                         p2p_protocol_wk_cmd( adapter, P2P_PRE_TX_PROVDISC_PROCESS_WK );
4901                         //issue_probereq_p2p(adapter, NULL);
4902                         //_set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
4903                 }
4904         }
4905         else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
4906         {
4907                 if ( _TRUE == pwdinfo->nego_req_info.benable )
4908                 {
4909                         p2p_protocol_wk_cmd( adapter, P2P_PRE_TX_NEGOREQ_PROCESS_WK );
4910                 }
4911         }
4912         else if ( rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ ) )
4913         {
4914                 if ( _TRUE == pwdinfo->invitereq_info.benable )
4915                 {
4916                         p2p_protocol_wk_cmd( adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK );
4917                 }
4918         }
4919         else
4920         {
4921                 DBG_8192C( "[%s] p2p_state is %d, ignore!!\n", __FUNCTION__, rtw_p2p_state(pwdinfo) );
4922         }
4923
4924         _exit_critical_bh(&pmlmepriv->lock, &irqL);
4925 }
4926
4927 static void find_phase_timer_process (void *FunctionContext)
4928 {
4929         _adapter *adapter = (_adapter *)FunctionContext;
4930         struct  wifidirect_info         *pwdinfo = &adapter->wdinfo;
4931
4932         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
4933                 return;
4934
4935         adapter->wdinfo.find_phase_state_exchange_cnt++;
4936
4937         p2p_protocol_wk_cmd( adapter, P2P_FIND_PHASE_WK );
4938 }
4939
4940 #ifdef CONFIG_CONCURRENT_MODE
4941 void ap_p2p_switch_timer_process (void *FunctionContext)
4942 {
4943         _adapter *adapter = (_adapter *)FunctionContext;
4944         struct  wifidirect_info         *pwdinfo = &adapter->wdinfo;
4945 #ifdef CONFIG_IOCTL_CFG80211
4946         struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(adapter);
4947 #endif
4948
4949         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
4950                 return;
4951
4952 #ifdef CONFIG_IOCTL_CFG80211
4953         ATOMIC_SET(&pwdev_priv->switch_ch_to, 1);
4954 #endif
4955
4956         p2p_protocol_wk_cmd( adapter, P2P_AP_P2P_CH_SWITCH_PROCESS_WK );
4957 }
4958 #endif
4959
4960 void reset_global_wifidirect_info( _adapter* padapter )
4961 {
4962         struct wifidirect_info  *pwdinfo;
4963
4964         pwdinfo = &padapter->wdinfo;
4965         pwdinfo->persistent_supported = 0;
4966         pwdinfo->session_available = _TRUE;
4967         rtw_tdls_wfd_enable(padapter, 0);
4968         pwdinfo->wfd_tdls_weaksec = _TRUE;
4969 }
4970
4971 #ifdef CONFIG_WFD
4972 int rtw_init_wifi_display_info(_adapter* padapter)
4973 {
4974         int     res = _SUCCESS;
4975         struct wifi_display_info *pwfd_info = &padapter->wfd_info;
4976
4977         // Used in P2P and TDLS
4978         pwfd_info->init_rtsp_ctrlport = 554;
4979 #ifdef CONFIG_IOCTL_CFG80211
4980         pwfd_info->rtsp_ctrlport = 0;
4981 #else
4982         pwfd_info->rtsp_ctrlport = pwfd_info->init_rtsp_ctrlport; /* set non-zero value for legacy wfd */
4983 #endif
4984         pwfd_info->tdls_rtsp_ctrlport = 0;
4985         pwfd_info->peer_rtsp_ctrlport = 0;      //      Reset to 0
4986         pwfd_info->wfd_enable = _FALSE;
4987         pwfd_info->wfd_device_type = WFD_DEVINFO_PSINK;
4988         pwfd_info->scan_result_type = SCAN_RESULT_P2P_ONLY;
4989
4990         // Used in P2P
4991         pwfd_info->peer_session_avail = _TRUE;
4992         pwfd_info->wfd_pc = _FALSE;
4993
4994         // Used in TDLS
4995         _rtw_memset( pwfd_info->ip_address, 0x00, 4 );
4996         _rtw_memset( pwfd_info->peer_ip_address, 0x00, 4 );
4997         return res;
4998
4999 }
5000
5001 inline void rtw_wfd_enable(_adapter *adapter, bool on)
5002 {
5003         struct wifi_display_info *wfdinfo = &adapter->wfd_info;
5004
5005         if (on) {
5006                 wfdinfo->rtsp_ctrlport = wfdinfo->init_rtsp_ctrlport;
5007                 wfdinfo->wfd_enable = _TRUE;
5008
5009         } else {
5010                 wfdinfo->wfd_enable = _FALSE;
5011                 wfdinfo->rtsp_ctrlport = 0;
5012         }
5013 }
5014
5015 inline void rtw_wfd_set_ctrl_port(_adapter *adapter, u16 port)
5016 {
5017         struct wifi_display_info *wfdinfo = &adapter->wfd_info;
5018
5019         wfdinfo->init_rtsp_ctrlport = port;
5020         if (wfdinfo->wfd_enable == _TRUE)
5021                 wfdinfo->rtsp_ctrlport = port;
5022         if (adapter->wdinfo.wfd_tdls_enable == 1)
5023                 wfdinfo->tdls_rtsp_ctrlport = port;
5024 }
5025
5026 inline void rtw_tdls_wfd_enable(_adapter *adapter, bool on)
5027 {
5028         struct wifi_display_info *wfdinfo = &adapter->wfd_info;
5029
5030         if (on) {
5031                 wfdinfo->tdls_rtsp_ctrlport = wfdinfo->init_rtsp_ctrlport;
5032                 adapter->wdinfo.wfd_tdls_enable = 1;
5033
5034         } else {
5035                 adapter->wdinfo.wfd_tdls_enable = 0;
5036                 wfdinfo->tdls_rtsp_ctrlport = 0;
5037         }
5038 }
5039
5040 u32 rtw_append_beacon_wfd_ie(_adapter *adapter, u8 *pbuf)
5041 {
5042         struct wifidirect_info *wdinfo = &adapter->wdinfo;
5043         struct mlme_priv *mlme = &adapter->mlmepriv;
5044         u8 build_ie_by_self = 0;
5045         u32 len = 0;
5046
5047         if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
5048                 goto exit;
5049
5050 #ifdef CONFIG_IOCTL_CFG80211
5051         if (_TRUE == wdinfo->wfd_info->wfd_enable)
5052 #endif
5053                 build_ie_by_self = 1;
5054
5055         if (build_ie_by_self)
5056                 len = build_beacon_wfd_ie(wdinfo, pbuf);
5057 #ifdef CONFIG_IOCTL_CFG80211
5058         else if (mlme->wfd_beacon_ie && mlme->wfd_beacon_ie_len > 0) {
5059                 len = mlme->wfd_beacon_ie_len;
5060                 _rtw_memcpy(pbuf, mlme->wfd_beacon_ie, len);
5061         }
5062 #endif
5063
5064 exit:
5065         return len;
5066 }
5067
5068 u32 rtw_append_probe_req_wfd_ie(_adapter *adapter, u8 *pbuf)
5069 {
5070         struct wifidirect_info *wdinfo = &adapter->wdinfo;
5071         struct mlme_priv *mlme = &adapter->mlmepriv;
5072         u8 build_ie_by_self = 0;
5073         u32 len = 0;
5074
5075         if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
5076                 goto exit;
5077
5078 #ifdef CONFIG_IOCTL_CFG80211
5079         if (_TRUE == wdinfo->wfd_info->wfd_enable)
5080 #endif
5081                 build_ie_by_self = 1;
5082
5083         if (build_ie_by_self)
5084                 len = build_probe_req_wfd_ie(wdinfo, pbuf);
5085 #ifdef CONFIG_IOCTL_CFG80211
5086         else if (mlme->wfd_probe_req_ie && mlme->wfd_probe_req_ie_len > 0) {
5087                 len = mlme->wfd_probe_req_ie_len;
5088                 _rtw_memcpy(pbuf, mlme->wfd_probe_req_ie, len);
5089         }
5090 #endif
5091
5092 exit:
5093         return len;
5094 }
5095
5096 u32 rtw_append_probe_resp_wfd_ie(_adapter *adapter, u8 *pbuf)
5097 {
5098         struct wifidirect_info *wdinfo = &adapter->wdinfo;
5099         struct mlme_priv *mlme = &adapter->mlmepriv;
5100         u8 build_ie_by_self = 0;
5101         u32 len = 0;
5102
5103         if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
5104                 goto exit;
5105
5106 #ifdef CONFIG_IOCTL_CFG80211
5107         if (_TRUE == wdinfo->wfd_info->wfd_enable)
5108 #endif
5109                 build_ie_by_self = 1;
5110
5111         if (build_ie_by_self)
5112                 len = build_probe_resp_wfd_ie(wdinfo, pbuf, 0);
5113 #ifdef CONFIG_IOCTL_CFG80211
5114         else if (mlme->wfd_probe_resp_ie && mlme->wfd_probe_resp_ie_len > 0) {
5115                 len = mlme->wfd_probe_resp_ie_len;
5116                 _rtw_memcpy(pbuf, mlme->wfd_probe_resp_ie, len);
5117         }
5118 #endif
5119
5120 exit:
5121         return len;
5122 }
5123
5124 u32 rtw_append_assoc_req_wfd_ie(_adapter *adapter, u8 *pbuf)
5125 {
5126         struct wifidirect_info *wdinfo = &adapter->wdinfo;
5127         struct mlme_priv *mlme = &adapter->mlmepriv;
5128         u8 build_ie_by_self = 0;
5129         u32 len = 0;
5130
5131         if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
5132                 goto exit;
5133
5134 #ifdef CONFIG_IOCTL_CFG80211
5135         if (_TRUE == wdinfo->wfd_info->wfd_enable)
5136 #endif
5137                 build_ie_by_self = 1;
5138
5139         if (build_ie_by_self)
5140                 len = build_assoc_req_wfd_ie(wdinfo, pbuf);
5141 #ifdef CONFIG_IOCTL_CFG80211
5142         else if (mlme->wfd_assoc_req_ie && mlme->wfd_assoc_req_ie_len > 0) {
5143                 len = mlme->wfd_assoc_req_ie_len;
5144                 _rtw_memcpy(pbuf, mlme->wfd_assoc_req_ie, len);
5145         }
5146 #endif
5147
5148 exit:
5149         return len;
5150 }
5151
5152 u32 rtw_append_assoc_resp_wfd_ie(_adapter *adapter, u8 *pbuf)
5153 {
5154         struct wifidirect_info *wdinfo = &adapter->wdinfo;
5155         struct mlme_priv *mlme = &adapter->mlmepriv;
5156         u8 build_ie_by_self = 0;
5157         u32 len = 0;
5158
5159         if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
5160                 goto exit;
5161
5162 #ifdef CONFIG_IOCTL_CFG80211
5163         if (_TRUE == wdinfo->wfd_info->wfd_enable)
5164 #endif
5165                 build_ie_by_self = 1;
5166
5167         if (build_ie_by_self)
5168                 len = build_assoc_resp_wfd_ie(wdinfo, pbuf);
5169 #ifdef CONFIG_IOCTL_CFG80211
5170         else if (mlme->wfd_assoc_resp_ie && mlme->wfd_assoc_resp_ie_len > 0) {
5171                 len = mlme->wfd_assoc_resp_ie_len;
5172                 _rtw_memcpy(pbuf, mlme->wfd_assoc_resp_ie, len);
5173         }
5174 #endif
5175
5176 exit:
5177         return len;
5178 }
5179
5180 #endif /* CONFIG_WFD */
5181
5182 void rtw_init_wifidirect_timers(_adapter* padapter)
5183 {
5184         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
5185
5186         _init_timer( &pwdinfo->find_phase_timer, padapter->pnetdev, find_phase_timer_process, padapter );
5187         _init_timer( &pwdinfo->restore_p2p_state_timer, padapter->pnetdev, restore_p2p_state_timer_process, padapter );
5188         _init_timer( &pwdinfo->pre_tx_scan_timer, padapter->pnetdev, pre_tx_scan_timer_process, padapter );
5189         _init_timer( &pwdinfo->reset_ch_sitesurvey, padapter->pnetdev, reset_ch_sitesurvey_timer_process, padapter );
5190         _init_timer( &pwdinfo->reset_ch_sitesurvey2, padapter->pnetdev, reset_ch_sitesurvey_timer_process2, padapter );
5191 #ifdef CONFIG_CONCURRENT_MODE
5192         _init_timer( &pwdinfo->ap_p2p_switch_timer, padapter->pnetdev, ap_p2p_switch_timer_process, padapter );
5193 #endif
5194 }
5195
5196 void rtw_init_wifidirect_addrs(_adapter* padapter, u8 *dev_addr, u8 *iface_addr)
5197 {
5198 #ifdef CONFIG_P2P
5199         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
5200
5201         /*init device&interface address */
5202         if (dev_addr) {
5203                 _rtw_memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN);
5204         }
5205         if (iface_addr) {
5206                 _rtw_memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN);
5207         }
5208 #endif
5209 }
5210
5211 void init_wifidirect_info( _adapter* padapter, enum P2P_ROLE role)
5212 {
5213         struct wifidirect_info  *pwdinfo;
5214 #ifdef CONFIG_WFD
5215         struct wifi_display_info        *pwfd_info = &padapter->wfd_info;
5216 #endif
5217 #ifdef CONFIG_CONCURRENT_MODE
5218         _adapter                                *pbuddy_adapter = padapter->pbuddy_adapter;
5219         struct wifidirect_info  *pbuddy_wdinfo = NULL;
5220         struct mlme_priv                *pbuddy_mlmepriv = NULL;
5221         struct mlme_ext_priv    *pbuddy_mlmeext = NULL;
5222 #endif
5223
5224         pwdinfo = &padapter->wdinfo;
5225
5226         pwdinfo->padapter = padapter;
5227
5228         //      1, 6, 11 are the social channel defined in the WiFi Direct specification.
5229         pwdinfo->social_chan[0] = 1;
5230         pwdinfo->social_chan[1] = 6;
5231         pwdinfo->social_chan[2] = 11;
5232         pwdinfo->social_chan[3] = 0;    //      channel 0 for scanning ending in site survey function.
5233
5234 #ifdef CONFIG_CONCURRENT_MODE
5235         if (pbuddy_adapter) {
5236                 pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
5237                 pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
5238                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
5239         }
5240
5241         if ( ( check_buddy_fwstate(padapter, _FW_LINKED ) == _TRUE ) &&
5242               ( ( pbuddy_mlmeext->cur_channel == 1) || ( pbuddy_mlmeext->cur_channel == 6 ) || ( pbuddy_mlmeext->cur_channel == 11 ) )
5243            )
5244         {
5245                 //      Use the AP's channel as the listen channel
5246                 //      This will avoid the channel switch between AP's channel and listen channel.
5247                 pwdinfo->listen_channel = pbuddy_mlmeext->cur_channel;
5248         }
5249         else
5250 #endif //CONFIG_CONCURRENT_MODE
5251         {
5252                 //      Use the channel 11 as the listen channel
5253                 pwdinfo->listen_channel = 11;
5254         }
5255
5256         if (role == P2P_ROLE_DEVICE)
5257         {
5258                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
5259                 #ifdef CONFIG_CONCURRENT_MODE
5260                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) == _TRUE )
5261                 {
5262                         rtw_p2p_set_state(pwdinfo, P2P_STATE_IDLE);
5263                 }
5264                 else
5265                 #endif
5266                 {
5267                         rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
5268                 }
5269                 pwdinfo->intent = 1;
5270                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_LISTEN);
5271         }
5272         else if (role == P2P_ROLE_CLIENT)
5273         {
5274                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
5275                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
5276                 pwdinfo->intent = 1;
5277                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
5278         }
5279         else if (role == P2P_ROLE_GO)
5280         {
5281                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
5282                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
5283                 pwdinfo->intent = 15;
5284                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
5285         }
5286
5287 //      Use the OFDM rate in the P2P probe response frame. ( 6(B), 9(B), 12, 18, 24, 36, 48, 54 )
5288         pwdinfo->support_rate[0] = 0x8c;        //      6(B)
5289         pwdinfo->support_rate[1] = 0x92;        //      9(B)
5290         pwdinfo->support_rate[2] = 0x18;        //      12
5291         pwdinfo->support_rate[3] = 0x24;        //      18
5292         pwdinfo->support_rate[4] = 0x30;        //      24
5293         pwdinfo->support_rate[5] = 0x48;        //      36
5294         pwdinfo->support_rate[6] = 0x60;        //      48
5295         pwdinfo->support_rate[7] = 0x6c;        //      54
5296
5297         _rtw_memcpy( ( void* ) pwdinfo->p2p_wildcard_ssid, "DIRECT-", 7 );
5298
5299         _rtw_memset( pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN );
5300         pwdinfo->device_name_len = 0;
5301
5302         _rtw_memset( &pwdinfo->invitereq_info, 0x00, sizeof( struct tx_invite_req_info ) );
5303         pwdinfo->invitereq_info.token = 3;      //      Token used for P2P invitation request frame.
5304
5305         _rtw_memset( &pwdinfo->inviteresp_info, 0x00, sizeof( struct tx_invite_resp_info ) );
5306         pwdinfo->inviteresp_info.token = 0;
5307
5308         pwdinfo->profileindex = 0;
5309         _rtw_memset( &pwdinfo->profileinfo[ 0 ], 0x00, sizeof( struct profile_info ) * P2P_MAX_PERSISTENT_GROUP_NUM );
5310
5311         rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
5312
5313         pwdinfo->listen_dwell = ( u8 ) (( rtw_get_current_time() % 3 ) + 1);
5314         //DBG_8192C( "[%s] listen_dwell time is %d00ms\n", __FUNCTION__, pwdinfo->listen_dwell );
5315
5316         _rtw_memset( &pwdinfo->tx_prov_disc_info, 0x00, sizeof( struct tx_provdisc_req_info ) );
5317         pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_NONE;
5318
5319         _rtw_memset( &pwdinfo->nego_req_info, 0x00, sizeof( struct tx_nego_req_info ) );
5320
5321         pwdinfo->device_password_id_for_nego = WPS_DPID_PBC;
5322         pwdinfo->negotiation_dialog_token = 1;
5323
5324         _rtw_memset( pwdinfo->nego_ssid, 0x00, WLAN_SSID_MAXLEN );
5325         pwdinfo->nego_ssidlen = 0;
5326
5327         pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO;
5328 #ifdef CONFIG_WFD
5329         pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY  | WPS_CONFIG_METHOD_PBC;
5330         pwdinfo->wfd_info = pwfd_info;
5331 #else
5332         pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD;
5333 #endif //CONFIG_WFD
5334         pwdinfo->channel_list_attr_len = 0;
5335         _rtw_memset( pwdinfo->channel_list_attr, 0x00, 100 );
5336
5337         _rtw_memset( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, 0x00, 4 );
5338         _rtw_memset( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, '0', 3 );
5339         _rtw_memset( &pwdinfo->groupid_info, 0x00, sizeof( struct group_id_info ) );
5340 #ifdef CONFIG_CONCURRENT_MODE
5341 #ifdef CONFIG_IOCTL_CFG80211
5342         pwdinfo->ext_listen_interval = 1000; //The interval to be available with legacy AP during p2p0-find/scan
5343         pwdinfo->ext_listen_period = 3000; //The time period to be available for P2P during nego
5344 #else //!CONFIG_IOCTL_CFG80211
5345         //pwdinfo->ext_listen_interval = 3000;
5346         //pwdinfo->ext_listen_period = 400;
5347         pwdinfo->ext_listen_interval = 1000;
5348         pwdinfo->ext_listen_period = 1000;
5349 #endif //!CONFIG_IOCTL_CFG80211
5350 #endif
5351
5352 // Commented by Kurt 20130319
5353 // For WiDi purpose: Use CFG80211 interface but controled WFD/RDS frame by driver itself.
5354 #ifdef CONFIG_IOCTL_CFG80211
5355         pwdinfo->driver_interface = DRIVER_CFG80211;
5356 #else
5357         pwdinfo->driver_interface = DRIVER_WEXT;
5358 #endif //CONFIG_IOCTL_CFG80211
5359
5360         pwdinfo->wfd_tdls_enable = 0;
5361         _rtw_memset( pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN );
5362         _rtw_memset( pwdinfo->p2p_peer_device_addr, 0x00, ETH_ALEN );
5363
5364         pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
5365         pwdinfo->rx_invitereq_info.operation_ch[1] = 0; //      Used to indicate the scan end in site survey function
5366 #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
5367         pwdinfo->rx_invitereq_info.operation_ch[2] = 0;
5368         pwdinfo->rx_invitereq_info.operation_ch[3] = 0;
5369         pwdinfo->rx_invitereq_info.operation_ch[4] = 0;
5370 #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
5371         pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
5372         pwdinfo->p2p_info.operation_ch[0] = 0;
5373         pwdinfo->p2p_info.operation_ch[1] = 0;                  //      Used to indicate the scan end in site survey function
5374 #ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
5375         pwdinfo->p2p_info.operation_ch[2] = 0;
5376         pwdinfo->p2p_info.operation_ch[3] = 0;
5377         pwdinfo->p2p_info.operation_ch[4] = 0;
5378 #endif //CONFIG_P2P_OP_CHK_SOCIAL_CH
5379         pwdinfo->p2p_info.scan_op_ch_only = 0;
5380 }
5381
5382 #ifdef CONFIG_DBG_P2P
5383
5384 /**
5385  * rtw_p2p_role_txt - Get the p2p role name as a text string
5386  * @role: P2P role
5387  * Returns: The state name as a printable text string
5388  */
5389 const char * rtw_p2p_role_txt(enum P2P_ROLE role)
5390 {
5391         switch (role) {
5392         case P2P_ROLE_DISABLE:
5393                 return "P2P_ROLE_DISABLE";
5394         case P2P_ROLE_DEVICE:
5395                 return "P2P_ROLE_DEVICE";
5396         case P2P_ROLE_CLIENT:
5397                 return "P2P_ROLE_CLIENT";
5398         case P2P_ROLE_GO:
5399                 return "P2P_ROLE_GO";
5400         default:
5401                 return "UNKNOWN";
5402         }
5403 }
5404
5405 /**
5406  * rtw_p2p_state_txt - Get the p2p state name as a text string
5407  * @state: P2P state
5408  * Returns: The state name as a printable text string
5409  */
5410 const char * rtw_p2p_state_txt(enum P2P_STATE state)
5411 {
5412         switch (state) {
5413         case P2P_STATE_NONE:
5414                 return "P2P_STATE_NONE";
5415         case P2P_STATE_IDLE:
5416                 return "P2P_STATE_IDLE";
5417         case P2P_STATE_LISTEN:
5418                 return "P2P_STATE_LISTEN";
5419         case P2P_STATE_SCAN:
5420                 return "P2P_STATE_SCAN";
5421         case P2P_STATE_FIND_PHASE_LISTEN:
5422                 return "P2P_STATE_FIND_PHASE_LISTEN";
5423         case P2P_STATE_FIND_PHASE_SEARCH:
5424                 return "P2P_STATE_FIND_PHASE_SEARCH";
5425         case P2P_STATE_TX_PROVISION_DIS_REQ:
5426                 return "P2P_STATE_TX_PROVISION_DIS_REQ";
5427         case P2P_STATE_RX_PROVISION_DIS_RSP:
5428                 return "P2P_STATE_RX_PROVISION_DIS_RSP";
5429         case P2P_STATE_RX_PROVISION_DIS_REQ:
5430                 return "P2P_STATE_RX_PROVISION_DIS_REQ";
5431         case P2P_STATE_GONEGO_ING:
5432                 return "P2P_STATE_GONEGO_ING";
5433         case P2P_STATE_GONEGO_OK:
5434                 return "P2P_STATE_GONEGO_OK";
5435         case P2P_STATE_GONEGO_FAIL:
5436                 return "P2P_STATE_GONEGO_FAIL";
5437         case P2P_STATE_RECV_INVITE_REQ_MATCH:
5438                 return "P2P_STATE_RECV_INVITE_REQ_MATCH";
5439         case P2P_STATE_PROVISIONING_ING:
5440                 return "P2P_STATE_PROVISIONING_ING";
5441         case P2P_STATE_PROVISIONING_DONE:
5442                 return "P2P_STATE_PROVISIONING_DONE";
5443         case P2P_STATE_TX_INVITE_REQ:
5444                 return "P2P_STATE_TX_INVITE_REQ";
5445         case P2P_STATE_RX_INVITE_RESP_OK:
5446                 return "P2P_STATE_RX_INVITE_RESP_OK";
5447         case P2P_STATE_RECV_INVITE_REQ_DISMATCH:
5448                 return "P2P_STATE_RECV_INVITE_REQ_DISMATCH";
5449         case P2P_STATE_RECV_INVITE_REQ_GO:
5450                 return "P2P_STATE_RECV_INVITE_REQ_GO";
5451         case P2P_STATE_RECV_INVITE_REQ_JOIN:
5452                 return "P2P_STATE_RECV_INVITE_REQ_JOIN";
5453         case P2P_STATE_RX_INVITE_RESP_FAIL:
5454                 return "P2P_STATE_RX_INVITE_RESP_FAIL";
5455         case P2P_STATE_RX_INFOR_NOREADY:
5456                 return "P2P_STATE_RX_INFOR_NOREADY";
5457         case P2P_STATE_TX_INFOR_NOREADY:
5458                 return "P2P_STATE_TX_INFOR_NOREADY";
5459         default:
5460                 return "UNKNOWN";
5461         }
5462 }
5463
5464 void dbg_rtw_p2p_set_state(struct wifidirect_info *wdinfo, enum P2P_STATE state, const char *caller, int line)
5465 {
5466         if(!_rtw_p2p_chk_state(wdinfo, state)) {
5467                 enum P2P_STATE old_state = _rtw_p2p_state(wdinfo);
5468                 _rtw_p2p_set_state(wdinfo, state);
5469                 DBG_871X("[CONFIG_DBG_P2P]%s:%d set_state from %s to %s\n", caller, line
5470                         , rtw_p2p_state_txt(old_state), rtw_p2p_state_txt(_rtw_p2p_state(wdinfo))
5471                 );
5472         } else {
5473                 DBG_871X("[CONFIG_DBG_P2P]%s:%d set_state to same state %s\n", caller, line
5474                         , rtw_p2p_state_txt(_rtw_p2p_state(wdinfo))
5475                 );
5476         }
5477 }
5478 void dbg_rtw_p2p_set_pre_state(struct wifidirect_info *wdinfo, enum P2P_STATE state, const char *caller, int line)
5479 {
5480         if(_rtw_p2p_pre_state(wdinfo) != state) {
5481                 enum P2P_STATE old_state = _rtw_p2p_pre_state(wdinfo);
5482                 _rtw_p2p_set_pre_state(wdinfo, state);
5483                 DBG_871X("[CONFIG_DBG_P2P]%s:%d set_pre_state from %s to %s\n", caller, line
5484                         , rtw_p2p_state_txt(old_state), rtw_p2p_state_txt(_rtw_p2p_pre_state(wdinfo))
5485                 );
5486         } else {
5487                 DBG_871X("[CONFIG_DBG_P2P]%s:%d set_pre_state to same state %s\n", caller, line
5488                         , rtw_p2p_state_txt(_rtw_p2p_pre_state(wdinfo))
5489                 );
5490         }
5491 }
5492 #if 0
5493 void dbg_rtw_p2p_restore_state(struct wifidirect_info *wdinfo, const char *caller, int line)
5494 {
5495         if(wdinfo->pre_p2p_state != -1) {
5496                 DBG_871X("[CONFIG_DBG_P2P]%s:%d restore from %s to %s\n", caller, line
5497                         , p2p_state_str[wdinfo->p2p_state], p2p_state_str[wdinfo->pre_p2p_state]
5498                 );
5499                 _rtw_p2p_restore_state(wdinfo);
5500         } else {
5501                 DBG_871X("[CONFIG_DBG_P2P]%s:%d restore no pre state, cur state %s\n", caller, line
5502                         , p2p_state_str[wdinfo->p2p_state]
5503                 );
5504         }
5505 }
5506 #endif
5507 void dbg_rtw_p2p_set_role(struct wifidirect_info *wdinfo, enum P2P_ROLE role, const char *caller, int line)
5508 {
5509         if(wdinfo->role != role) {
5510                 enum P2P_ROLE old_role = wdinfo->role;
5511                 _rtw_p2p_set_role(wdinfo, role);
5512                 DBG_871X("[CONFIG_DBG_P2P]%s:%d set_role from %s to %s\n", caller, line
5513                         , rtw_p2p_role_txt(old_role), rtw_p2p_role_txt(wdinfo->role)
5514                 );
5515         } else {
5516                 DBG_871X("[CONFIG_DBG_P2P]%s:%d set_role to same role %s\n", caller, line
5517                         , rtw_p2p_role_txt(wdinfo->role)
5518                 );
5519         }
5520 }
5521 #endif //CONFIG_DBG_P2P
5522
5523
5524 int rtw_p2p_enable(_adapter *padapter, enum P2P_ROLE role)
5525 {
5526         int ret = _SUCCESS;
5527         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
5528
5529         if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT|| role == P2P_ROLE_GO)
5530         {
5531                 u8 channel, ch_offset;
5532                 u16 bwmode;
5533
5534 #ifdef CONFIG_CONCURRENT_MODE
5535                 _adapter                                *pbuddy_adapter = padapter->pbuddy_adapter;
5536                 struct wifidirect_info  *pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
5537                 //      Commented by Albert 2011/12/30
5538                 //      The driver just supports 1 P2P group operation.
5539                 //      So, this function will do nothing if the buddy adapter had enabled the P2P function.
5540                 if(!rtw_p2p_chk_state(pbuddy_wdinfo, P2P_STATE_NONE))
5541                 {
5542                         //      The buddy adapter had enabled the P2P function.
5543                         return ret;
5544                 }
5545 #endif //CONFIG_CONCURRENT_MODE
5546
5547                 //leave IPS/Autosuspend
5548                 if (_FAIL == rtw_pwr_wakeup(padapter)) {
5549                         ret = _FAIL;
5550                         goto exit;
5551                 }
5552
5553                 //      Added by Albert 2011/03/22
5554                 //      In the P2P mode, the driver should not support the b mode.
5555                 //      So, the Tx packet shouldn't use the CCK rate
5556                 update_tx_basic_rate(padapter, WIRELESS_11AGN);
5557
5558                 //Enable P2P function
5559                 init_wifidirect_info(padapter, role);
5560
5561                 rtw_hal_set_odm_var(padapter,HAL_ODM_P2P_STATE,NULL,_TRUE);
5562                 #ifdef CONFIG_WFD
5563                 if (hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
5564                         rtw_hal_set_odm_var(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, _TRUE);
5565                 #endif
5566
5567         }
5568         else if (role == P2P_ROLE_DISABLE)
5569         {
5570 #ifdef CONFIG_INTEL_WIDI
5571                 if( padapter->mlmepriv.p2p_reject_disable == _TRUE )
5572                         return ret;
5573 #endif //CONFIG_INTEL_WIDI
5574
5575 #ifdef CONFIG_IOCTL_CFG80211
5576                 if( padapter->wdinfo.driver_interface == DRIVER_CFG80211 )
5577                         adapter_wdev_data(padapter)->p2p_enabled = _FALSE;
5578 #endif //CONFIG_IOCTL_CFG80211
5579
5580
5581                 //Disable P2P function
5582                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
5583                 {
5584                         _cancel_timer_ex( &pwdinfo->find_phase_timer );
5585                         _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
5586                         _cancel_timer_ex( &pwdinfo->pre_tx_scan_timer);
5587                         _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey);
5588                         _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey2);
5589                         reset_ch_sitesurvey_timer_process( padapter );
5590                         reset_ch_sitesurvey_timer_process2( padapter );
5591                         #ifdef CONFIG_CONCURRENT_MODE
5592                         _cancel_timer_ex( &pwdinfo->ap_p2p_switch_timer);
5593                         #endif
5594                         rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
5595                         rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_NONE);
5596                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DISABLE);
5597                         _rtw_memset(&pwdinfo->rx_prov_disc_info, 0x00, sizeof(struct rx_provdisc_req_info));
5598
5599                         /* Remove profiles in wifidirect_info structure. */
5600                         _rtw_memset( &pwdinfo->profileinfo[ 0 ], 0x00, sizeof( struct profile_info ) * P2P_MAX_PERSISTENT_GROUP_NUM );
5601                         pwdinfo->profileindex = 0;
5602                 }
5603
5604                 rtw_hal_set_odm_var(padapter,HAL_ODM_P2P_STATE,NULL,_FALSE);
5605                 #ifdef CONFIG_WFD
5606                 if (hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
5607                         rtw_hal_set_odm_var(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, _FALSE);
5608                 #endif
5609
5610                 if (_FAIL == rtw_pwr_wakeup(padapter)) {
5611                         ret = _FAIL;
5612                         goto exit;
5613                 }
5614
5615                 //Restore to initial setting.
5616                 update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
5617
5618 #ifdef CONFIG_INTEL_WIDI
5619                 rtw_reset_widi_info(padapter);
5620 #endif //CONFIG_INTEL_WIDI
5621
5622                 //For WiDi purpose.
5623 #ifdef CONFIG_IOCTL_CFG80211
5624                 pwdinfo->driver_interface = DRIVER_CFG80211;
5625 #else
5626                 pwdinfo->driver_interface = DRIVER_WEXT;
5627 #endif //CONFIG_IOCTL_CFG80211
5628
5629         }
5630
5631 exit:
5632         return ret;
5633 }
5634
5635 #endif //CONFIG_P2P