drivers/net/wireless/rockchip_wlan/rtl8723bu/core/rtw_p2p.c

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