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