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