rtl8821ce/core/rtw_p2p.c

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