drivers/staging/rtl8723au/core/rtw_p2p.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  ******************************************************************************/
  15 #define _RTW_P2P_C_
  16
  17 #include <drv_types.h>
  18 #include <rtw_p2p.h>
  19 #include <wifi.h>
  20
  21 #ifdef CONFIG_8723AU_P2P
  22
  23 static int rtw_p2p_is_channel_list_ok(u8 desired_ch, u8* ch_list, u8 ch_cnt)
  24 {
  25         int found = 0, i = 0;
  26
  27         for (i = 0; i < ch_cnt; i++)
  28         {
  29                 if (ch_list[ i ] == desired_ch)
  30                 {
  31                         found = 1;
  32                         break;
  33                 }
  34         }
  35         return found;
  36 }
  37
  38 static int is_any_client_associated(struct rtw_adapter *padapter)
  39 {
  40         return padapter->stapriv.asoc_list_cnt ? true : false;
  41 }
  42
  43 static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf)
  44 {
  45         struct list_head *phead, *plist;
  46         u32 len = 0;
  47         u16 attr_len = 0;
  48         u8 tmplen, *pdata_attr, *pstart, *pcur;
  49         struct sta_info *psta;
  50         struct rtw_adapter *padapter = pwdinfo->padapter;
  51         struct sta_priv *pstapriv = &padapter->stapriv;
  52
  53         DBG_8723A("%s\n", __func__);
  54
  55         pdata_attr = kzalloc(MAX_P2P_IE_LEN, GFP_ATOMIC);
  56
  57         pstart = pdata_attr;
  58         pcur = pdata_attr;
  59
  60         spin_lock_bh(&pstapriv->asoc_list_lock);
  61         phead = &pstapriv->asoc_list;
  62
  63         list_for_each(plist, phead) {
  64                 psta = container_of(plist, struct sta_info, asoc_list);
  65
  66                 if (psta->is_p2p_device)
  67                 {
  68                         tmplen = 0;
  69
  70                         pcur++;
  71
  72                         /* P2P device address */
  73                         memcpy(pcur, psta->dev_addr, ETH_ALEN);
  74                         pcur += ETH_ALEN;
  75
  76                         /* P2P interface address */
  77                         memcpy(pcur, psta->hwaddr, ETH_ALEN);
  78                         pcur += ETH_ALEN;
  79
  80                         *pcur = psta->dev_cap;
  81                         pcur++;
  82
  83                         /* u16*)(pcur) = cpu_to_be16(psta->config_methods); */
  84                         put_unaligned_be16(psta->config_methods, pcur);
  85                         pcur += 2;
  86
  87                         memcpy(pcur, psta->primary_dev_type, 8);
  88                         pcur += 8;
  89
  90                         *pcur = psta->num_of_secdev_type;
  91                         pcur++;
  92
  93                         memcpy(pcur, psta->secdev_types_list, psta->num_of_secdev_type*8);
  94                         pcur += psta->num_of_secdev_type*8;
  95
  96                         if (psta->dev_name_len>0)
  97                         {
  98                                 /* u16*)(pcur) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
  99                                 put_unaligned_be16(WPS_ATTR_DEVICE_NAME, pcur);
 100                                 pcur += 2;
 101
 102                                 /* u16*)(pcur) = cpu_to_be16(psta->dev_name_len); */
 103                                 put_unaligned_be16(psta->dev_name_len, pcur);
 104                                 pcur += 2;
 105
 106                                 memcpy(pcur, psta->dev_name, psta->dev_name_len);
 107                                 pcur += psta->dev_name_len;
 108                         }
 109
 110                         tmplen = (u8)(pcur-pstart);
 111
 112                         *pstart = (tmplen-1);
 113
 114                         attr_len += tmplen;
 115
 116                         /* pstart += tmplen; */
 117                         pstart = pcur;
 118
 119                 }
 120
 121         }
 122         spin_unlock_bh(&pstapriv->asoc_list_lock);
 123
 124         if (attr_len>0)
 125         {
 126                 len = rtw_set_p2p_attr_content23a(pbuf, P2P_ATTR_GROUP_INFO, attr_len, pdata_attr);
 127         }
 128
 129         kfree(pdata_attr);
 130
 131         return len;
 132 }
 133
 134 static void issue_group_disc_req(struct wifidirect_info *pwdinfo, u8 *da)
 135 {
 136         struct xmit_frame                       *pmgntframe;
 137         struct pkt_attrib                       *pattrib;
 138         unsigned char                                   *pframe;
 139         struct ieee80211_hdr    *pwlanhdr;
 140         unsigned short                          *fctrl;
 141         struct rtw_adapter *padapter = pwdinfo->padapter;
 142         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 143         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 144         unsigned char category = WLAN_CATEGORY_VENDOR_SPECIFIC;/* P2P action frame */
 145         u32     p2poui = cpu_to_be32(P2POUI);
 146         u8      oui_subtype = P2P_GO_DISC_REQUEST;
 147         u8      dialogToken = 0;
 148
 149         DBG_8723A("[%s]\n", __func__);
 150
 151         if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
 152         {
 153                 return;
 154         }
 155
 156         /* update attribute */
 157         pattrib = &pmgntframe->attrib;
 158         update_mgntframe_attrib23a(padapter, pattrib);
 159
 160         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 161
 162         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 163         pwlanhdr = (struct ieee80211_hdr *)pframe;
 164
 165         fctrl = &pwlanhdr->frame_control;
 166         *fctrl = 0;
 167
 168         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
 169         memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
 170         memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
 171
 172         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 173         pmlmeext->mgnt_seq++;
 174         SetFrameSubType(pframe, WIFI_ACTION);
 175
 176         pframe += sizeof(struct ieee80211_hdr_3addr);
 177         pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
 178
 179         /* Build P2P action frame header */
 180         pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
 181         pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
 182         pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
 183         pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
 184
 185         /* there is no IE in this P2P action frame */
 186
 187         pattrib->last_txcmdsz = pattrib->pktlen;
 188
 189         dump_mgntframe23a(padapter, pmgntframe);
 190 }
 191
 192 static void issue_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
 193 {
 194         struct xmit_frame                       *pmgntframe;
 195         struct pkt_attrib                       *pattrib;
 196         unsigned char                                   *pframe;
 197         struct ieee80211_hdr    *pwlanhdr;
 198         unsigned short                          *fctrl;
 199         struct rtw_adapter *padapter = pwdinfo->padapter;
 200         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 201         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 202         unsigned char category = WLAN_CATEGORY_PUBLIC;
 203         u8                      action = P2P_PUB_ACTION_ACTION;
 204         u32                     p2poui = cpu_to_be32(P2POUI);
 205         u8                      oui_subtype = P2P_DEVDISC_RESP;
 206         u8 p2pie[8] = { 0x00 };
 207         u32 p2pielen = 0;
 208
 209         DBG_8723A("[%s]\n", __func__);
 210
 211         if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
 212         {
 213                 return;
 214         }
 215
 216         /* update attribute */
 217         pattrib = &pmgntframe->attrib;
 218         update_mgntframe_attrib23a(padapter, pattrib);
 219
 220         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 221
 222         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 223         pwlanhdr = (struct ieee80211_hdr *)pframe;
 224
 225         fctrl = &pwlanhdr->frame_control;
 226         *fctrl = 0;
 227
 228         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
 229         memcpy(pwlanhdr->addr2, pwdinfo->device_addr, ETH_ALEN);
 230         memcpy(pwlanhdr->addr3, pwdinfo->device_addr, ETH_ALEN);
 231
 232         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 233         pmlmeext->mgnt_seq++;
 234         SetFrameSubType(pframe, WIFI_ACTION);
 235
 236         pframe += sizeof(struct ieee80211_hdr_3addr);
 237         pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
 238
 239         /* Build P2P public action frame header */
 240         pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
 241         pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
 242         pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
 243                                      &pattrib->pktlen);
 244         pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
 245         pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
 246
 247         /* Build P2P IE */
 248         /*      P2P OUI */
 249         p2pielen = 0;
 250         p2pie[ p2pielen++ ] = 0x50;
 251         p2pie[ p2pielen++ ] = 0x6F;
 252         p2pie[ p2pielen++ ] = 0x9A;
 253         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
 254
 255         /*  P2P_ATTR_STATUS */
 256         p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
 257
 258         pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen);
 259
 260         pattrib->last_txcmdsz = pattrib->pktlen;
 261
 262         dump_mgntframe23a(padapter, pmgntframe);
 263 }
 264
 265 static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8* raddr, u8* frame_body, u16 config_method)
 266 {
 267         struct rtw_adapter *padapter = pwdinfo->padapter;
 268         unsigned char category = WLAN_CATEGORY_PUBLIC;
 269         u8                      action = P2P_PUB_ACTION_ACTION;
 270         u8                      dialogToken = frame_body[7];    /*      The Dialog Token of provisioning discovery request frame. */
 271         u32                     p2poui = cpu_to_be32(P2POUI);
 272         u8                      oui_subtype = P2P_PROVISION_DISC_RESP;
 273         u8                      wpsie[ 100 ] = { 0x00 };
 274         u8                      wpsielen = 0;
 275 #ifdef CONFIG_8723AU_P2P
 276         u32                                     wfdielen = 0;
 277 #endif /* CONFIG_8723AU_P2P */
 278
 279         struct xmit_frame                       *pmgntframe;
 280         struct pkt_attrib                       *pattrib;
 281         unsigned char                                   *pframe;
 282         struct ieee80211_hdr    *pwlanhdr;
 283         unsigned short                          *fctrl;
 284         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 285         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 286
 287         if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
 288                 return;
 289
 290         /* update attribute */
 291         pattrib = &pmgntframe->attrib;
 292         update_mgntframe_attrib23a(padapter, pattrib);
 293
 294         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 295
 296         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 297         pwlanhdr = (struct ieee80211_hdr *)pframe;
 298
 299         fctrl = &pwlanhdr->frame_control;
 300         *fctrl = 0;
 301
 302         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
 303         memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
 304         memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
 305
 306         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 307         pmlmeext->mgnt_seq++;
 308         SetFrameSubType(pframe, WIFI_ACTION);
 309
 310         pframe += sizeof(struct ieee80211_hdr_3addr);
 311         pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
 312
 313         pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
 314         pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
 315         pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
 316                                      &pattrib->pktlen);
 317         pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
 318         pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
 319
 320         wpsielen = 0;
 321         /*      WPS OUI */
 322         /* u32*) (wpsie) = cpu_to_be32(WPSOUI); */
 323         put_unaligned_be32(WPSOUI, wpsie);
 324         wpsielen += 4;
 325
 326         /*      Config Method */
 327         /*      Type: */
 328         /* u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD); */
 329         put_unaligned_be16(WPS_ATTR_CONF_METHOD, wpsie + wpsielen);
 330         wpsielen += 2;
 331
 332         /*      Length: */
 333         /* u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002); */
 334         put_unaligned_be16(0x0002, wpsie + wpsielen);
 335         wpsielen += 2;
 336
 337         /*      Value: */
 338         /* u16*) (wpsie + wpsielen) = cpu_to_be16(config_method); */
 339         put_unaligned_be16(config_method, wpsie + wpsielen);
 340         wpsielen += 2;
 341
 342         pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen);
 343
 344 #ifdef CONFIG_8723AU_P2P
 345         wfdielen = build_provdisc_resp_wfd_ie(pwdinfo, pframe);
 346         pframe += wfdielen;
 347         pattrib->pktlen += wfdielen;
 348 #endif /* CONFIG_8723AU_P2P */
 349
 350         pattrib->last_txcmdsz = pattrib->pktlen;
 351
 352         dump_mgntframe23a(padapter, pmgntframe);
 353
 354         return;
 355 }
 356
 357 static void issue_p2p_presence_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
 358 {
 359         struct xmit_frame                       *pmgntframe;
 360         struct pkt_attrib                       *pattrib;
 361         unsigned char                                   *pframe;
 362         struct ieee80211_hdr    *pwlanhdr;
 363         unsigned short                          *fctrl;
 364         struct rtw_adapter *padapter = pwdinfo->padapter;
 365         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 366         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 367         unsigned char category = WLAN_CATEGORY_VENDOR_SPECIFIC;/* P2P action frame */
 368         u32     p2poui = cpu_to_be32(P2POUI);
 369         u8      oui_subtype = P2P_PRESENCE_RESPONSE;
 370         u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
 371         u8 noa_attr_content[32] = { 0x00 };
 372         u32 p2pielen = 0;
 373
 374         DBG_8723A("[%s]\n", __func__);
 375
 376         if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
 377         {
 378                 return;
 379         }
 380
 381         /* update attribute */
 382         pattrib = &pmgntframe->attrib;
 383         update_mgntframe_attrib23a(padapter, pattrib);
 384
 385         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 386
 387         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 388         pwlanhdr = (struct ieee80211_hdr *)pframe;
 389
 390         fctrl = &pwlanhdr->frame_control;
 391         *fctrl = 0;
 392
 393         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
 394         memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
 395         memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
 396
 397         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 398         pmlmeext->mgnt_seq++;
 399         SetFrameSubType(pframe, WIFI_ACTION);
 400
 401         pframe += sizeof(struct ieee80211_hdr_3addr);
 402         pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
 403
 404         /* Build P2P action frame header */
 405         pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
 406         pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
 407                                      &pattrib->pktlen);
 408         pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
 409         pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
 410
 411         /* Add P2P IE header */
 412         /*      P2P OUI */
 413         p2pielen = 0;
 414         p2pie[ p2pielen++ ] = 0x50;
 415         p2pie[ p2pielen++ ] = 0x6F;
 416         p2pie[ p2pielen++ ] = 0x9A;
 417         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
 418
 419         /* Add Status attribute in P2P IE */
 420         p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
 421
 422         /* Add NoA attribute in P2P IE */
 423         noa_attr_content[0] = 0x1;/* index */
 424         noa_attr_content[1] = 0x0;/* CTWindow and OppPS Parameters */
 425
 426         /* todo: Notice of Absence Descriptor(s) */
 427
 428         p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_NOA, 2, noa_attr_content);
 429
 430         pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie,
 431                                &pattrib->pktlen);
 432
 433         pattrib->last_txcmdsz = pattrib->pktlen;
 434
 435         dump_mgntframe23a(padapter, pmgntframe);
 436 }
 437
 438 u32 build_beacon_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf)
 439 {
 440         u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
 441         u16 capability = 0;
 442         u32 len = 0, p2pielen = 0;
 443
 444         /*      P2P OUI */
 445         p2pielen = 0;
 446         p2pie[ p2pielen++ ] = 0x50;
 447         p2pie[ p2pielen++ ] = 0x6F;
 448         p2pie[ p2pielen++ ] = 0x9A;
 449         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
 450
 451         /*      According to the P2P Specification, the beacon frame should contain 3 P2P attributes */
 452         /*      1. P2P Capability */
 453         /*      2. P2P Device ID */
 454         /*      3. Notice of Absence (NOA) */
 455
 456         /*      P2P Capability ATTR */
 457         /*      Type: */
 458         /*      Length: */
 459         /*      Value: */
 460         /*      Device Capability Bitmap, 1 byte */
 461         /*      Be able to participate in additional P2P Groups and */
 462         /*      support the P2P Invitation Procedure */
 463         /*      Group Capability Bitmap, 1 byte */
 464         capability = P2P_DEVCAP_INVITATION_PROC|P2P_DEVCAP_CLIENT_DISCOVERABILITY;
 465         capability |=  ((P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS) << 8);
 466         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
 467                 capability |= (P2P_GRPCAP_GROUP_FORMATION<<8);
 468
 469         capability = cpu_to_le16(capability);
 470
 471         p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8*)&capability);
 472
 473         /*  P2P Device ID ATTR */
 474         p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_DEVICE_ID, ETH_ALEN, pwdinfo->device_addr);
 475
 476         /*  Notice of Absence ATTR */
 477         /*      Type: */
 478         /*      Length: */
 479         /*      Value: */
 480
 481         /* go_add_noa_attr(pwdinfo); */
 482
 483         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
 484
 485         return len;
 486 }
 487
 488 #ifdef CONFIG_8723AU_P2P
 489 u32 build_beacon_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
 490 {
 491         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
 492         u32 len = 0, wfdielen = 0;
 493         struct rtw_adapter *padapter = pwdinfo->padapter;
 494         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
 495         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
 496
 497         /*      WFD OUI */
 498         wfdielen = 0;
 499         wfdie[ wfdielen++ ] = 0x50;
 500         wfdie[ wfdielen++ ] = 0x6F;
 501         wfdie[ wfdielen++ ] = 0x9A;
 502         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
 503
 504         /*      Commented by Albert 20110812 */
 505         /*      According to the WFD Specification, the beacon frame should contain 4 WFD attributes */
 506         /*      1. WFD Device Information */
 507         /*      2. Associated BSSID */
 508         /*      3. Coupled Sink Information */
 509
 510         /*      WFD Device Information ATTR */
 511         /*      Type: */
 512         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
 513
 514         /*      Length: */
 515         /*      Note: In the WFD specification, the size of length field is 2. */
 516         put_unaligned_be16(0x0006, wfdie + wfdielen);
 517         wfdielen += 2;
 518
 519         /*      Value1: */
 520         /*      WFD device information */
 521
 522         if (P2P_ROLE_GO == pwdinfo->role)
 523         {
 524                 if (is_any_client_associated(pwdinfo->padapter))
 525                 {
 526                         /*      WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) */
 527                         put_unaligned_be16(pwfd_info->wfd_device_type |
 528                                            WFD_DEVINFO_WSD, wfdie + wfdielen);
 529                 }
 530                 else
 531                 {
 532                         /*      WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
 533                         put_unaligned_be16(pwfd_info->wfd_device_type |
 534                                            WFD_DEVINFO_SESSION_AVAIL |
 535                                            WFD_DEVINFO_WSD, wfdie + wfdielen);
 536                 }
 537
 538         }
 539         else
 540         {
 541                 /*      WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
 542                 put_unaligned_be16(pwfd_info->wfd_device_type |
 543                                    WFD_DEVINFO_SESSION_AVAIL |
 544                                    WFD_DEVINFO_WSD, wfdie + wfdielen);
 545         }
 546
 547         wfdielen += 2;
 548
 549         /*      Value2: */
 550         /*      Session Management Control Port */
 551         /*      Default TCP port for RTSP messages is 554 */
 552         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
 553         wfdielen += 2;
 554
 555         /*      Value3: */
 556         /*      WFD Device Maximum Throughput */
 557         /*      300Mbps is the maximum throughput */
 558         put_unaligned_be16(300, wfdie + wfdielen);
 559         wfdielen += 2;
 560
 561         /*      Associated BSSID ATTR */
 562         /*      Type: */
 563         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
 564
 565         /*      Length: */
 566         /*      Note: In the WFD specification, the size of length field is 2. */
 567         put_unaligned_be16(0x0006, wfdie + wfdielen);
 568         wfdielen += 2;
 569
 570         /*      Value: */
 571         /*      Associated BSSID */
 572         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
 573         {
 574                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
 575         }
 576         else
 577         {
 578                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
 579         }
 580
 581         wfdielen += ETH_ALEN;
 582
 583         /*      Coupled Sink Information ATTR */
 584         /*      Type: */
 585         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
 586
 587         /*      Length: */
 588         /*      Note: In the WFD specification, the size of length field is 2. */
 589         put_unaligned_be16(0x0007, wfdie + wfdielen);
 590         wfdielen += 2;
 591
 592         /*      Value: */
 593         /*      Coupled Sink Status bitmap */
 594         /*      Not coupled/available for Coupling */
 595         wfdie[ wfdielen++ ] = 0;
 596         /*   MAC Addr. */
 597         wfdie[ wfdielen++ ] = 0;
 598         wfdie[ wfdielen++ ] = 0;
 599         wfdie[ wfdielen++ ] = 0;
 600         wfdie[ wfdielen++ ] = 0;
 601         wfdie[ wfdielen++ ] = 0;
 602         wfdie[ wfdielen++ ] = 0;
 603
 604         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
 605
 606         return len;
 607 }
 608
 609 u32 build_probe_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
 610 {
 611         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
 612         u32 len = 0, wfdielen = 0;
 613         struct rtw_adapter *padapter = pwdinfo->padapter;
 614         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
 615         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
 616
 617         /*      WFD OUI */
 618         wfdielen = 0;
 619         wfdie[ wfdielen++ ] = 0x50;
 620         wfdie[ wfdielen++ ] = 0x6F;
 621         wfdie[ wfdielen++ ] = 0x9A;
 622         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
 623
 624         /*      Commented by Albert 20110812 */
 625         /*      According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
 626         /*      1. WFD Device Information */
 627         /*      2. Associated BSSID */
 628         /*      3. Coupled Sink Information */
 629
 630         /*      WFD Device Information ATTR */
 631         /*      Type: */
 632         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
 633
 634         /*      Length: */
 635         /*      Note: In the WFD specification, the size of length field is 2. */
 636         put_unaligned_be16(0x0006, wfdie + wfdielen);
 637         wfdielen += 2;
 638
 639         /*      Value1: */
 640         /*      WFD device information */
 641
 642         if (1 == pwdinfo->wfd_tdls_enable)
 643         {
 644                 /*      WFD primary sink + available for WFD session + WiFi TDLS mode + WSC (WFD Service Discovery) */
 645                 put_unaligned_be16(pwfd_info->wfd_device_type |
 646                                    WFD_DEVINFO_SESSION_AVAIL |
 647                                    WFD_DEVINFO_WSD |
 648                                    WFD_DEVINFO_PC_TDLS, wfdie + wfdielen);
 649         }
 650         else
 651         {
 652                 /*      WFD primary sink + available for WFD session + WiFi Direct mode + WSC (WFD Service Discovery) */
 653                 put_unaligned_be16(pwfd_info->wfd_device_type |
 654                                    WFD_DEVINFO_SESSION_AVAIL |
 655                                    WFD_DEVINFO_WSD, wfdie + wfdielen);
 656         }
 657
 658         wfdielen += 2;
 659
 660         /*      Value2: */
 661         /*      Session Management Control Port */
 662         /*      Default TCP port for RTSP messages is 554 */
 663         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
 664         wfdielen += 2;
 665
 666         /*      Value3: */
 667         /*      WFD Device Maximum Throughput */
 668         /*      300Mbps is the maximum throughput */
 669         put_unaligned_be16(300, wfdie + wfdielen);
 670         wfdielen += 2;
 671
 672         /*      Associated BSSID ATTR */
 673         /*      Type: */
 674         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
 675
 676         /*      Length: */
 677         /*      Note: In the WFD specification, the size of length field is 2. */
 678         put_unaligned_be16(0x0006, wfdie + wfdielen);
 679         wfdielen += 2;
 680
 681         /*      Value: */
 682         /*      Associated BSSID */
 683         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
 684         {
 685                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
 686         }
 687         else
 688         {
 689                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
 690         }
 691
 692         wfdielen += ETH_ALEN;
 693
 694         /*      Coupled Sink Information ATTR */
 695         /*      Type: */
 696         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
 697
 698         /*      Length: */
 699         /*      Note: In the WFD specification, the size of length field is 2. */
 700         put_unaligned_be16(0x0007, wfdie + wfdielen);
 701         wfdielen += 2;
 702
 703         /*      Value: */
 704         /*      Coupled Sink Status bitmap */
 705         /*      Not coupled/available for Coupling */
 706         wfdie[ wfdielen++ ] = 0;
 707         /*   MAC Addr. */
 708         wfdie[ wfdielen++ ] = 0;
 709         wfdie[ wfdielen++ ] = 0;
 710         wfdie[ wfdielen++ ] = 0;
 711         wfdie[ wfdielen++ ] = 0;
 712         wfdie[ wfdielen++ ] = 0;
 713         wfdie[ wfdielen++ ] = 0;
 714
 715         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
 716
 717         return len;
 718 }
 719
 720 u32 build_probe_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 tunneled)
 721 {
 722         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
 723         u32 len = 0, wfdielen = 0;
 724         struct rtw_adapter *padapter = pwdinfo->padapter;
 725         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
 726         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
 727
 728         /*      WFD OUI */
 729         wfdielen = 0;
 730         wfdie[ wfdielen++ ] = 0x50;
 731         wfdie[ wfdielen++ ] = 0x6F;
 732         wfdie[ wfdielen++ ] = 0x9A;
 733         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
 734
 735         /*      Commented by Albert 20110812 */
 736         /*      According to the WFD Specification, the probe response frame should contain 4 WFD attributes */
 737         /*      1. WFD Device Information */
 738         /*      2. Associated BSSID */
 739         /*      3. Coupled Sink Information */
 740         /*      4. WFD Session Information */
 741
 742         /*      WFD Device Information ATTR */
 743         /*      Type: */
 744         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
 745
 746         /*      Length: */
 747         /*      Note: In the WFD specification, the size of length field is 2. */
 748         put_unaligned_be16(0x0006, wfdie + wfdielen);
 749         wfdielen += 2;
 750
 751         /*      Value1: */
 752         /*      WFD device information */
 753         /*      WFD primary sink + available for WFD session + WiFi Direct mode */
 754
 755         if (true == pwdinfo->session_available)
 756         {
 757                 if (P2P_ROLE_GO == pwdinfo->role)
 758                 {
 759                         if (is_any_client_associated(pwdinfo->padapter))
 760                         {
 761                                 if (pwdinfo->wfd_tdls_enable)
 762                                 {
 763                                         /*      TDLS mode + WSD (WFD Service Discovery) */
 764                                         put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
 765                                 }
 766                                 else
 767                                 {
 768                                         /*      WiFi Direct mode + WSD (WFD Service Discovery) */
 769                                         put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
 770                                 }
 771                         }
 772                         else
 773                         {
 774                                 if (pwdinfo->wfd_tdls_enable)
 775                                 {
 776                                         /*      available for WFD session + TDLS mode + WSD (WFD Service Discovery) */
 777                                         put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
 778                                 }
 779                                 else
 780                                 {
 781                                         /*      available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
 782                                         put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
 783                                 }
 784                         }
 785                 }
 786                 else
 787                 {
 788                         if (pwdinfo->wfd_tdls_enable)
 789                         {
 790                                 /*      available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
 791                                 put_unaligned_be16(pwfd_info->wfd_device_type |
 792                                                    WFD_DEVINFO_SESSION_AVAIL |
 793                                                    WFD_DEVINFO_WSD |
 794                                                    WFD_DEVINFO_PC_TDLS |
 795                                                    WFD_DEVINFO_HDCP_SUPPORT,
 796                                                    wfdie + wfdielen);
 797                         }
 798                         else
 799                         {
 800
 801                                 /*      available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
 802                                 put_unaligned_be16(pwfd_info->wfd_device_type |
 803                                                    WFD_DEVINFO_SESSION_AVAIL |
 804                                                    WFD_DEVINFO_WSD |
 805                                                    WFD_DEVINFO_HDCP_SUPPORT,
 806                                                    wfdie + wfdielen);
 807                         }
 808                 }
 809         }
 810         else
 811         {
 812                 if (pwdinfo->wfd_tdls_enable)
 813                 {
 814                         put_unaligned_be16(pwfd_info->wfd_device_type |
 815                                            WFD_DEVINFO_WSD |
 816                                            WFD_DEVINFO_PC_TDLS |
 817                                            WFD_DEVINFO_HDCP_SUPPORT,
 818                                            wfdie + wfdielen);
 819                 }
 820                 else
 821                 {
 822                         put_unaligned_be16(pwfd_info->wfd_device_type |
 823                                            WFD_DEVINFO_WSD |
 824                                            WFD_DEVINFO_HDCP_SUPPORT,
 825                                            wfdie + wfdielen);
 826                 }
 827
 828         }
 829
 830         wfdielen += 2;
 831
 832         /*      Value2: */
 833         /*      Session Management Control Port */
 834         /*      Default TCP port for RTSP messages is 554 */
 835         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
 836         wfdielen += 2;
 837
 838         /*      Value3: */
 839         /*      WFD Device Maximum Throughput */
 840         /*      300Mbps is the maximum throughput */
 841         put_unaligned_be16(300, wfdie + wfdielen);
 842         wfdielen += 2;
 843
 844         /*      Associated BSSID ATTR */
 845         /*      Type: */
 846         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
 847
 848         /*      Length: */
 849         /*      Note: In the WFD specification, the size of length field is 2. */
 850         put_unaligned_be16(0x0006, wfdie + wfdielen);
 851         wfdielen += 2;
 852
 853         /*      Value: */
 854         /*      Associated BSSID */
 855         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
 856         {
 857                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
 858         }
 859         else
 860         {
 861                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
 862         }
 863
 864         wfdielen += ETH_ALEN;
 865
 866         /*      Coupled Sink Information ATTR */
 867         /*      Type: */
 868         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
 869
 870         /*      Length: */
 871         /*      Note: In the WFD specification, the size of length field is 2. */
 872         put_unaligned_be16(0x0007, wfdie + wfdielen);
 873         wfdielen += 2;
 874
 875         /*      Value: */
 876         /*      Coupled Sink Status bitmap */
 877         /*      Not coupled/available for Coupling */
 878         wfdie[ wfdielen++ ] = 0;
 879         /*   MAC Addr. */
 880         wfdie[ wfdielen++ ] = 0;
 881         wfdie[ wfdielen++ ] = 0;
 882         wfdie[ wfdielen++ ] = 0;
 883         wfdie[ wfdielen++ ] = 0;
 884         wfdie[ wfdielen++ ] = 0;
 885         wfdie[ wfdielen++ ] = 0;
 886
 887         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
 888         {
 889                 /*      WFD Session Information ATTR */
 890                 /*      Type: */
 891                 wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
 892
 893                 /*      Length: */
 894                 /*      Note: In the WFD specification, the size of length field is 2. */
 895                 put_unaligned_be16(0x0000, wfdie + wfdielen);
 896                 wfdielen += 2;
 897
 898                 /*      Todo: to add the list of WFD device info descriptor in WFD group. */
 899
 900         }
 901
 902         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
 903
 904         return len;
 905 }
 906
 907 u32 build_assoc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
 908 {
 909         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
 910         u32 len = 0, wfdielen = 0;
 911         struct rtw_adapter                                      *padapter = NULL;
 912         struct mlme_priv                        *pmlmepriv = NULL;
 913         struct wifi_display_info                *pwfd_info = NULL;
 914
 915         /*      WFD OUI */
 916         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
 917         {
 918                 return 0;
 919         }
 920
 921         padapter = pwdinfo->padapter;
 922         pmlmepriv = &padapter->mlmepriv;
 923         pwfd_info = padapter->wdinfo.wfd_info;
 924
 925         wfdielen = 0;
 926         wfdie[ wfdielen++ ] = 0x50;
 927         wfdie[ wfdielen++ ] = 0x6F;
 928         wfdie[ wfdielen++ ] = 0x9A;
 929         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
 930
 931         /*      Commented by Albert 20110812 */
 932         /*      According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
 933         /*      1. WFD Device Information */
 934         /*      2. Associated BSSID */
 935         /*      3. Coupled Sink Information */
 936
 937         /*      WFD Device Information ATTR */
 938         /*      Type: */
 939         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
 940
 941         /*      Length: */
 942         /*      Note: In the WFD specification, the size of length field is 2. */
 943         put_unaligned_be16(0x0006, wfdie + wfdielen);
 944         wfdielen += 2;
 945
 946         /*      Value1: */
 947         /*      WFD device information */
 948         /*      WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
 949         put_unaligned_be16(pwfd_info->wfd_device_type |
 950                            WFD_DEVINFO_SESSION_AVAIL |
 951                            WFD_DEVINFO_WSD, wfdie + wfdielen);
 952         wfdielen += 2;
 953
 954         /*      Value2: */
 955         /*      Session Management Control Port */
 956         /*      Default TCP port for RTSP messages is 554 */
 957         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
 958         wfdielen += 2;
 959
 960         /*      Value3: */
 961         /*      WFD Device Maximum Throughput */
 962         /*      300Mbps is the maximum throughput */
 963         put_unaligned_be16(300, wfdie + wfdielen);
 964         wfdielen += 2;
 965
 966         /*      Associated BSSID ATTR */
 967         /*      Type: */
 968         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
 969
 970         /*      Length: */
 971         /*      Note: In the WFD specification, the size of length field is 2. */
 972         put_unaligned_be16(0x0006, wfdie + wfdielen);
 973         wfdielen += 2;
 974
 975         /*      Value: */
 976         /*      Associated BSSID */
 977         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
 978         {
 979                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
 980         }
 981         else
 982         {
 983                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
 984         }
 985
 986         wfdielen += ETH_ALEN;
 987
 988         /*      Coupled Sink Information ATTR */
 989         /*      Type: */
 990         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
 991
 992         /*      Length: */
 993         /*      Note: In the WFD specification, the size of length field is 2. */
 994         put_unaligned_be16(0x0007, wfdie + wfdielen);
 995         wfdielen += 2;
 996
 997         /*      Value: */
 998         /*      Coupled Sink Status bitmap */
 999         /*      Not coupled/available for Coupling */
1000         wfdie[ wfdielen++ ] = 0;
1001         /*   MAC Addr. */
1002         wfdie[ wfdielen++ ] = 0;
1003         wfdie[ wfdielen++ ] = 0;
1004         wfdie[ wfdielen++ ] = 0;
1005         wfdie[ wfdielen++ ] = 0;
1006         wfdie[ wfdielen++ ] = 0;
1007         wfdie[ wfdielen++ ] = 0;
1008
1009         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1010
1011         return len;
1012 }
1013
1014 u32 build_assoc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1015 {
1016         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1017         u32 len = 0, wfdielen = 0;
1018         struct rtw_adapter *padapter = pwdinfo->padapter;
1019         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1020         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1021
1022         /*      WFD OUI */
1023         wfdielen = 0;
1024         wfdie[ wfdielen++ ] = 0x50;
1025         wfdie[ wfdielen++ ] = 0x6F;
1026         wfdie[ wfdielen++ ] = 0x9A;
1027         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
1028
1029         /*      Commented by Albert 20110812 */
1030         /*      According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
1031         /*      1. WFD Device Information */
1032         /*      2. Associated BSSID */
1033         /*      3. Coupled Sink Information */
1034
1035         /*      WFD Device Information ATTR */
1036         /*      Type: */
1037         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1038
1039         /*      Length: */
1040         /*      Note: In the WFD specification, the size of length field is 2. */
1041         put_unaligned_be16(0x0006, wfdie + wfdielen);
1042         wfdielen += 2;
1043
1044         /*      Value1: */
1045         /*      WFD device information */
1046         /*      WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
1047         put_unaligned_be16(pwfd_info->wfd_device_type |
1048                            WFD_DEVINFO_SESSION_AVAIL |
1049                            WFD_DEVINFO_WSD, wfdie + wfdielen);
1050         wfdielen += 2;
1051
1052         /*      Value2: */
1053         /*      Session Management Control Port */
1054         /*      Default TCP port for RTSP messages is 554 */
1055         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
1056         wfdielen += 2;
1057
1058         /*      Value3: */
1059         /*      WFD Device Maximum Throughput */
1060         /*      300Mbps is the maximum throughput */
1061         put_unaligned_be16(300, wfdie + wfdielen);
1062         wfdielen += 2;
1063
1064         /*      Associated BSSID ATTR */
1065         /*      Type: */
1066         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1067
1068         /*      Length: */
1069         /*      Note: In the WFD specification, the size of length field is 2. */
1070         put_unaligned_be16(0x0006, wfdie + wfdielen);
1071         wfdielen += 2;
1072
1073         /*      Value: */
1074         /*      Associated BSSID */
1075         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
1076         {
1077                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
1078         }
1079         else
1080         {
1081                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
1082         }
1083
1084         wfdielen += ETH_ALEN;
1085
1086         /*      Coupled Sink Information ATTR */
1087         /*      Type: */
1088         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1089
1090         /*      Length: */
1091         /*      Note: In the WFD specification, the size of length field is 2. */
1092         put_unaligned_be16(0x0007, wfdie + wfdielen);
1093         wfdielen += 2;
1094
1095         /*      Value: */
1096         /*      Coupled Sink Status bitmap */
1097         /*      Not coupled/available for Coupling */
1098         wfdie[ wfdielen++ ] = 0;
1099         /*   MAC Addr. */
1100         wfdie[ wfdielen++ ] = 0;
1101         wfdie[ wfdielen++ ] = 0;
1102         wfdie[ wfdielen++ ] = 0;
1103         wfdie[ wfdielen++ ] = 0;
1104         wfdie[ wfdielen++ ] = 0;
1105         wfdie[ wfdielen++ ] = 0;
1106
1107         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1108
1109         return len;
1110 }
1111
1112 u32 build_nego_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1113 {
1114         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1115         u32 len = 0, wfdielen = 0;
1116         struct rtw_adapter *padapter = pwdinfo->padapter;
1117         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1118         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1119
1120         /*      WFD OUI */
1121         wfdielen = 0;
1122         wfdie[ wfdielen++ ] = 0x50;
1123         wfdie[ wfdielen++ ] = 0x6F;
1124         wfdie[ wfdielen++ ] = 0x9A;
1125         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
1126
1127         /*      Commented by Albert 20110825 */
1128         /*      According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
1129         /*      1. WFD Device Information */
1130         /*      2. Associated BSSID (Optional) */
1131         /*      3. Local IP Adress (Optional) */
1132
1133         /*      WFD Device Information ATTR */
1134         /*      Type: */
1135         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1136
1137         /*      Length: */
1138         /*      Note: In the WFD specification, the size of length field is 2. */
1139         put_unaligned_be16(0x0006, wfdie + wfdielen);
1140         wfdielen += 2;
1141
1142         /*      Value1: */
1143         /*      WFD device information */
1144         /*      WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */
1145         put_unaligned_be16(pwfd_info->wfd_device_type |
1146                            WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL,
1147                            wfdie + wfdielen);
1148         wfdielen += 2;
1149
1150         /*      Value2: */
1151         /*      Session Management Control Port */
1152         /*      Default TCP port for RTSP messages is 554 */
1153         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
1154         wfdielen += 2;
1155
1156         /*      Value3: */
1157         /*      WFD Device Maximum Throughput */
1158         /*      300Mbps is the maximum throughput */
1159         put_unaligned_be16(300, wfdie + wfdielen);
1160         wfdielen += 2;
1161
1162         /*      Associated BSSID ATTR */
1163         /*      Type: */
1164         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1165
1166         /*      Length: */
1167         /*      Note: In the WFD specification, the size of length field is 2. */
1168         put_unaligned_be16(0x0006, wfdie + wfdielen);
1169         wfdielen += 2;
1170
1171         /*      Value: */
1172         /*      Associated BSSID */
1173         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
1174         {
1175                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
1176         }
1177         else
1178         {
1179                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
1180         }
1181
1182         wfdielen += ETH_ALEN;
1183
1184         /*      Coupled Sink Information ATTR */
1185         /*      Type: */
1186         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1187
1188         /*      Length: */
1189         /*      Note: In the WFD specification, the size of length field is 2. */
1190         put_unaligned_be16(0x0007, wfdie + wfdielen);
1191         wfdielen += 2;
1192
1193         /*      Value: */
1194         /*      Coupled Sink Status bitmap */
1195         /*      Not coupled/available for Coupling */
1196         wfdie[ wfdielen++ ] = 0;
1197         /*   MAC Addr. */
1198         wfdie[ wfdielen++ ] = 0;
1199         wfdie[ wfdielen++ ] = 0;
1200         wfdie[ wfdielen++ ] = 0;
1201         wfdie[ wfdielen++ ] = 0;
1202         wfdie[ wfdielen++ ] = 0;
1203         wfdie[ wfdielen++ ] = 0;
1204
1205         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1206
1207         return len;
1208 }
1209
1210 u32 build_nego_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1211 {
1212         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1213         u32 len = 0, wfdielen = 0;
1214         struct rtw_adapter *padapter = pwdinfo->padapter;
1215         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1216         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1217
1218         /*      WFD OUI */
1219         wfdielen = 0;
1220         wfdie[ wfdielen++ ] = 0x50;
1221         wfdie[ wfdielen++ ] = 0x6F;
1222         wfdie[ wfdielen++ ] = 0x9A;
1223         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
1224
1225         /*      Commented by Albert 20110825 */
1226         /*      According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
1227         /*      1. WFD Device Information */
1228         /*      2. Associated BSSID (Optional) */
1229         /*      3. Local IP Adress (Optional) */
1230
1231         /*      WFD Device Information ATTR */
1232         /*      Type: */
1233         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1234
1235         /*      Length: */
1236         /*      Note: In the WFD specification, the size of length field is 2. */
1237         put_unaligned_be16(0x0006, wfdie + wfdielen);
1238         wfdielen += 2;
1239
1240         /*      Value1: */
1241         /*      WFD device information */
1242         /*      WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */
1243         put_unaligned_be16(pwfd_info->wfd_device_type |
1244                            WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL,
1245                            wfdie + wfdielen);
1246         wfdielen += 2;
1247
1248         /*      Value2: */
1249         /*      Session Management Control Port */
1250         /*      Default TCP port for RTSP messages is 554 */
1251         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
1252         wfdielen += 2;
1253
1254         /*      Value3: */
1255         /*      WFD Device Maximum Throughput */
1256         /*      300Mbps is the maximum throughput */
1257         put_unaligned_be16(300, wfdie + wfdielen);
1258         wfdielen += 2;
1259
1260         /*      Associated BSSID ATTR */
1261         /*      Type: */
1262         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1263
1264         /*      Length: */
1265         /*      Note: In the WFD specification, the size of length field is 2. */
1266         put_unaligned_be16(0x0006, wfdie + wfdielen);
1267         wfdielen += 2;
1268
1269         /*      Value: */
1270         /*      Associated BSSID */
1271         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
1272         {
1273                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
1274         }
1275         else
1276         {
1277                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
1278         }
1279
1280         wfdielen += ETH_ALEN;
1281
1282         /*      Coupled Sink Information ATTR */
1283         /*      Type: */
1284         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1285
1286         /*      Length: */
1287         /*      Note: In the WFD specification, the size of length field is 2. */
1288         put_unaligned_be16(0x0007, wfdie + wfdielen);
1289         wfdielen += 2;
1290
1291         /*      Value: */
1292         /*      Coupled Sink Status bitmap */
1293         /*      Not coupled/available for Coupling */
1294         wfdie[ wfdielen++ ] = 0;
1295         /*   MAC Addr. */
1296         wfdie[ wfdielen++ ] = 0;
1297         wfdie[ wfdielen++ ] = 0;
1298         wfdie[ wfdielen++ ] = 0;
1299         wfdie[ wfdielen++ ] = 0;
1300         wfdie[ wfdielen++ ] = 0;
1301         wfdie[ wfdielen++ ] = 0;
1302
1303         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1304
1305         return len;
1306 }
1307
1308 u32 build_nego_confirm_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1309 {
1310         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1311         u32 len = 0, wfdielen = 0;
1312         struct rtw_adapter *padapter = pwdinfo->padapter;
1313         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1314         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1315
1316         /*      WFD OUI */
1317         wfdielen = 0;
1318         wfdie[ wfdielen++ ] = 0x50;
1319         wfdie[ wfdielen++ ] = 0x6F;
1320         wfdie[ wfdielen++ ] = 0x9A;
1321         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
1322
1323         /*      Commented by Albert 20110825 */
1324         /*      According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
1325         /*      1. WFD Device Information */
1326         /*      2. Associated BSSID (Optional) */
1327         /*      3. Local IP Adress (Optional) */
1328
1329         /*      WFD Device Information ATTR */
1330         /*      Type: */
1331         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1332
1333         /*      Length: */
1334         /*      Note: In the WFD specification, the size of length field is 2. */
1335         put_unaligned_be16(0x0006, wfdie + wfdielen);
1336         wfdielen += 2;
1337
1338         /*      Value1: */
1339         /*      WFD device information */
1340         /*      WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */
1341         put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD |
1342                            WFD_DEVINFO_SESSION_AVAIL, wfdie + wfdielen);
1343         wfdielen += 2;
1344
1345         /*      Value2: */
1346         /*      Session Management Control Port */
1347         /*      Default TCP port for RTSP messages is 554 */
1348         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
1349         wfdielen += 2;
1350
1351         /*      Value3: */
1352         /*      WFD Device Maximum Throughput */
1353         /*      300Mbps is the maximum throughput */
1354         put_unaligned_be16(300, wfdie + wfdielen);
1355         wfdielen += 2;
1356
1357         /*      Associated BSSID ATTR */
1358         /*      Type: */
1359         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1360
1361         /*      Length: */
1362         /*      Note: In the WFD specification, the size of length field is 2. */
1363         put_unaligned_be16(0x0006, wfdie + wfdielen);
1364         wfdielen += 2;
1365
1366         /*      Value: */
1367         /*      Associated BSSID */
1368         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
1369         {
1370                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
1371         }
1372         else
1373         {
1374                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
1375         }
1376
1377         wfdielen += ETH_ALEN;
1378
1379         /*      Coupled Sink Information ATTR */
1380         /*      Type: */
1381         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1382
1383         /*      Length: */
1384         /*      Note: In the WFD specification, the size of length field is 2. */
1385         put_unaligned_be16(0x0007, wfdie + wfdielen);
1386         wfdielen += 2;
1387
1388         /*      Value: */
1389         /*      Coupled Sink Status bitmap */
1390         /*      Not coupled/available for Coupling */
1391         wfdie[ wfdielen++ ] = 0;
1392         /*   MAC Addr. */
1393         wfdie[ wfdielen++ ] = 0;
1394         wfdie[ wfdielen++ ] = 0;
1395         wfdie[ wfdielen++ ] = 0;
1396         wfdie[ wfdielen++ ] = 0;
1397         wfdie[ wfdielen++ ] = 0;
1398         wfdie[ wfdielen++ ] = 0;
1399
1400         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1401
1402         return len;
1403 }
1404
1405 u32 build_invitation_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1406 {
1407         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1408         u32 len = 0, wfdielen = 0;
1409         struct rtw_adapter *padapter = pwdinfo->padapter;
1410         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1411         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1412
1413         /*      WFD OUI */
1414         wfdielen = 0;
1415         wfdie[ wfdielen++ ] = 0x50;
1416         wfdie[ wfdielen++ ] = 0x6F;
1417         wfdie[ wfdielen++ ] = 0x9A;
1418         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
1419
1420         /*      Commented by Albert 20110825 */
1421         /*      According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
1422         /*      1. WFD Device Information */
1423         /*      2. Associated BSSID (Optional) */
1424         /*      3. Local IP Adress (Optional) */
1425
1426         /*      WFD Device Information ATTR */
1427         /*      Type: */
1428         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1429
1430         /*      Length: */
1431         /*      Note: In the WFD specification, the size of length field is 2. */
1432         put_unaligned_be16(0x0006, wfdie + wfdielen);
1433         wfdielen += 2;
1434
1435         /*      Value1: */
1436         /*      WFD device information */
1437         /*      WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
1438         put_unaligned_be16(pwfd_info->wfd_device_type |
1439                            WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
1440                            wfdie + wfdielen);
1441         wfdielen += 2;
1442
1443         /*      Value2: */
1444         /*      Session Management Control Port */
1445         /*      Default TCP port for RTSP messages is 554 */
1446         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
1447         wfdielen += 2;
1448
1449         /*      Value3: */
1450         /*      WFD Device Maximum Throughput */
1451         /*      300Mbps is the maximum throughput */
1452         put_unaligned_be16(300, wfdie + wfdielen);
1453         wfdielen += 2;
1454
1455         /*      Associated BSSID ATTR */
1456         /*      Type: */
1457         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1458
1459         /*      Length: */
1460         /*      Note: In the WFD specification, the size of length field is 2. */
1461         put_unaligned_be16(0x0006, wfdie + wfdielen);
1462         wfdielen += 2;
1463
1464         /*      Value: */
1465         /*      Associated BSSID */
1466         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
1467         {
1468                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
1469         }
1470         else
1471         {
1472                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
1473         }
1474
1475         wfdielen += ETH_ALEN;
1476
1477         /*      Coupled Sink Information ATTR */
1478         /*      Type: */
1479         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1480
1481         /*      Length: */
1482         /*      Note: In the WFD specification, the size of length field is 2. */
1483         put_unaligned_be16(0x0007, wfdie + wfdielen);
1484         wfdielen += 2;
1485
1486         /*      Value: */
1487         /*      Coupled Sink Status bitmap */
1488         /*      Not coupled/available for Coupling */
1489         wfdie[ wfdielen++ ] = 0;
1490         /*   MAC Addr. */
1491         wfdie[ wfdielen++ ] = 0;
1492         wfdie[ wfdielen++ ] = 0;
1493         wfdie[ wfdielen++ ] = 0;
1494         wfdie[ wfdielen++ ] = 0;
1495         wfdie[ wfdielen++ ] = 0;
1496         wfdie[ wfdielen++ ] = 0;
1497
1498         if (P2P_ROLE_GO == pwdinfo->role)
1499         {
1500                 /*      WFD Session Information ATTR */
1501                 /*      Type: */
1502                 wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
1503
1504                 /*      Length: */
1505                 /*      Note: In the WFD specification, the size of length field is 2. */
1506                 put_unaligned_be16(0x0000, wfdie + wfdielen);
1507                 wfdielen += 2;
1508
1509                 /*      Todo: to add the list of WFD device info descriptor in WFD group. */
1510
1511         }
1512
1513         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1514
1515         return len;
1516 }
1517
1518 u32 build_invitation_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1519 {
1520         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1521         u32 len = 0, wfdielen = 0;
1522         struct rtw_adapter *padapter = pwdinfo->padapter;
1523         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1524         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1525
1526         /*      WFD OUI */
1527         wfdielen = 0;
1528         wfdie[ wfdielen++ ] = 0x50;
1529         wfdie[ wfdielen++ ] = 0x6F;
1530         wfdie[ wfdielen++ ] = 0x9A;
1531         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
1532
1533         /*      Commented by Albert 20110825 */
1534         /*      According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
1535         /*      1. WFD Device Information */
1536         /*      2. Associated BSSID (Optional) */
1537         /*      3. Local IP Adress (Optional) */
1538
1539         /*      WFD Device Information ATTR */
1540         /*      Type: */
1541         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1542
1543         /*      Length: */
1544         /*      Note: In the WFD specification, the size of length field is 2. */
1545         put_unaligned_be16(0x0006, wfdie + wfdielen);
1546         wfdielen += 2;
1547
1548         /*      Value1: */
1549         /*      WFD device information */
1550         /*      WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
1551         put_unaligned_be16(pwfd_info->wfd_device_type |
1552                            WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
1553                            wfdie + wfdielen);
1554         wfdielen += 2;
1555
1556         /*      Value2: */
1557         /*      Session Management Control Port */
1558         /*      Default TCP port for RTSP messages is 554 */
1559         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
1560         wfdielen += 2;
1561
1562         /*      Value3: */
1563         /*      WFD Device Maximum Throughput */
1564         /*      300Mbps is the maximum throughput */
1565         put_unaligned_be16(300, wfdie + wfdielen);
1566         wfdielen += 2;
1567
1568         /*      Associated BSSID ATTR */
1569         /*      Type: */
1570         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1571
1572         /*      Length: */
1573         /*      Note: In the WFD specification, the size of length field is 2. */
1574         put_unaligned_be16(0x0006, wfdie + wfdielen);
1575         wfdielen += 2;
1576
1577         /*      Value: */
1578         /*      Associated BSSID */
1579         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
1580         {
1581                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
1582         }
1583         else
1584         {
1585                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
1586         }
1587
1588         wfdielen += ETH_ALEN;
1589
1590         /*      Coupled Sink Information ATTR */
1591         /*      Type: */
1592         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1593
1594         /*      Length: */
1595         /*      Note: In the WFD specification, the size of length field is 2. */
1596         put_unaligned_be16(0x0007, wfdie + wfdielen);
1597         wfdielen += 2;
1598
1599         /*      Value: */
1600         /*      Coupled Sink Status bitmap */
1601         /*      Not coupled/available for Coupling */
1602         wfdie[ wfdielen++ ] = 0;
1603         /*   MAC Addr. */
1604         wfdie[ wfdielen++ ] = 0;
1605         wfdie[ wfdielen++ ] = 0;
1606         wfdie[ wfdielen++ ] = 0;
1607         wfdie[ wfdielen++ ] = 0;
1608         wfdie[ wfdielen++ ] = 0;
1609         wfdie[ wfdielen++ ] = 0;
1610
1611         if (P2P_ROLE_GO == pwdinfo->role)
1612         {
1613                 /*      WFD Session Information ATTR */
1614                 /*      Type: */
1615                 wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
1616
1617                 /*      Length: */
1618                 /*      Note: In the WFD specification, the size of length field is 2. */
1619                 put_unaligned_be16(0x0000, wfdie + wfdielen);
1620                 wfdielen += 2;
1621
1622                 /*      Todo: to add the list of WFD device info descriptor in WFD group. */
1623
1624         }
1625
1626         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1627
1628         return len;
1629 }
1630
1631 u32 build_provdisc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1632 {
1633         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1634         u32 len = 0, wfdielen = 0;
1635         struct rtw_adapter *padapter = pwdinfo->padapter;
1636         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1637         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1638
1639         /*      WFD OUI */
1640         wfdielen = 0;
1641         wfdie[ wfdielen++ ] = 0x50;
1642         wfdie[ wfdielen++ ] = 0x6F;
1643         wfdie[ wfdielen++ ] = 0x9A;
1644         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
1645
1646         /*      Commented by Albert 20110825 */
1647         /*      According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
1648         /*      1. WFD Device Information */
1649         /*      2. Associated BSSID (Optional) */
1650         /*      3. Local IP Adress (Optional) */
1651
1652         /*      WFD Device Information ATTR */
1653         /*      Type: */
1654         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1655
1656         /*      Length: */
1657         /*      Note: In the WFD specification, the size of length field is 2. */
1658         put_unaligned_be16(0x0006, wfdie + wfdielen);
1659         wfdielen += 2;
1660
1661         /*      Value1: */
1662         /*      WFD device information */
1663         /*      WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
1664         put_unaligned_be16(pwfd_info->wfd_device_type |
1665                            WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
1666                            wfdie + wfdielen);
1667         wfdielen += 2;
1668
1669         /*      Value2: */
1670         /*      Session Management Control Port */
1671         /*      Default TCP port for RTSP messages is 554 */
1672         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
1673         wfdielen += 2;
1674
1675         /*      Value3: */
1676         /*      WFD Device Maximum Throughput */
1677         /*      300Mbps is the maximum throughput */
1678         put_unaligned_be16(300, wfdie + wfdielen);
1679         wfdielen += 2;
1680
1681         /*      Associated BSSID ATTR */
1682         /*      Type: */
1683         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1684
1685         /*      Length: */
1686         /*      Note: In the WFD specification, the size of length field is 2. */
1687         put_unaligned_be16(0x0006, wfdie + wfdielen);
1688         wfdielen += 2;
1689
1690         /*      Value: */
1691         /*      Associated BSSID */
1692         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
1693         {
1694                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
1695         }
1696         else
1697         {
1698                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
1699         }
1700
1701         wfdielen += ETH_ALEN;
1702
1703         /*      Coupled Sink Information ATTR */
1704         /*      Type: */
1705         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1706
1707         /*      Length: */
1708         /*      Note: In the WFD specification, the size of length field is 2. */
1709         put_unaligned_be16(0x0007, wfdie + wfdielen);
1710         wfdielen += 2;
1711
1712         /*      Value: */
1713         /*      Coupled Sink Status bitmap */
1714         /*      Not coupled/available for Coupling */
1715         wfdie[ wfdielen++ ] = 0;
1716         /*   MAC Addr. */
1717         wfdie[ wfdielen++ ] = 0;
1718         wfdie[ wfdielen++ ] = 0;
1719         wfdie[ wfdielen++ ] = 0;
1720         wfdie[ wfdielen++ ] = 0;
1721         wfdie[ wfdielen++ ] = 0;
1722         wfdie[ wfdielen++ ] = 0;
1723
1724         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1725
1726         return len;
1727 }
1728
1729 u32 build_provdisc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
1730 {
1731         u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
1732         u32 len = 0, wfdielen = 0;
1733         struct rtw_adapter *padapter = pwdinfo->padapter;
1734         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1735         struct wifi_display_info*       pwfd_info = padapter->wdinfo.wfd_info;
1736
1737         /*      WFD OUI */
1738         wfdielen = 0;
1739         wfdie[ wfdielen++ ] = 0x50;
1740         wfdie[ wfdielen++ ] = 0x6F;
1741         wfdie[ wfdielen++ ] = 0x9A;
1742         wfdie[ wfdielen++ ] = 0x0A;     /*      WFA WFD v1.0 */
1743
1744         /*      Commented by Albert 20110825 */
1745         /*      According to the WFD Specification, the provision discovery response frame should contain 3 WFD attributes */
1746         /*      1. WFD Device Information */
1747         /*      2. Associated BSSID (Optional) */
1748         /*      3. Local IP Adress (Optional) */
1749
1750         /*      WFD Device Information ATTR */
1751         /*      Type: */
1752         wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
1753
1754         /*      Length: */
1755         /*      Note: In the WFD specification, the size of length field is 2. */
1756         put_unaligned_be16(0x0006, wfdie + wfdielen);
1757         wfdielen += 2;
1758
1759         /*      Value1: */
1760         /*      WFD device information */
1761         /*      WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
1762         put_unaligned_be16(pwfd_info->wfd_device_type |
1763                            WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
1764                            wfdie + wfdielen);
1765         wfdielen += 2;
1766
1767         /*      Value2: */
1768         /*      Session Management Control Port */
1769         /*      Default TCP port for RTSP messages is 554 */
1770         put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
1771         wfdielen += 2;
1772
1773         /*      Value3: */
1774         /*      WFD Device Maximum Throughput */
1775         /*      300Mbps is the maximum throughput */
1776         put_unaligned_be16(300, wfdie + wfdielen);
1777         wfdielen += 2;
1778
1779         /*      Associated BSSID ATTR */
1780         /*      Type: */
1781         wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
1782
1783         /*      Length: */
1784         /*      Note: In the WFD specification, the size of length field is 2. */
1785         put_unaligned_be16(0x0006, wfdie + wfdielen);
1786         wfdielen += 2;
1787
1788         /*      Value: */
1789         /*      Associated BSSID */
1790         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
1791         {
1792                 memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
1793         }
1794         else
1795         {
1796                 memset(wfdie + wfdielen, 0x00, ETH_ALEN);
1797         }
1798
1799         wfdielen += ETH_ALEN;
1800
1801         /*      Coupled Sink Information ATTR */
1802         /*      Type: */
1803         wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
1804
1805         /*      Length: */
1806         /*      Note: In the WFD specification, the size of length field is 2. */
1807         put_unaligned_be16(0x0007, wfdie + wfdielen);
1808         wfdielen += 2;
1809
1810         /*      Value: */
1811         /*      Coupled Sink Status bitmap */
1812         /*      Not coupled/available for Coupling */
1813         wfdie[ wfdielen++ ] = 0;
1814         /*   MAC Addr. */
1815         wfdie[ wfdielen++ ] = 0;
1816         wfdie[ wfdielen++ ] = 0;
1817         wfdie[ wfdielen++ ] = 0;
1818         wfdie[ wfdielen++ ] = 0;
1819         wfdie[ wfdielen++ ] = 0;
1820         wfdie[ wfdielen++ ] = 0;
1821
1822         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
1823
1824         return len;
1825 }
1826
1827 #endif /* CONFIG_8723AU_P2P */
1828
1829 u32 build_probe_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf)
1830 {
1831         u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
1832         u32 len = 0, p2pielen = 0;
1833
1834         /*      P2P OUI */
1835         p2pielen = 0;
1836         p2pie[ p2pielen++ ] = 0x50;
1837         p2pie[ p2pielen++ ] = 0x6F;
1838         p2pie[ p2pielen++ ] = 0x9A;
1839         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
1840
1841         /*      Commented by Albert 20100907 */
1842         /*      According to the P2P Specification, the probe response frame should contain 5 P2P attributes */
1843         /*      1. P2P Capability */
1844         /*      2. Extended Listen Timing */
1845         /*      3. Notice of Absence (NOA)      (Only GO needs this) */
1846         /*      4. Device Info */
1847         /*      5. Group Info   (Only GO need this) */
1848
1849         /*      P2P Capability ATTR */
1850         /*      Type: */
1851         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
1852
1853         /*      Length: */
1854         /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
1855         put_unaligned_le16(0x0002, p2pie + p2pielen);
1856         p2pielen += 2;
1857
1858         /*      Value: */
1859         /*      Device Capability Bitmap, 1 byte */
1860         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
1861
1862         /*      Group Capability Bitmap, 1 byte */
1863         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
1864         {
1865                 p2pie[ p2pielen ] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS);
1866
1867                 if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
1868                         p2pie[ p2pielen ] |= P2P_GRPCAP_GROUP_FORMATION;
1869
1870                 p2pielen++;
1871         }
1872         else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE))
1873         {
1874                 /*      Group Capability Bitmap, 1 byte */
1875                 if (pwdinfo->persistent_supported)
1876                         p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
1877                 else
1878                         p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
1879         }
1880
1881         /*      Extended Listen Timing ATTR */
1882         /*      Type: */
1883         p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
1884
1885         /*      Length: */
1886         /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004); */
1887         put_unaligned_le16(0x0004, p2pie + p2pielen);
1888         p2pielen += 2;
1889
1890         /*      Value: */
1891         /*      Availability Period */
1892         /* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
1893         put_unaligned_le16(0xFFFF, p2pie + p2pielen);
1894         p2pielen += 2;
1895
1896         /*      Availability Interval */
1897         /* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
1898         put_unaligned_le16(0xFFFF, p2pie + p2pielen);
1899         p2pielen += 2;
1900
1901         /*  Notice of Absence ATTR */
1902         /*      Type: */
1903         /*      Length: */
1904         /*      Value: */
1905         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
1906         {
1907                 /* go_add_noa_attr(pwdinfo); */
1908         }
1909
1910         /*      Device Info ATTR */
1911         /*      Type: */
1912         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
1913
1914         /*      Length: */
1915         /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
1916         /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
1917         /* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
1918         put_unaligned_le16(21 + pwdinfo->device_name_len, p2pie + p2pielen);
1919         p2pielen += 2;
1920
1921         /*      Value: */
1922         /*      P2P Device Address */
1923         memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
1924         p2pielen += ETH_ALEN;
1925
1926         /*      Config Method */
1927         /*      This field should be big endian. Noted by P2P specification. */
1928         /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm); */
1929         put_unaligned_be16(pwdinfo->supported_wps_cm, p2pie + p2pielen);
1930         p2pielen += 2;
1931
1932         /*      Primary Device Type */
1933         /*      Category ID */
1934         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
1935         put_unaligned_be16(WPS_PDT_CID_MULIT_MEDIA, p2pie + p2pielen);
1936         p2pielen += 2;
1937
1938         /*      OUI */
1939         /* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
1940         put_unaligned_be32(WPSOUI, p2pie + p2pielen);
1941         p2pielen += 4;
1942
1943         /*      Sub Category ID */
1944         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
1945         put_unaligned_be16(WPS_PDT_SCID_MEDIA_SERVER, p2pie + p2pielen);
1946         p2pielen += 2;
1947
1948         /*      Number of Secondary Device Types */
1949         p2pie[ p2pielen++ ] = 0x00;     /*      No Secondary Device Type List */
1950
1951         /*      Device Name */
1952         /*      Type: */
1953         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
1954         put_unaligned_be16(WPS_ATTR_DEVICE_NAME, p2pie + p2pielen);
1955         p2pielen += 2;
1956
1957         /*      Length: */
1958         /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
1959         put_unaligned_be16(pwdinfo->device_name_len, p2pie + p2pielen);
1960         p2pielen += 2;
1961
1962         /*      Value: */
1963         memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
1964         p2pielen += pwdinfo->device_name_len;
1965
1966         /*  Group Info ATTR */
1967         /*      Type: */
1968         /*      Length: */
1969         /*      Value: */
1970         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
1971         {
1972                 p2pielen += go_add_group_info_attr(pwdinfo, p2pie + p2pielen);
1973         }
1974
1975         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
1976
1977         return len;
1978 }
1979
1980 u32 build_prov_disc_request_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf, u8* pssid, u8 ussidlen, u8* pdev_raddr)
1981 {
1982         u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
1983         u32 len = 0, p2pielen = 0;
1984
1985         /*      P2P OUI */
1986         p2pielen = 0;
1987         p2pie[ p2pielen++ ] = 0x50;
1988         p2pie[ p2pielen++ ] = 0x6F;
1989         p2pie[ p2pielen++ ] = 0x9A;
1990         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
1991
1992         /*      Commented by Albert 20110301 */
1993         /*      According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes */
1994         /*      1. P2P Capability */
1995         /*      2. Device Info */
1996         /*      3. Group ID (When joining an operating P2P Group) */
1997
1998         /*      P2P Capability ATTR */
1999         /*      Type: */
2000         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
2001
2002         /*      Length: */
2003         /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
2004         put_unaligned_le16(0x0002, p2pie + p2pielen);
2005         p2pielen += 2;
2006
2007         /*      Value: */
2008         /*      Device Capability Bitmap, 1 byte */
2009         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
2010
2011         /*      Group Capability Bitmap, 1 byte */
2012         if (pwdinfo->persistent_supported)
2013                 p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
2014         else
2015                 p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
2016
2017         /*      Device Info ATTR */
2018         /*      Type: */
2019         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
2020
2021         /*      Length: */
2022         /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
2023         /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
2024         /* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
2025         put_unaligned_le16(21 + pwdinfo->device_name_len, p2pie + p2pielen);
2026         p2pielen += 2;
2027
2028         /*      Value: */
2029         /*      P2P Device Address */
2030         memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
2031         p2pielen += ETH_ALEN;
2032
2033         /*      Config Method */
2034         /*      This field should be big endian. Noted by P2P specification. */
2035         if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC)
2036         {
2037                 /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC); */
2038                 put_unaligned_be16(WPS_CONFIG_METHOD_PBC, p2pie + p2pielen);
2039         }
2040         else
2041         {
2042                 /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY); */
2043                 put_unaligned_be16(WPS_CONFIG_METHOD_DISPLAY, p2pie + p2pielen);
2044         }
2045
2046         p2pielen += 2;
2047
2048         /*      Primary Device Type */
2049         /*      Category ID */
2050         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
2051         put_unaligned_be16(WPS_PDT_CID_MULIT_MEDIA, p2pie + p2pielen);
2052         p2pielen += 2;
2053
2054         /*      OUI */
2055         /* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
2056         put_unaligned_be32(WPSOUI, p2pie + p2pielen);
2057         p2pielen += 4;
2058
2059         /*      Sub Category ID */
2060         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
2061         put_unaligned_be16(WPS_PDT_SCID_MEDIA_SERVER, p2pie + p2pielen);
2062         p2pielen += 2;
2063
2064         /*      Number of Secondary Device Types */
2065         p2pie[ p2pielen++ ] = 0x00;     /*      No Secondary Device Type List */
2066
2067         /*      Device Name */
2068         /*      Type: */
2069         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
2070         put_unaligned_be16(WPS_ATTR_DEVICE_NAME, p2pie + p2pielen);
2071         p2pielen += 2;
2072
2073         /*      Length: */
2074         /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
2075         put_unaligned_be16(pwdinfo->device_name_len, p2pie + p2pielen);
2076         p2pielen += 2;
2077
2078         /*      Value: */
2079         memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
2080         p2pielen += pwdinfo->device_name_len;
2081
2082         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT))
2083         {
2084                 /*      Added by Albert 2011/05/19 */
2085                 /*      In this case, the pdev_raddr is the device address of the group owner. */
2086
2087                 /*      P2P Group ID ATTR */
2088                 /*      Type: */
2089                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
2090
2091                 /*      Length: */
2092                 /* u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + ussidlen); */
2093                 put_unaligned_le16(ETH_ALEN + ussidlen, p2pie + p2pielen);
2094                 p2pielen += 2;
2095
2096                 /*      Value: */
2097                 memcpy(p2pie + p2pielen, pdev_raddr, ETH_ALEN);
2098                 p2pielen += ETH_ALEN;
2099
2100                 memcpy(p2pie + p2pielen, pssid, ussidlen);
2101                 p2pielen += ussidlen;
2102
2103         }
2104
2105         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
2106
2107         return len;
2108 }
2109
2110 u32 build_assoc_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 status_code)
2111 {
2112         u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
2113         u32 len = 0, p2pielen = 0;
2114
2115         /*      P2P OUI */
2116         p2pielen = 0;
2117         p2pie[ p2pielen++ ] = 0x50;
2118         p2pie[ p2pielen++ ] = 0x6F;
2119         p2pie[ p2pielen++ ] = 0x9A;
2120         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
2121
2122         /*  According to the P2P Specification, the Association response frame should contain 2 P2P attributes */
2123         /*      1. Status */
2124         /*      2. Extended Listen Timing (optional) */
2125
2126         /*      Status ATTR */
2127         p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status_code);
2128
2129         /*  Extended Listen Timing ATTR */
2130         /*      Type: */
2131         /*      Length: */
2132         /*      Value: */
2133
2134         pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
2135
2136         return len;
2137 }
2138
2139 u32 build_deauth_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf)
2140 {
2141         u32 len = 0;
2142
2143         return len;
2144 }
2145
2146 u32 process_probe_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
2147 {
2148         u8 *p;
2149         u32 ret = false;
2150         u8 *p2pie;
2151         u32     p2pielen = 0;
2152         int ssid_len = 0, rate_cnt = 0;
2153
2154         p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt,
2155                         len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_);
2156
2157         if (rate_cnt <= 4)
2158         {
2159                 int i, g_rate = 0;
2160
2161                 for (i = 0; i < rate_cnt; i++)
2162                 {
2163                         if (((*(p + 2 + i) & 0xff) != 0x02) &&
2164                                 ((*(p + 2 + i) & 0xff) != 0x04) &&
2165                                 ((*(p + 2 + i) & 0xff) != 0x0B) &&
2166                                 ((*(p + 2 + i) & 0xff) != 0x16))
2167                         {
2168                                 g_rate = 1;
2169                         }
2170                 }
2171
2172                 if (g_rate == 0)
2173                 {
2174                         /*      There is no OFDM rate included in SupportedRates IE of this probe request frame */
2175                         /*      The driver should response this probe request. */
2176                         return ret;
2177                 }
2178         }
2179         else
2180         {
2181                 /*      rate_cnt > 4 means the SupportRates IE contains the OFDM rate because the count of CCK rates are 4. */
2182                 /*      We should proceed the following check for this probe request. */
2183         }
2184
2185         /*      Added comments by Albert 20100906 */
2186         /*      There are several items we should check here. */
2187         /*      1. This probe request frame must contain the P2P IE. (Done) */
2188         /*      2. This probe request frame must contain the wildcard SSID. (Done) */
2189         /*      3. Wildcard BSSID. (Todo) */
2190         /*      4. Destination Address. (Done in mgt_dispatcher23a function) */
2191         /*      5. Requested Device Type in WSC IE. (Todo) */
2192         /*      6. Device ID attribute in P2P IE. (Todo) */
2193
2194         p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len,
2195                         len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_);
2196
2197         ssid_len &= 0xff;       /*      Just last 1 byte is valid for ssid len of the probe request */
2198         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2199         {
2200                 if ((p2pie = rtw_get_p2p_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_, NULL, &p2pielen)))
2201                 {
2202                         if ((p) && !memcmp((void *)(p+2), (void *)pwdinfo->p2p_wildcard_ssid, 7))
2203                         {
2204                                 /* todo: */
2205                                 /* Check Requested Device Type attributes in WSC IE. */
2206                                 /* Check Device ID attribute in P2P IE */
2207
2208                                 ret = true;
2209                         }
2210                         else if ((p != NULL) && (ssid_len == 0))
2211                         {
2212                                 ret = true;
2213                         }
2214                 }
2215                 else
2216                 {
2217                         /* non -p2p device */
2218                 }
2219
2220         }
2221
2222         return ret;
2223 }
2224
2225 u32 process_assoc_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len, struct sta_info *psta)
2226 {
2227         u8 status_code = P2P_STATUS_SUCCESS;
2228         u8 *pbuf, *pattr_content = NULL;
2229         u32 attr_contentlen = 0;
2230         u16 cap_attr = 0;
2231         unsigned short ie_offset;
2232         u8 * ies;
2233         u32 ies_len;
2234         u8 * p2p_ie;
2235         u32     p2p_ielen = 0;
2236         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe;
2237
2238         if (!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2239                 return P2P_STATUS_FAIL_REQUEST_UNABLE;
2240
2241         if (ieee80211_is_assoc_req(hdr->frame_control))
2242                 ie_offset = _ASOCREQ_IE_OFFSET_;
2243         else /*  WIFI_REASSOCREQ */
2244                 ie_offset = _REASOCREQ_IE_OFFSET_;
2245
2246         ies = pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset;
2247         ies_len = len - sizeof(struct ieee80211_hdr_3addr) - ie_offset;
2248
2249         p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
2250
2251         if (!p2p_ie)
2252         {
2253                 DBG_8723A("[%s] P2P IE not Found!!\n", __func__);
2254                 status_code =  P2P_STATUS_FAIL_INVALID_PARAM;
2255         }
2256         else
2257         {
2258                 DBG_8723A("[%s] P2P IE Found!!\n", __func__);
2259         }
2260
2261         while (p2p_ie)
2262         {
2263                 /* Check P2P Capability ATTR */
2264                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*) &attr_contentlen))
2265                 {
2266                         DBG_8723A("[%s] Got P2P Capability Attr!!\n", __func__);
2267                         cap_attr = le16_to_cpu(cap_attr);
2268                         psta->dev_cap = cap_attr&0xff;
2269                 }
2270
2271                 /* Check Extended Listen Timing ATTR */
2272
2273                 /* Check P2P Device Info ATTR */
2274                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint*)&attr_contentlen))
2275                 {
2276                         DBG_8723A("[%s] Got P2P DEVICE INFO Attr!!\n", __func__);
2277                         pattr_content = pbuf = kzalloc(attr_contentlen,
2278                                                        GFP_ATOMIC);
2279                         if (pattr_content) {
2280                                 u8 num_of_secdev_type;
2281                                 u16 dev_name_len;
2282
2283                                 rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, pattr_content, (uint*)&attr_contentlen);
2284
2285                                 memcpy(psta->dev_addr,  pattr_content, ETH_ALEN);/* P2P Device Address */
2286
2287                                 pattr_content += ETH_ALEN;
2288
2289                                 memcpy(&psta->config_methods, pattr_content, 2);/* Config Methods */
2290                                 psta->config_methods = be16_to_cpu(psta->config_methods);
2291
2292                                 pattr_content += 2;
2293
2294                                 memcpy(psta->primary_dev_type, pattr_content, 8);
2295
2296                                 pattr_content += 8;
2297
2298                                 num_of_secdev_type = *pattr_content;
2299                                 pattr_content += 1;
2300
2301                                 if (num_of_secdev_type == 0)
2302                                 {
2303                                         psta->num_of_secdev_type = 0;
2304                                 }
2305                                 else
2306                                 {
2307                                         u32 len;
2308
2309                                         psta->num_of_secdev_type = num_of_secdev_type;
2310
2311                                         len = (sizeof(psta->secdev_types_list)<(num_of_secdev_type*8)) ? (sizeof(psta->secdev_types_list)) : (num_of_secdev_type*8);
2312
2313                                         memcpy(psta->secdev_types_list, pattr_content, len);
2314
2315                                         pattr_content += (num_of_secdev_type*8);
2316                                 }
2317
2318                                 /* dev_name_len = attr_contentlen - ETH_ALEN - 2 - 8 - 1 - (num_of_secdev_type*8); */
2319                                 psta->dev_name_len = 0;
2320                                 if (WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(u16*)pattr_content))
2321                                 {
2322                                         dev_name_len = be16_to_cpu(*(u16*)(pattr_content+2));
2323
2324                                         psta->dev_name_len = (sizeof(psta->dev_name)<dev_name_len) ? sizeof(psta->dev_name):dev_name_len;
2325
2326                                         memcpy(psta->dev_name, pattr_content+4, psta->dev_name_len);
2327                                 }
2328
2329                                 kfree(pbuf);
2330
2331                         }
2332
2333                 }
2334
2335                 /* Get the next P2P IE */
2336                 p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
2337
2338         }
2339
2340         return status_code;
2341 }
2342
2343 u32 process_p2p_devdisc_req23a(struct wifidirect_info *pwdinfo, u8 *pframe,
2344                             uint len)
2345 {
2346         u8 *frame_body;
2347         u8 status, dialogToken;
2348         struct sta_info *psta = NULL;
2349         struct rtw_adapter *padapter = pwdinfo->padapter;
2350         struct sta_priv *pstapriv = &padapter->stapriv;
2351         u8 *p2p_ie;
2352         u32     p2p_ielen = 0;
2353         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe;
2354
2355         frame_body = (unsigned char *)
2356                 (pframe + sizeof(struct ieee80211_hdr_3addr));
2357
2358         dialogToken = frame_body[7];
2359         status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
2360
2361         if ((p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
2362                                      len - _PUBLIC_ACTION_IE_OFFSET_, NULL,
2363                                      &p2p_ielen))) {
2364                 u8 groupid[38] = { 0x00 };
2365                 u8 dev_addr[ETH_ALEN] = { 0x00 };
2366                 u32 attr_contentlen = 0;
2367
2368                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen,
2369                                              P2P_ATTR_GROUP_ID, groupid,
2370                                              &attr_contentlen)) {
2371                         if (!memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) &&
2372                             !memcmp(pwdinfo->p2p_group_ssid, groupid + ETH_ALEN,
2373                                     pwdinfo->p2p_group_ssid_len)) {
2374                                 attr_contentlen = 0;
2375
2376                                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen,
2377                                                              P2P_ATTR_DEVICE_ID,
2378                                                              dev_addr,
2379                                                              &attr_contentlen)) {
2380                                         struct list_head *phead, *plist, *ptmp;
2381
2382                                         spin_lock_bh(&pstapriv->asoc_list_lock);
2383                                         phead = &pstapriv->asoc_list;
2384
2385                                         list_for_each_safe(plist, ptmp, phead) {
2386                                                 psta = container_of(plist, struct sta_info, asoc_list);
2387
2388                                                 if (psta->is_p2p_device && (psta->dev_cap&P2P_DEVCAP_CLIENT_DISCOVERABILITY) &&
2389                                                    !memcmp(psta->dev_addr, dev_addr, ETH_ALEN))
2390                                                 {
2391                                                         /* spin_unlock_bh(&pstapriv->asoc_list_lock); */
2392                                                         /* issue GO Discoverability Request */
2393                                                         issue_group_disc_req(pwdinfo, psta->hwaddr);
2394                                                         /* spin_lock_bh(&pstapriv->asoc_list_lock); */
2395                                                         status = P2P_STATUS_SUCCESS;
2396                                                         break;
2397                                                 } else {
2398                                                         status = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
2399                                                 }
2400                                         }
2401                                         spin_unlock_bh(&pstapriv->asoc_list_lock);
2402                                 } else {
2403                                         status = P2P_STATUS_FAIL_INVALID_PARAM;
2404                                 }
2405                         } else {
2406                                 status = P2P_STATUS_FAIL_INVALID_PARAM;
2407                         }
2408                 }
2409         }
2410
2411         /* issue Device Discoverability Response */
2412         issue_p2p_devdisc_resp(pwdinfo, hdr->addr2, status, dialogToken);
2413
2414         return (status == P2P_STATUS_SUCCESS) ? true:false;
2415 }
2416
2417 u32 process_p2p_devdisc_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
2418 {
2419         return true;
2420 }
2421
2422 u8 process_p2p_provdisc_req23a(struct wifidirect_info *pwdinfo,
2423                             u8 *pframe, uint len)
2424 {
2425         u8 *frame_body;
2426         u8 *wpsie;
2427         u8 *ptr = NULL;
2428         uint    wps_ielen = 0, attr_contentlen = 0;
2429         u16     uconfig_method = 0;
2430         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe;
2431
2432         frame_body = (pframe + sizeof(struct ieee80211_hdr_3addr));
2433
2434         wpsie = rtw_get_wps_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
2435                                len - _PUBLIC_ACTION_IE_OFFSET_, NULL,
2436                                &wps_ielen);
2437         if (!wpsie)
2438                 goto out;
2439
2440         if (!rtw_get_wps_attr_content23a(wpsie, wps_ielen, WPS_ATTR_CONF_METHOD,
2441                                      (u8 *)&uconfig_method, &attr_contentlen))
2442                 goto out;
2443
2444         uconfig_method = be16_to_cpu(uconfig_method);
2445         ptr = pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req;
2446
2447         switch (uconfig_method)
2448         {
2449         case WPS_CM_DISPLYA:
2450                 memcpy(ptr, "dis", 3);
2451                 break;
2452
2453         case WPS_CM_LABEL:
2454                 memcpy(ptr, "lab", 3);
2455                 break;
2456
2457         case WPS_CM_PUSH_BUTTON:
2458                 memcpy(ptr, "pbc", 3);
2459                 break;
2460
2461         case WPS_CM_KEYPAD:
2462                 memcpy(ptr, "pad", 3);
2463                 break;
2464         }
2465         issue_p2p_provision_resp(pwdinfo, hdr->addr2, frame_body,
2466                                  uconfig_method);
2467
2468 out:
2469         DBG_8723A("[%s] config method = %s\n", __func__, ptr);
2470
2471         return true;
2472 }
2473
2474 u8 process_p2p_provdisc_resp23a(struct wifidirect_info *pwdinfo,  u8 *pframe)
2475 {
2476
2477         return true;
2478 }
2479
2480 static u8 rtw_p2p_get_peer_ch_list(struct wifidirect_info *pwdinfo, u8 *ch_content, u8 ch_cnt, u8 *peer_ch_list)
2481 {
2482         u8 i = 0, j = 0;
2483         u8 temp = 0;
2484         u8 ch_no = 0;
2485         ch_content += 3;
2486         ch_cnt -= 3;
2487
2488         while(ch_cnt > 0)
2489         {
2490                 ch_content += 1;
2491                 ch_cnt -= 1;
2492                 temp = *ch_content;
2493                 for (i = 0 ; i < temp ; i++, j++)
2494                 {
2495                         peer_ch_list[j] = *(ch_content + 1 + i);
2496                 }
2497                 ch_content += (temp + 1);
2498                 ch_cnt -= (temp + 1);
2499                 ch_no += temp ;
2500         }
2501
2502         return ch_no;
2503 }
2504
2505 static u8 rtw_p2p_ch_inclusion(struct mlme_ext_priv *pmlmeext, u8 *peer_ch_list, u8 peer_ch_num, u8 *ch_list_inclusioned)
2506 {
2507         int     i = 0, j = 0, temp = 0;
2508         u8 ch_no = 0;
2509
2510         for (i = 0; i < peer_ch_num; i++)
2511         {
2512                 for (j = temp; j < pmlmeext->max_chan_nums; j++)
2513                 {
2514                         if (*(peer_ch_list + i) == pmlmeext->channel_set[ j ].ChannelNum)
2515                         {
2516                                 ch_list_inclusioned[ ch_no++ ] = *(peer_ch_list + i);
2517                                 temp = j;
2518                                 break;
2519                         }
2520                 }
2521         }
2522
2523         return ch_no;
2524 }
2525
2526 u8 process_p2p_group_negotation_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
2527 {
2528         struct rtw_adapter *padapter = pwdinfo->padapter;
2529         u8      result = P2P_STATUS_SUCCESS;
2530         u32     p2p_ielen = 0, wps_ielen = 0;
2531         u8 * ies;
2532         u32 ies_len;
2533         u8 *p2p_ie;
2534         u8 *wpsie;
2535         u16             wps_devicepassword_id = 0x0000;
2536         uint    wps_devicepassword_id_len = 0;
2537 #ifdef CONFIG_8723AU_P2P
2538         u8      wfd_ie[MAX_WFD_IE_LEN] = { 0x00 };
2539         u32     wfd_ielen = 0;
2540 #endif /*  CONFIG_8723AU_P2P */
2541
2542         if ((wpsie = rtw_get_wps_ie23a(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen)))
2543         {
2544                 /*      Commented by Kurt 20120113 */
2545                 /*      If some device wants to do p2p handshake without sending prov_disc_req */
2546                 /*      We have to get peer_req_cm from here. */
2547                 if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3))
2548                 {
2549                         rtw_get_wps_attr_content23a(wpsie, wps_ielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len);
2550                         wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id);
2551
2552                         if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
2553                         {
2554                                 memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
2555                         }
2556                         else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
2557                         {
2558                                 memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
2559                         }
2560                         else
2561                         {
2562                                 memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
2563                         }
2564                 }
2565         }
2566         else
2567         {
2568                 DBG_8723A("[%s] WPS IE not Found!!\n", __func__);
2569                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
2570                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2571                 return result;
2572         }
2573
2574         if (pwdinfo->ui_got_wps_info == P2P_NO_WPSINFO)
2575         {
2576                 result = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
2577                 rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_INFOR_NOREADY);
2578                 return result;
2579         }
2580
2581         ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
2582         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
2583
2584         p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
2585
2586         if (!p2p_ie)
2587         {
2588                 DBG_8723A("[%s] P2P IE not Found!!\n", __func__);
2589                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
2590                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2591         }
2592
2593         while (p2p_ie)
2594         {
2595                 u8      attr_content = 0x00;
2596                 u32     attr_contentlen = 0;
2597                 u8      ch_content[50] = { 0x00 };
2598                 uint    ch_cnt = 0;
2599                 u8      peer_ch_list[50] = { 0x00 };
2600                 u8      peer_ch_num = 0;
2601                 u8      ch_list_inclusioned[50] = { 0x00 };
2602                 u8      ch_num_inclusioned = 0;
2603                 u16     cap_attr;
2604
2605                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING);
2606
2607                 /* Check P2P Capability ATTR */
2608                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen))
2609                         cap_attr = le16_to_cpu(cap_attr);
2610
2611                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen))
2612                 {
2613                         DBG_8723A("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01);
2614                         pwdinfo->peer_intent = attr_content;    /*      include both intent and tie breaker values. */
2615
2616                         if (pwdinfo->intent == (pwdinfo->peer_intent >> 1))
2617                         {
2618                                 /*      Try to match the tie breaker value */
2619                                 if (pwdinfo->intent == P2P_MAX_INTENT)
2620                                 {
2621                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
2622                                         result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
2623                                 }
2624                                 else
2625                                 {
2626                                         if (attr_content & 0x01)
2627                                         {
2628                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
2629                                         }
2630                                         else
2631                                         {
2632                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
2633                                         }
2634                                 }
2635                         }
2636                         else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1))
2637                         {
2638                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
2639                         }
2640                         else
2641                         {
2642                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
2643                         }
2644
2645                         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2646                         {
2647                                 /*      Store the group id information. */
2648                                 memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
2649                                 memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
2650                         }
2651                 }
2652
2653                 attr_contentlen = 0;
2654                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen))
2655                 {
2656                         if (attr_contentlen != ETH_ALEN)
2657                         {
2658                                 memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
2659                         }
2660                 }
2661
2662                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, ch_content, &ch_cnt))
2663                 {
2664                         peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, ch_content, ch_cnt, peer_ch_list);
2665                         ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
2666
2667                         if (ch_num_inclusioned == 0)
2668                         {
2669                                 DBG_8723A("[%s] No common channel in channel list!\n", __func__);
2670                                 result = P2P_STATUS_FAIL_NO_COMMON_CH;
2671                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2672                                 break;
2673                         }
2674
2675                         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2676                         {
2677                                 if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
2678                                                                                                 ch_list_inclusioned, ch_num_inclusioned))
2679                                 {
2680                                         {
2681                                                 u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
2682                                                 attr_contentlen = 0;
2683
2684                                                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
2685                                                 {
2686                                                         peer_operating_ch = operatingch_info[4];
2687                                                 }
2688
2689                                                 if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
2690                                                                                                                 ch_list_inclusioned, ch_num_inclusioned))
2691                                                 {
2692                                                         /**
2693                                                          *      Change our operating channel as peer's for compatibility.
2694                                                          */
2695                                                         pwdinfo->operating_channel = peer_operating_ch;
2696                                                         DBG_8723A("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
2697                                                 }
2698                                                 else
2699                                                 {
2700                                                         /*  Take first channel of ch_list_inclusioned as operating channel */
2701                                                         pwdinfo->operating_channel = ch_list_inclusioned[0];
2702                                                         DBG_8723A("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
2703                                                 }
2704                                         }
2705
2706                                 }
2707                         }
2708                 }
2709
2710                 /* Get the next P2P IE */
2711                 p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
2712         }
2713
2714 #ifdef CONFIG_8723AU_P2P
2715         /*      Added by Albert 20110823 */
2716         /*      Try to get the TCP port information when receiving the negotiation request. */
2717         if (rtw_get_wfd_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen))
2718         {
2719                 u8      attr_content[ 10 ] = { 0x00 };
2720                 u32     attr_contentlen = 0;
2721
2722                 DBG_8723A("[%s] WFD IE Found!!\n", __func__);
2723                 rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
2724                 if (attr_contentlen)
2725                 {
2726                         pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2);
2727                         DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport);
2728                 }
2729         }
2730 #endif /*  CONFIG_8723AU_P2P */
2731
2732         return result;
2733 }
2734
2735 u8 process_p2p_group_negotation_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
2736 {
2737         struct rtw_adapter *padapter = pwdinfo->padapter;
2738         u8      result = P2P_STATUS_SUCCESS;
2739         u32     p2p_ielen, wps_ielen;
2740         u8 * ies;
2741         u32 ies_len;
2742         u8 * p2p_ie;
2743 #ifdef CONFIG_8723AU_P2P
2744         u8      wfd_ie[MAX_WFD_IE_LEN] = { 0x00 };
2745         u32     wfd_ielen = 0;
2746 #endif /*  CONFIG_8723AU_P2P */
2747
2748         ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
2749         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
2750
2751         /*      Be able to know which one is the P2P GO and which one is P2P client. */
2752
2753         if (rtw_get_wps_ie23a(ies, ies_len, NULL, &wps_ielen))
2754         {
2755
2756         }
2757         else
2758         {
2759                 DBG_8723A("[%s] WPS IE not Found!!\n", __func__);
2760                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
2761                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2762         }
2763
2764         p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
2765         if (!p2p_ie)
2766         {
2767                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
2768                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2769                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
2770         }
2771         else
2772         {
2773
2774                 u8      attr_content = 0x00;
2775                 u32     attr_contentlen = 0;
2776                 u8      operatingch_info[5] = { 0x00 };
2777                 u8      groupid[ 38 ];
2778                 u16     cap_attr;
2779                 u8      peer_ch_list[50] = { 0x00 };
2780                 u8      peer_ch_num = 0;
2781                 u8      ch_list_inclusioned[50] = { 0x00 };
2782                 u8      ch_num_inclusioned = 0;
2783
2784                 while (p2p_ie)  /*      Found the P2P IE. */
2785                 {
2786
2787                         /* Check P2P Capability ATTR */
2788                         if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen))
2789                                 cap_attr = le16_to_cpu(cap_attr);
2790
2791                         rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
2792                         if (attr_contentlen == 1)
2793                         {
2794                                 DBG_8723A("[%s] Status = %d\n", __func__, attr_content);
2795                                 if (attr_content == P2P_STATUS_SUCCESS)
2796                                 {
2797                                         /*      Do nothing. */
2798                                 }
2799                                 else
2800                                 {
2801                                         if (P2P_STATUS_FAIL_INFO_UNAVAILABLE == attr_content) {
2802                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INFOR_NOREADY);
2803                                         } else {
2804                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2805                                         }
2806                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
2807                                         result = attr_content;
2808                                         break;
2809                                 }
2810                         }
2811
2812                         /*      Try to get the peer's interface address */
2813                         attr_contentlen = 0;
2814                         if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen))
2815                         {
2816                                 if (attr_contentlen != ETH_ALEN)
2817                                 {
2818                                         memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
2819                                 }
2820                         }
2821
2822                         /*      Try to get the peer's intent and tie breaker value. */
2823                         attr_content = 0x00;
2824                         attr_contentlen = 0;
2825                         if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen))
2826                         {
2827                                 DBG_8723A("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01);
2828                                 pwdinfo->peer_intent = attr_content;    /*      include both intent and tie breaker values. */
2829
2830                                 if (pwdinfo->intent == (pwdinfo->peer_intent >> 1))
2831                                 {
2832                                         /*      Try to match the tie breaker value */
2833                                         if (pwdinfo->intent == P2P_MAX_INTENT)
2834                                         {
2835                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
2836                                                 result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
2837                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2838                                         }
2839                                         else
2840                                         {
2841                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
2842                                                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
2843                                                 if (attr_content & 0x01)
2844                                                 {
2845                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
2846                                                 }
2847                                                 else
2848                                                 {
2849                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
2850                                                 }
2851                                         }
2852                                 }
2853                                 else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1))
2854                                 {
2855                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
2856                                         rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
2857                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
2858                                 }
2859                                 else
2860                                 {
2861                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
2862                                         rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
2863                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
2864                                 }
2865
2866                                 if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2867                                 {
2868                                         /*      Store the group id information. */
2869                                         memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
2870                                         memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
2871
2872                                 }
2873                         }
2874
2875                         /*      Try to get the operation channel information */
2876
2877                         attr_contentlen = 0;
2878                         if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
2879                         {
2880                                 DBG_8723A("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]);
2881                                 pwdinfo->peer_operating_ch = operatingch_info[4];
2882                         }
2883
2884                         /*      Try to get the channel list information */
2885                         if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, pwdinfo->channel_list_attr, &pwdinfo->channel_list_attr_len))
2886                         {
2887                                 DBG_8723A("[%s] channel list attribute found, len = %d\n", __func__,  pwdinfo->channel_list_attr_len);
2888
2889                                 peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len, peer_ch_list);
2890                                 ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
2891
2892                                 if (ch_num_inclusioned == 0)
2893                                 {
2894                                         DBG_8723A("[%s] No common channel in channel list!\n", __func__);
2895                                         result = P2P_STATUS_FAIL_NO_COMMON_CH;
2896                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
2897                                         break;
2898                                 }
2899
2900                                 if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
2901                                 {
2902                                         if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
2903                                                                                                         ch_list_inclusioned, ch_num_inclusioned))
2904                                         {
2905                                                 {
2906                                                         u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
2907                                                         attr_contentlen = 0;
2908
2909                                                         if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
2910                                                         {
2911                                                                 peer_operating_ch = operatingch_info[4];
2912                                                         }
2913
2914                                                         if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
2915                                                                                                                         ch_list_inclusioned, ch_num_inclusioned))
2916                                                         {
2917                                                                 /**
2918                                                                  *      Change our operating channel as peer's for compatibility.
2919                                                                  */
2920                                                                 pwdinfo->operating_channel = peer_operating_ch;
2921                                                                 DBG_8723A("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
2922                                                         }
2923                                                         else
2924                                                         {
2925                                                                 /*  Take first channel of ch_list_inclusioned as operating channel */
2926                                                                 pwdinfo->operating_channel = ch_list_inclusioned[0];
2927                                                                 DBG_8723A("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
2928                                                         }
2929                                                 }
2930
2931                                         }
2932                                 }
2933
2934                         }
2935                         else
2936                         {
2937                                 DBG_8723A("[%s] channel list attribute not found!\n", __func__);
2938                         }
2939
2940                         /*      Try to get the group id information if peer is GO */
2941                         attr_contentlen = 0;
2942                         memset(groupid, 0x00, 38);
2943                         if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen))
2944                         {
2945                                 memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
2946                                 memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
2947                         }
2948
2949                         /* Get the next P2P IE */
2950                         p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
2951                 }
2952
2953         }
2954
2955 #ifdef CONFIG_8723AU_P2P
2956         /*      Added by Albert 20111122 */
2957         /*      Try to get the TCP port information when receiving the negotiation response. */
2958         if (rtw_get_wfd_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen))
2959         {
2960                 u8      attr_content[ 10 ] = { 0x00 };
2961                 u32     attr_contentlen = 0;
2962
2963                 DBG_8723A("[%s] WFD IE Found!!\n", __func__);
2964                 rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
2965                 if (attr_contentlen)
2966                 {
2967                         pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2);
2968                         DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport);
2969                 }
2970         }
2971 #endif /*  CONFIG_8723AU_P2P */
2972
2973         return result;
2974 }
2975
2976 u8 process_p2p_group_negotation_confirm23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
2977 {
2978         u8 * ies;
2979         u32 ies_len;
2980         u8 * p2p_ie;
2981         u32     p2p_ielen = 0;
2982         u8      result = P2P_STATUS_SUCCESS;
2983         ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
2984         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
2985
2986         p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
2987         while (p2p_ie)  /*      Found the P2P IE. */
2988         {
2989                 u8      attr_content = 0x00, operatingch_info[5] = { 0x00 };
2990                 u8      groupid[ 38 ] = { 0x00 };
2991                 u32     attr_contentlen = 0;
2992
2993                 pwdinfo->negotiation_dialog_token = 1;
2994                 rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
2995                 if (attr_contentlen == 1)
2996                 {
2997                         DBG_8723A("[%s] Status = %d\n", __func__, attr_content);
2998                         result = attr_content;
2999
3000                         if (attr_content == P2P_STATUS_SUCCESS)
3001                         {
3002                                 del_timer_sync(&pwdinfo->restore_p2p_state_timer);
3003
3004                                 /*      Commented by Albert 20100911 */
3005                                 /*      Todo: Need to handle the case which both Intents are the same. */
3006                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
3007                                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
3008                                 if ((pwdinfo->intent) > (pwdinfo->peer_intent >> 1))
3009                                 {
3010                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
3011                                 }
3012                                 else if ((pwdinfo->intent) < (pwdinfo->peer_intent >> 1))
3013                                 {
3014                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
3015                                 }
3016                                 else
3017                                 {
3018                                         /*      Have to compare the Tie Breaker */
3019                                         if (pwdinfo->peer_intent & 0x01)
3020                                         {
3021                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
3022                                         }
3023                                         else
3024                                         {
3025                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
3026                                         }
3027                                 }
3028                         }
3029                         else
3030                         {
3031                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
3032                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
3033                                 break;
3034                         }
3035                 }
3036
3037                 /*      Try to get the group id information */
3038                 attr_contentlen = 0;
3039                 memset(groupid, 0x00, 38);
3040                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen))
3041                 {
3042                         DBG_8723A("[%s] Ssid = %s, ssidlen = %zu\n", __func__, &groupid[ETH_ALEN], strlen(&groupid[ETH_ALEN]));
3043                         memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
3044                         memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
3045                 }
3046
3047                 attr_contentlen = 0;
3048                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
3049                 {
3050                         DBG_8723A("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]);
3051                         pwdinfo->peer_operating_ch = operatingch_info[4];
3052                 }
3053
3054                 /* Get the next P2P IE */
3055                 p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
3056
3057         }
3058
3059         return result;
3060 }
3061
3062 u8 process_p2p_presence_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
3063 {
3064         u8 *frame_body;
3065         u8 dialogToken = 0;
3066         u8 status = P2P_STATUS_SUCCESS;
3067         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe;
3068
3069         frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
3070
3071         dialogToken = frame_body[6];
3072
3073         /* todo: check NoA attribute */
3074
3075         issue_p2p_presence_resp(pwdinfo, hdr->addr2, status, dialogToken);
3076
3077         return true;
3078 }
3079
3080 static void find_phase_handler(struct rtw_adapter *padapter)
3081 {
3082         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3083         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
3084         struct cfg80211_ssid ssid;
3085         u8                                      _status = 0;
3086
3087
3088
3089         memset((unsigned char*)&ssid, 0, sizeof(struct cfg80211_ssid));
3090         memcpy(ssid.ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN);
3091         ssid.ssid_len = P2P_WILDCARD_SSID_LEN;
3092
3093         rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
3094
3095         spin_lock_bh(&pmlmepriv->lock);
3096         _status = rtw_sitesurvey_cmd23a(padapter, &ssid, 1, NULL, 0);
3097         spin_unlock_bh(&pmlmepriv->lock);
3098
3099
3100 }
3101
3102 void p2p_concurrent_handler(struct rtw_adapter* padapter);
3103
3104 static void restore_p2p_state_handler(struct rtw_adapter *padapter)
3105 {
3106         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3107
3108         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
3109                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
3110
3111         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
3112
3113         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
3114                 /*      In the P2P client mode, the driver should not switch back to its listen channel */
3115                 /*      because this P2P client should stay at the operating channel of P2P GO. */
3116                 set_channel_bwmode23a(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
3117         }
3118 }
3119
3120 static void pre_tx_invitereq_handler(struct rtw_adapter *padapter)
3121 {
3122         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3123         u8      val8 = 1;
3124
3125         set_channel_bwmode23a(padapter, pwdinfo->invitereq_info.peer_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
3126         padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3127         issue23a_probereq_p2p(padapter, NULL);
3128         mod_timer(&pwdinfo->pre_tx_scan_timer,
3129                   jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT));
3130
3131
3132 }
3133
3134 static void pre_tx_provdisc_handler(struct rtw_adapter *padapter)
3135 {
3136         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3137         u8      val8 = 1;
3138
3139
3140         set_channel_bwmode23a(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
3141         rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3142         issue23a_probereq_p2p(padapter, NULL);
3143         mod_timer(&pwdinfo->pre_tx_scan_timer,
3144                   jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT));
3145
3146
3147 }
3148
3149 static void pre_tx_negoreq_handler(struct rtw_adapter *padapter)
3150 {
3151         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3152         u8      val8 = 1;
3153
3154
3155         set_channel_bwmode23a(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
3156         rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
3157         issue23a_probereq_p2p(padapter, NULL);
3158         mod_timer(&pwdinfo->pre_tx_scan_timer,
3159                   jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT));
3160
3161
3162 }
3163
3164 static void ro_ch_handler(struct rtw_adapter *padapter)
3165 {
3166         struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
3167         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
3168         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
3169
3170         if (pcfg80211_wdinfo->restore_channel != pmlmeext->cur_channel) {
3171                 if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED))
3172                         pmlmeext->cur_channel = pcfg80211_wdinfo->restore_channel;
3173
3174                 set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
3175                                       HAL_PRIME_CHNL_OFFSET_DONT_CARE,
3176                                       HT_CHANNEL_WIDTH_20);
3177         }
3178
3179         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
3180
3181         pcfg80211_wdinfo->is_ro_ch = false;
3182
3183         DBG_8723A("cfg80211_remain_on_channel_expired\n");
3184
3185         rtw_cfg80211_remain_on_channel_expired(padapter,
3186                 pcfg80211_wdinfo->remain_on_ch_cookie,
3187                 &pcfg80211_wdinfo->remain_on_ch_channel,
3188                 pcfg80211_wdinfo->remain_on_ch_type, GFP_KERNEL);
3189 }
3190
3191 static void ro_ch_timer_process (unsigned long data)
3192 {
3193         struct rtw_adapter *adapter = (struct rtw_adapter *)data;
3194
3195         p2p_protocol_wk_cmd23a(adapter, P2P_RO_CH_WK);
3196 }
3197
3198 #ifdef CONFIG_8723AU_P2P
3199 void rtw_append_wfd_ie(struct rtw_adapter *padapter, u8 *buf, u32* len)
3200 {
3201         unsigned char   *frame_body;
3202         u8 category, action, OUI_Subtype, dialogToken = 0;
3203         u32     wfdielen = 0;
3204
3205         frame_body = (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr));
3206         category = frame_body[0];
3207
3208         if (category == WLAN_CATEGORY_PUBLIC) {
3209                 action = frame_body[1];
3210                 if (action == ACT_PUBLIC_VENDOR &&
3211                     !memcmp(frame_body+2, P2P_OUI23A, 4)) {
3212                         OUI_Subtype = frame_body[6];
3213                         dialogToken = frame_body[7];
3214                         switch (OUI_Subtype)/* OUI Subtype */ {
3215                         case P2P_GO_NEGO_REQ:
3216                                 wfdielen = build_nego_req_wfd_ie(&padapter->wdinfo, buf + (*len));
3217                                 (*len) += wfdielen;
3218                                 break;
3219                         case P2P_GO_NEGO_RESP:
3220                                 wfdielen = build_nego_resp_wfd_ie(&padapter->wdinfo, buf + (*len));
3221                                 (*len) += wfdielen;
3222                                 break;
3223                         case P2P_GO_NEGO_CONF:
3224                                 wfdielen = build_nego_confirm_wfd_ie(&padapter->wdinfo, buf + (*len));
3225                                 (*len) += wfdielen;
3226                                 break;
3227                         case P2P_INVIT_REQ:
3228                                 wfdielen = build_invitation_req_wfd_ie(&padapter->wdinfo, buf + (*len));
3229                                 (*len) += wfdielen;
3230                                 break;
3231                         case P2P_INVIT_RESP:
3232                                 wfdielen = build_invitation_resp_wfd_ie(&padapter->wdinfo, buf + (*len));
3233                                 (*len) += wfdielen;
3234                                 break;
3235                         case P2P_DEVDISC_REQ:
3236                                 break;
3237                         case P2P_DEVDISC_RESP:
3238                                 break;
3239                         case P2P_PROVISION_DISC_REQ:
3240                                 wfdielen = build_provdisc_req_wfd_ie(&padapter->wdinfo, buf + (*len));
3241                                 (*len) += wfdielen;
3242                                 break;
3243                         case P2P_PROVISION_DISC_RESP:
3244                                 wfdielen = build_provdisc_resp_wfd_ie(&padapter->wdinfo, buf + (*len));
3245                                 (*len) += wfdielen;
3246                                 break;
3247                         default:
3248                                 break;
3249                         }
3250                 }
3251         } else if (category == WLAN_CATEGORY_VENDOR_SPECIFIC) {
3252                 OUI_Subtype = frame_body[5];
3253                 dialogToken = frame_body[6];
3254         } else {
3255                 DBG_8723A("%s, action frame category =%d\n", __func__, category);
3256         }
3257 }
3258 #endif
3259
3260 int rtw_p2p_check_frames(struct rtw_adapter *padapter, const u8 *buf, u32 len, u8 tx)
3261 {
3262         int is_p2p_frame = (-1);
3263         unsigned char   *frame_body;
3264         u8 category, action, OUI_Subtype, dialogToken = 0;
3265         u8 *p2p_ie = NULL;
3266         uint p2p_ielen = 0;
3267         struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev);
3268
3269         frame_body = (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr));
3270         category = frame_body[0];
3271         /* just for check */
3272         if (category == WLAN_CATEGORY_PUBLIC)
3273         {
3274                 action = frame_body[1];
3275                 if (action == ACT_PUBLIC_VENDOR &&
3276                     !memcmp(frame_body+2, P2P_OUI23A, 4)) {
3277                         OUI_Subtype = frame_body[6];
3278                         dialogToken = frame_body[7];
3279                         is_p2p_frame = OUI_Subtype;
3280                         p2p_ie = rtw_get_p2p_ie23a(
3281                                 (u8 *)buf+sizeof(struct ieee80211_hdr_3addr)+_PUBLIC_ACTION_IE_OFFSET_,
3282                                 len-sizeof(struct ieee80211_hdr_3addr)-_PUBLIC_ACTION_IE_OFFSET_,
3283                                 NULL, &p2p_ielen);
3284
3285                         switch (OUI_Subtype) {/* OUI Subtype */
3286                         u8 *cont;
3287                         uint cont_len;
3288                         case P2P_GO_NEGO_REQ:
3289                                 DBG_8723A("RTW_%s:P2P_GO_NEGO_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
3290                                 break;
3291                         case P2P_GO_NEGO_RESP:
3292                                 cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
3293                                 DBG_8723A("RTW_%s:P2P_GO_NEGO_RESP, dialogToken =%d, status:%d\n", (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1);
3294
3295                                 if (!tx)
3296                                         pwdev_priv->provdisc_req_issued = false;
3297                                 break;
3298                         case P2P_GO_NEGO_CONF:
3299                                 cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
3300                                 DBG_8723A("RTW_%s:P2P_GO_NEGO_CONF, dialogToken =%d, status:%d\n",
3301                                           (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1);
3302                                 break;
3303                         case P2P_INVIT_REQ:
3304                         {
3305                                 struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info;
3306                                 int flags = -1;
3307                                 int op_ch = 0;
3308
3309                                 if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, NULL, &cont_len)))
3310                                         flags = *cont;
3311                                 if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
3312                                         op_ch = *(cont+4);
3313
3314                                 if (invit_info->token != dialogToken)
3315                                         rtw_wdev_invit_info_init(invit_info);
3316
3317                                 invit_info->token = dialogToken;
3318                                 invit_info->flags = (flags ==-1) ? 0x0 : flags;
3319                                 invit_info->req_op_ch = op_ch;
3320
3321                                 DBG_8723A("RTW_%s:P2P_INVIT_REQ, dialogToken =%d, flags:0x%02x, op_ch:%d\n",
3322                                           (tx) ? "Tx" : "Rx", dialogToken, flags, op_ch);
3323                                 break;
3324                         }
3325                         case P2P_INVIT_RESP:
3326                         {
3327                                 struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info;
3328                                 int status = -1;
3329                                 int op_ch = 0;
3330
3331                                 if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len)))
3332                                         status = *cont;
3333                                 if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
3334                                         op_ch = *(cont+4);
3335
3336                                 if (invit_info->token != dialogToken) {
3337                                         rtw_wdev_invit_info_init(invit_info);
3338                                 } else {
3339                                         invit_info->token = 0;
3340                                         invit_info->status = (status ==-1) ? 0xff : status;
3341                                         invit_info->rsp_op_ch = op_ch;
3342                                 }
3343
3344                                 DBG_8723A("RTW_%s:P2P_INVIT_RESP, dialogToken =%d, status:%d, op_ch:%d\n",
3345                                           (tx == true)?"Tx":"Rx", dialogToken, status, op_ch);
3346                                 break;
3347                         }
3348                         case P2P_DEVDISC_REQ:
3349                                 DBG_8723A("RTW_%s:P2P_DEVDISC_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
3350                                 break;
3351                         case P2P_DEVDISC_RESP:
3352                                 cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
3353                                 DBG_8723A("RTW_%s:P2P_DEVDISC_RESP, dialogToken =%d, status:%d\n", (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1);
3354                                 break;
3355                         case P2P_PROVISION_DISC_REQ:
3356                         {
3357                                 size_t frame_body_len = len - sizeof(struct ieee80211_hdr_3addr);
3358                                 u8 *p2p_ie;
3359                                 uint p2p_ielen = 0;
3360                                 uint contentlen = 0;
3361
3362                                 DBG_8723A("RTW_%s:P2P_PROVISION_DISC_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
3363
3364                                 pwdev_priv->provdisc_req_issued = false;
3365
3366                                 p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
3367                                                            frame_body_len - _PUBLIC_ACTION_IE_OFFSET_,
3368                                                            NULL, &p2p_ielen);
3369                                 if (p2p_ie) {
3370                                         if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, NULL, &contentlen))
3371                                                 pwdev_priv->provdisc_req_issued = false;/* case: p2p_client join p2p GO */
3372                                         else
3373                                                 pwdev_priv->provdisc_req_issued = true;/* case: p2p_devices connection before Nego req. */
3374                                 }
3375                         }
3376                                 break;
3377                         case P2P_PROVISION_DISC_RESP:
3378                                 DBG_8723A("RTW_%s:P2P_PROVISION_DISC_RESP, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
3379                                 break;
3380                         default:
3381                                 DBG_8723A("RTW_%s:OUI_Subtype =%d, dialogToken =%d\n", (tx == true)?"Tx":"Rx", OUI_Subtype, dialogToken);
3382                                 break;
3383                         }
3384
3385                 }
3386
3387         }
3388         else if (category == WLAN_CATEGORY_VENDOR_SPECIFIC)
3389         {
3390                 OUI_Subtype = frame_body[5];
3391                 dialogToken = frame_body[6];
3392
3393                 is_p2p_frame = OUI_Subtype;
3394
3395                 switch (OUI_Subtype) {
3396                 case P2P_NOTICE_OF_ABSENCE:
3397                         DBG_8723A("RTW_%s:P2P_NOTICE_OF_ABSENCE, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
3398                         break;
3399                 case P2P_PRESENCE_REQUEST:
3400                         DBG_8723A("RTW_%s:P2P_PRESENCE_REQUEST, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
3401                         break;
3402                 case P2P_PRESENCE_RESPONSE:
3403                         DBG_8723A("RTW_%s:P2P_PRESENCE_RESPONSE, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
3404                         break;
3405                 case P2P_GO_DISC_REQUEST:
3406                         DBG_8723A("RTW_%s:P2P_GO_DISC_REQUEST, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
3407                         break;
3408                 default:
3409                         DBG_8723A("RTW_%s:OUI_Subtype =%d, dialogToken =%d\n", (tx == true)?"TX":"RX", OUI_Subtype, dialogToken);
3410                         break;
3411                 }
3412
3413         } else {
3414                 DBG_8723A("RTW_%s:action frame category =%d\n", (tx == true)?"TX":"RX", category);
3415         }
3416         return is_p2p_frame;
3417 }
3418
3419 void rtw_init_cfg80211_wifidirect_info(struct rtw_adapter *padapter)
3420 {
3421         struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
3422
3423         memset(pcfg80211_wdinfo, 0x00, sizeof(struct cfg80211_wifidirect_info));
3424
3425         setup_timer(&pcfg80211_wdinfo->remain_on_ch_timer,
3426                     ro_ch_timer_process, (unsigned long)padapter);
3427 }
3428
3429 void p2p_protocol_wk_hdl23a(struct rtw_adapter *padapter, int intCmdType)
3430 {
3431         switch (intCmdType) {
3432         case P2P_FIND_PHASE_WK:
3433                 find_phase_handler(padapter);
3434                 break;
3435         case P2P_RESTORE_STATE_WK:
3436                 restore_p2p_state_handler(padapter);
3437                 break;
3438         case P2P_PRE_TX_PROVDISC_PROCESS_WK:
3439                 pre_tx_provdisc_handler(padapter);
3440                 break;
3441         case P2P_PRE_TX_INVITEREQ_PROCESS_WK:
3442                 pre_tx_invitereq_handler(padapter);
3443                 break;
3444         case P2P_PRE_TX_NEGOREQ_PROCESS_WK:
3445                 pre_tx_negoreq_handler(padapter);
3446                 break;
3447         case P2P_RO_CH_WK:
3448                 ro_ch_handler(padapter);
3449                 break;
3450         }
3451 }
3452
3453 #ifdef CONFIG_8723AU_P2P
3454 void process_p2p_ps_ie23a(struct rtw_adapter *padapter, u8 *IEs, u32 IELength)
3455 {
3456         u8 * ies;
3457         u32 ies_len;
3458         u8 * p2p_ie;
3459         u32     p2p_ielen = 0;
3460         u8      noa_attr[MAX_P2P_IE_LEN] = { 0x00 };/*  NoA length should be n*(13) + 2 */
3461         u32     attr_contentlen = 0;
3462
3463         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
3464         u8      find_p2p = false, find_p2p_ps = false;
3465         u8      noa_offset, noa_num, noa_index;
3466
3467
3468
3469         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
3470         {
3471                 return;
3472         }
3473         if (IELength <= _BEACON_IE_OFFSET_)
3474                 return;
3475
3476         ies = IEs + _BEACON_IE_OFFSET_;
3477         ies_len = IELength - _BEACON_IE_OFFSET_;
3478
3479         p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
3480
3481         while(p2p_ie)
3482         {
3483                 find_p2p = true;
3484                 /*  Get Notice of Absence IE. */
3485                 if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_NOA, noa_attr, &attr_contentlen))
3486                 {
3487                         find_p2p_ps = true;
3488                         noa_index = noa_attr[0];
3489
3490                         if ((pwdinfo->p2p_ps_mode == P2P_PS_NONE) ||
3491                                 (noa_index != pwdinfo->noa_index))/*  if index change, driver should reconfigure related setting. */
3492                         {
3493                                 pwdinfo->noa_index = noa_index;
3494                                 pwdinfo->opp_ps = noa_attr[1] >> 7;
3495                                 pwdinfo->ctwindow = noa_attr[1] & 0x7F;
3496
3497                                 noa_offset = 2;
3498                                 noa_num = 0;
3499                                 /*  NoA length should be n*(13) + 2 */
3500                                 if (attr_contentlen > 2)
3501                                 {
3502                                         while(noa_offset < attr_contentlen)
3503                                         {
3504                                                 /* memcpy(&wifidirect_info->noa_count[noa_num], &noa_attr[noa_offset], 1); */
3505                                                 pwdinfo->noa_count[noa_num] = noa_attr[noa_offset];
3506                                                 noa_offset += 1;
3507
3508                                                 memcpy(&pwdinfo->noa_duration[noa_num], &noa_attr[noa_offset], 4);
3509                                                 noa_offset += 4;
3510
3511                                                 memcpy(&pwdinfo->noa_interval[noa_num], &noa_attr[noa_offset], 4);
3512                                                 noa_offset += 4;
3513
3514                                                 memcpy(&pwdinfo->noa_start_time[noa_num], &noa_attr[noa_offset], 4);
3515                                                 noa_offset += 4;
3516
3517                                                 noa_num++;
3518                                         }
3519                                 }
3520                                 pwdinfo->noa_num = noa_num;
3521
3522                                 if (pwdinfo->opp_ps == 1)
3523                                 {
3524                                         pwdinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
3525                                         /*  driver should wait LPS for entering CTWindow */
3526                                         if (padapter->pwrctrlpriv.bFwCurrentInPSMode == true)
3527                                         {
3528                                                 p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 1);
3529                                         }
3530                                 }
3531                                 else if (pwdinfo->noa_num > 0)
3532                                 {
3533                                         pwdinfo->p2p_ps_mode = P2P_PS_NOA;
3534                                         p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 1);
3535                                 }
3536                                 else if (pwdinfo->p2p_ps_mode > P2P_PS_NONE)
3537                                 {
3538                                         p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1);
3539                                 }
3540                         }
3541
3542                         break; /*  find target, just break. */
3543                 }
3544
3545                 /* Get the next P2P IE */
3546                 p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
3547
3548         }
3549
3550         if (find_p2p == true)
3551         {
3552                 if ((pwdinfo->p2p_ps_mode > P2P_PS_NONE) && (find_p2p_ps == false))
3553                 {
3554                         p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1);
3555                 }
3556         }
3557
3558
3559 }
3560
3561 void p2p_ps_wk_hdl23a(struct rtw_adapter *padapter, u8 p2p_ps_state)
3562 {
3563         struct pwrctrl_priv             *pwrpriv = &padapter->pwrctrlpriv;
3564         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
3565
3566
3567
3568         /*  Pre action for p2p state */
3569         switch (p2p_ps_state)
3570         {
3571                 case P2P_PS_DISABLE:
3572                         pwdinfo->p2p_ps_state = p2p_ps_state;
3573
3574                         rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
3575
3576                         pwdinfo->noa_index = 0;
3577                         pwdinfo->ctwindow = 0;
3578                         pwdinfo->opp_ps = 0;
3579                         pwdinfo->noa_num = 0;
3580                         pwdinfo->p2p_ps_mode = P2P_PS_NONE;
3581                         if (padapter->pwrctrlpriv.bFwCurrentInPSMode == true)
3582                         {
3583                                 if (pwrpriv->smart_ps == 0)
3584                                 {
3585                                         pwrpriv->smart_ps = 2;
3586                                         rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)&padapter->pwrctrlpriv.pwr_mode);
3587                                 }
3588                         }
3589                         break;
3590                 case P2P_PS_ENABLE:
3591                         if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
3592                                 pwdinfo->p2p_ps_state = p2p_ps_state;
3593
3594                                 if (pwdinfo->ctwindow > 0)
3595                                 {
3596                                         if (pwrpriv->smart_ps != 0)
3597                                         {
3598                                                 pwrpriv->smart_ps = 0;
3599                                                 DBG_8723A("%s(): Enter CTW, change SmartPS\n", __func__);
3600                                                 rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)&padapter->pwrctrlpriv.pwr_mode);
3601                                         }
3602                                 }
3603                                 rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
3604                         }
3605                         break;
3606                 case P2P_PS_SCAN:
3607                 case P2P_PS_SCAN_DONE:
3608                 case P2P_PS_ALLSTASLEEP:
3609                         if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
3610                                 pwdinfo->p2p_ps_state = p2p_ps_state;
3611                                 rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
3612                         }
3613                         break;
3614                 default:
3615                         break;
3616         }
3617
3618
3619 }
3620
3621 u8 p2p_ps_wk_cmd23a(struct rtw_adapter*padapter, u8 p2p_ps_state, u8 enqueue)
3622 {
3623         struct cmd_obj  *ph2c;
3624         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
3625         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
3626         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
3627         u8      res = _SUCCESS;
3628
3629
3630
3631         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
3632                 return res;
3633
3634         if (enqueue) {
3635                 ph2c = (struct cmd_obj *)kzalloc(sizeof(struct cmd_obj),
3636                                                  GFP_ATOMIC);
3637                 if (!ph2c) {
3638                         res = _FAIL;
3639                         goto exit;
3640                 }
3641
3642                 pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)
3643                         kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
3644                 if (pdrvextra_cmd_parm == NULL) {
3645                         kfree(ph2c);
3646                         res = _FAIL;
3647                         goto exit;
3648                 }
3649
3650                 pdrvextra_cmd_parm->ec_id = P2P_PS_WK_CID;
3651                 pdrvextra_cmd_parm->type_size = p2p_ps_state;
3652                 pdrvextra_cmd_parm->pbuf = NULL;
3653
3654                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
3655
3656                 res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
3657         }
3658         else
3659         {
3660                 p2p_ps_wk_hdl23a(padapter, p2p_ps_state);
3661         }
3662
3663 exit:
3664
3665
3666
3667         return res;
3668 }
3669 #endif /*  CONFIG_8723AU_P2P */
3670
3671 static void reset_ch_sitesurvey_timer_process(unsigned long data)
3672 {
3673         struct rtw_adapter *adapter = (struct rtw_adapter *)data;
3674         struct  wifidirect_info *pwdinfo = &adapter->wdinfo;
3675
3676         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
3677                 return;
3678
3679         DBG_8723A("[%s] In\n", __func__);
3680         /*      Reset the operation channel information */
3681         pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
3682         pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
3683 }
3684
3685 static void reset_ch_sitesurvey_timer_process2(unsigned long data)
3686 {
3687         struct rtw_adapter *adapter = (struct rtw_adapter *)data;
3688         struct wifidirect_info *pwdinfo = &adapter->wdinfo;
3689
3690         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
3691                 return;
3692
3693         DBG_8723A("[%s] In\n", __func__);
3694         /*      Reset the operation channel information */
3695         pwdinfo->p2p_info.operation_ch[0] = 0;
3696         pwdinfo->p2p_info.scan_op_ch_only = 0;
3697 }
3698
3699 static void restore_p2p_state_timer_process (unsigned long data)
3700 {
3701         struct rtw_adapter *adapter = (struct rtw_adapter *)data;
3702         struct  wifidirect_info         *pwdinfo = &adapter->wdinfo;
3703
3704         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
3705                 return;
3706
3707         p2p_protocol_wk_cmd23a(adapter, P2P_RESTORE_STATE_WK);
3708 }
3709
3710 static void pre_tx_scan_timer_process (unsigned long data)
3711 {
3712         struct rtw_adapter *adapter = (struct rtw_adapter *)data;
3713         struct wifidirect_info *pwdinfo = &adapter->wdinfo;
3714         struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
3715
3716         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
3717                 return;
3718
3719         spin_lock_bh(&pmlmepriv->lock);
3720
3721         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
3722         {
3723                 if (true == pwdinfo->tx_prov_disc_info.benable) /*      the provision discovery request frame is trigger to send or not */
3724                 {
3725                         p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_PROVDISC_PROCESS_WK);
3726                         /* issue23a_probereq_p2p(adapter, NULL); */
3727                         /* _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); */
3728                 }
3729         }
3730         else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
3731         {
3732                 if (true == pwdinfo->nego_req_info.benable)
3733                 {
3734                         p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_NEGOREQ_PROCESS_WK);
3735                 }
3736         }
3737         else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ))
3738         {
3739                 if (true == pwdinfo->invitereq_info.benable)
3740                 {
3741                         p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK);
3742                 }
3743         }
3744         else
3745         {
3746                 DBG_8723A("[%s] p2p_state is %d, ignore!!\n", __func__, rtw_p2p_state(pwdinfo));
3747         }
3748
3749         spin_unlock_bh(&pmlmepriv->lock);
3750 }
3751
3752 static void find_phase_timer_process (unsigned long data)
3753 {
3754         struct rtw_adapter *adapter = (struct rtw_adapter *)data;
3755         struct  wifidirect_info         *pwdinfo = &adapter->wdinfo;
3756
3757         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
3758                 return;
3759
3760         adapter->wdinfo.find_phase_state_exchange_cnt++;
3761
3762         p2p_protocol_wk_cmd23a(adapter, P2P_FIND_PHASE_WK);
3763 }
3764
3765 void reset_global_wifidirect_info23a(struct rtw_adapter *padapter)
3766 {
3767         struct wifidirect_info  *pwdinfo;
3768
3769         pwdinfo = &padapter->wdinfo;
3770         pwdinfo->persistent_supported = 0;
3771         pwdinfo->session_available = true;
3772         pwdinfo->wfd_tdls_enable = 0;
3773         pwdinfo->wfd_tdls_weaksec = 0;
3774 }
3775
3776 #ifdef CONFIG_8723AU_P2P
3777 int rtw_init_wifi_display_info(struct rtw_adapter* padapter)
3778 {
3779         int     res = _SUCCESS;
3780         struct wifi_display_info *pwfd_info = &padapter->wfd_info;
3781
3782         /*  Used in P2P and TDLS */
3783         pwfd_info->rtsp_ctrlport = 554;
3784         pwfd_info->peer_rtsp_ctrlport = 0;      /*      Reset to 0 */
3785         pwfd_info->wfd_enable = false;
3786         pwfd_info->wfd_device_type = WFD_DEVINFO_PSINK;
3787         pwfd_info->scan_result_type = SCAN_RESULT_P2P_ONLY;
3788
3789         /*  Used in P2P */
3790         pwfd_info->peer_session_avail = true;
3791         pwfd_info->wfd_pc = false;
3792
3793         /*  Used in TDLS */
3794         memset(pwfd_info->ip_address, 0x00, 4);
3795         memset(pwfd_info->peer_ip_address, 0x00, 4);
3796         return res;
3797 }
3798 #endif /* CONFIG_8723AU_P2P */
3799
3800 void rtw_init_wifidirect_timers23a(struct rtw_adapter* padapter)
3801 {
3802         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
3803
3804         setup_timer(&pwdinfo->find_phase_timer, find_phase_timer_process,
3805                     (unsigned long)padapter);
3806         setup_timer(&pwdinfo->restore_p2p_state_timer,
3807                     restore_p2p_state_timer_process, (unsigned long)padapter);
3808         setup_timer(&pwdinfo->pre_tx_scan_timer, pre_tx_scan_timer_process,
3809                     (unsigned long)padapter);
3810         setup_timer(&pwdinfo->reset_ch_sitesurvey,
3811                     reset_ch_sitesurvey_timer_process, (unsigned long)padapter);
3812         setup_timer(&pwdinfo->reset_ch_sitesurvey2,
3813                     reset_ch_sitesurvey_timer_process2,
3814                     (unsigned long)padapter);
3815 }
3816
3817 void rtw_init_wifidirect_addrs23a(struct rtw_adapter* padapter, u8 *dev_addr, u8 *iface_addr)
3818 {
3819 #ifdef CONFIG_8723AU_P2P
3820         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
3821
3822         /*init device&interface address */
3823         if (dev_addr) {
3824                 memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN);
3825         }
3826         if (iface_addr) {
3827                 memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN);
3828         }
3829 #endif
3830 }
3831
3832 void init_wifidirect_info23a(struct rtw_adapter *padapter, enum P2P_ROLE role)
3833 {
3834         struct wifidirect_info  *pwdinfo;
3835 #ifdef CONFIG_8723AU_P2P
3836         struct wifi_display_info        *pwfd_info = &padapter->wfd_info;
3837 #endif
3838
3839         pwdinfo = &padapter->wdinfo;
3840
3841         pwdinfo->padapter = padapter;
3842
3843         /*      1, 6, 11 are the social channel defined in the WiFi Direct specification. */
3844         pwdinfo->social_chan[0] = 1;
3845         pwdinfo->social_chan[1] = 6;
3846         pwdinfo->social_chan[2] = 11;
3847         pwdinfo->social_chan[3] = 0;    /*      channel 0 for scanning ending in site survey function. */
3848
3849         /*      Use the channel 11 as the listen channel */
3850         pwdinfo->listen_channel = 11;
3851
3852         if (role == P2P_ROLE_DEVICE)
3853         {
3854                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
3855                 rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
3856                 pwdinfo->intent = 1;
3857                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_LISTEN);
3858         }
3859         else if (role == P2P_ROLE_CLIENT)
3860         {
3861                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
3862                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
3863                 pwdinfo->intent = 1;
3864                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
3865         }
3866         else if (role == P2P_ROLE_GO)
3867         {
3868                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
3869                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
3870                 pwdinfo->intent = 15;
3871                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
3872         }
3873
3874 /*      Use the OFDM rate in the P2P probe response frame. (6(B), 9(B), 12, 18, 24, 36, 48, 54) */
3875         pwdinfo->support_rate[0] = 0x8c;        /*      6(B) */
3876         pwdinfo->support_rate[1] = 0x92;        /*      9(B) */
3877         pwdinfo->support_rate[2] = 0x18;        /*      12 */
3878         pwdinfo->support_rate[3] = 0x24;        /*      18 */
3879         pwdinfo->support_rate[4] = 0x30;        /*      24 */
3880         pwdinfo->support_rate[5] = 0x48;        /*      36 */
3881         pwdinfo->support_rate[6] = 0x60;        /*      48 */
3882         pwdinfo->support_rate[7] = 0x6c;        /*      54 */
3883
3884         memcpy((void*) pwdinfo->p2p_wildcard_ssid, "DIRECT-", 7);
3885
3886         memset(pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN);
3887         pwdinfo->device_name_len = 0;
3888
3889         memset(&pwdinfo->invitereq_info, 0x00, sizeof(struct tx_invite_req_info));
3890         pwdinfo->invitereq_info.token = 3;      /*      Token used for P2P invitation request frame. */
3891
3892         memset(&pwdinfo->inviteresp_info, 0x00, sizeof(struct tx_invite_resp_info));
3893         pwdinfo->inviteresp_info.token = 0;
3894
3895         pwdinfo->profileindex = 0;
3896         memset(&pwdinfo->profileinfo[ 0 ], 0x00, sizeof(struct profile_info) * P2P_MAX_PERSISTENT_GROUP_NUM);
3897
3898         rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
3899
3900         pwdinfo->listen_dwell = (u8) ((jiffies % 3) + 1);
3901         /* DBG_8723A("[%s] listen_dwell time is %d00ms\n", __func__, pwdinfo->listen_dwell); */
3902
3903         memset(&pwdinfo->tx_prov_disc_info, 0x00, sizeof(struct tx_provdisc_req_info));
3904         pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_NONE;
3905
3906         memset(&pwdinfo->nego_req_info, 0x00, sizeof(struct tx_nego_req_info));
3907
3908         pwdinfo->device_password_id_for_nego = WPS_DPID_PBC;
3909         pwdinfo->negotiation_dialog_token = 1;
3910
3911         memset(pwdinfo->nego_ssid, 0x00, IEEE80211_MAX_SSID_LEN);
3912         pwdinfo->nego_ssidlen = 0;
3913
3914         pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO;
3915 #ifdef CONFIG_8723AU_P2P
3916         pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY  | WPS_CONFIG_METHOD_PBC;
3917         pwdinfo->wfd_info = pwfd_info;
3918 #else
3919         pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD;
3920 #endif /* CONFIG_8723AU_P2P */
3921         pwdinfo->channel_list_attr_len = 0;
3922         memset(pwdinfo->channel_list_attr, 0x00, 100);
3923
3924         memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, 0x00, 4);
3925         memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, '0', 3);
3926         memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
3927         pwdinfo->wfd_tdls_enable = 0;
3928         memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
3929         memset(pwdinfo->p2p_peer_device_addr, 0x00, ETH_ALEN);
3930
3931         pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
3932         pwdinfo->rx_invitereq_info.operation_ch[1] = 0; /*      Used to indicate the scan end in site survey function */
3933         pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
3934         pwdinfo->p2p_info.operation_ch[0] = 0;
3935         pwdinfo->p2p_info.operation_ch[1] = 0;                  /*      Used to indicate the scan end in site survey function */
3936         pwdinfo->p2p_info.scan_op_ch_only = 0;
3937 }
3938
3939 int rtw_p2p_enable23a(struct rtw_adapter *padapter, enum P2P_ROLE role)
3940 {
3941         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
3942         int ret = _SUCCESS;
3943
3944         if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT ||
3945             role == P2P_ROLE_GO) {
3946                 /* leave IPS/Autosuspend */
3947                 if (_FAIL == rtw_pwr_wakeup(padapter)) {
3948                         ret = _FAIL;
3949                         goto exit;
3950                 }
3951
3952                 /*      Added by Albert 2011/03/22 */
3953                 /*      In the P2P mode, the driver should not support the b mode. */
3954                 /*      So, the Tx packet shouldn't use the CCK rate */
3955                 update_tx_basic_rate23a(padapter, WIRELESS_11AGN);
3956
3957                 /* Enable P2P function */
3958                 init_wifidirect_info23a(padapter, role);
3959
3960                 rtw_hal_set_odm_var23a(padapter, HAL_ODM_P2P_STATE, NULL, true);
3961                 #ifdef CONFIG_8723AU_P2P
3962                 rtw_hal_set_odm_var23a(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, true);
3963                 #endif
3964
3965         }
3966         else if (role == P2P_ROLE_DISABLE)
3967         {
3968                 if (_FAIL == rtw_pwr_wakeup(padapter)) {
3969                         ret = _FAIL;
3970                         goto exit;
3971                 }
3972
3973                 /* Disable P2P function */
3974                 if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
3975                 {
3976                         del_timer_sync(&pwdinfo->find_phase_timer);
3977                         del_timer_sync(&pwdinfo->restore_p2p_state_timer);
3978                         del_timer_sync(&pwdinfo->pre_tx_scan_timer);
3979                         del_timer_sync(&pwdinfo->reset_ch_sitesurvey);
3980                         del_timer_sync(&pwdinfo->reset_ch_sitesurvey2);
3981                         reset_ch_sitesurvey_timer_process((unsigned long)padapter);
3982                         reset_ch_sitesurvey_timer_process2((unsigned long)padapter);
3983                         rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
3984                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DISABLE);
3985                         memset(&pwdinfo->rx_prov_disc_info, 0x00, sizeof(struct rx_provdisc_req_info));
3986                 }
3987
3988                 rtw_hal_set_odm_var23a(padapter, HAL_ODM_P2P_STATE, NULL, false);
3989                 #ifdef CONFIG_8723AU_P2P
3990                 rtw_hal_set_odm_var23a(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, false);
3991                 #endif
3992
3993                 /* Restore to initial setting. */
3994                 update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode);
3995         }
3996
3997 exit:
3998         return ret;
3999 }
4000
4001 #endif /* CONFIG_8723AU_P2P */