drivers/staging/rtl8188eu/core/rtw_p2p.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  *
  19  ******************************************************************************/
  20 #define _RTW_P2P_C_
  21
  22 #include <drv_types.h>
  23 #include <rtw_p2p.h>
  24 #include <wifi.h>
  25
  26 #ifdef CONFIG_88EU_P2P
  27
  28 static int rtw_p2p_is_channel_list_ok(u8 desired_ch, u8 *ch_list, u8 ch_cnt)
  29 {
  30         int found = 0, i = 0;
  31
  32         for (i = 0; i < ch_cnt; i++) {
  33                 if (ch_list[i] == desired_ch) {
  34                         found = 1;
  35                         break;
  36                 }
  37         }
  38         return found;
  39 }
  40
  41 static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf)
  42 {
  43         struct list_head *phead, *plist;
  44         u32 len = 0;
  45         u16 attr_len = 0;
  46         u8 tmplen, *pdata_attr, *pstart, *pcur;
  47         struct sta_info *psta = NULL;
  48         struct adapter *padapter = pwdinfo->padapter;
  49         struct sta_priv *pstapriv = &padapter->stapriv;
  50
  51         DBG_88E("%s\n", __func__);
  52
  53         pdata_attr = rtw_zmalloc(MAX_P2P_IE_LEN);
  54
  55         pstart = pdata_attr;
  56         pcur = pdata_attr;
  57
  58         spin_lock_bh(&pstapriv->asoc_list_lock);
  59         phead = &pstapriv->asoc_list;
  60         plist = phead->next;
  61
  62         /* look up sta asoc_queue */
  63         while ((rtw_end_of_queue_search(phead, plist)) == false) {
  64                 psta = container_of(plist, struct sta_info, asoc_list);
  65
  66                 plist = plist->next;
  67
  68
  69                 if (psta->is_p2p_device) {
  70                         tmplen = 0;
  71
  72                         pcur++;
  73
  74                         /* P2P device address */
  75                         memcpy(pcur, psta->dev_addr, ETH_ALEN);
  76                         pcur += ETH_ALEN;
  77
  78                         /* P2P interface address */
  79                         memcpy(pcur, psta->hwaddr, ETH_ALEN);
  80                         pcur += ETH_ALEN;
  81
  82                         *pcur = psta->dev_cap;
  83                         pcur++;
  84
  85                         /* u16*)(pcur) = cpu_to_be16(psta->config_methods); */
  86                         RTW_PUT_BE16(pcur, psta->config_methods);
  87                         pcur += 2;
  88
  89                         memcpy(pcur, psta->primary_dev_type, 8);
  90                         pcur += 8;
  91
  92                         *pcur = psta->num_of_secdev_type;
  93                         pcur++;
  94
  95                         memcpy(pcur, psta->secdev_types_list, psta->num_of_secdev_type*8);
  96                         pcur += psta->num_of_secdev_type*8;
  97
  98                         if (psta->dev_name_len > 0) {
  99                                 /* u16*)(pcur) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
 100                                 RTW_PUT_BE16(pcur, WPS_ATTR_DEVICE_NAME);
 101                                 pcur += 2;
 102
 103                                 /* u16*)(pcur) = cpu_to_be16(psta->dev_name_len); */
 104                                 RTW_PUT_BE16(pcur, psta->dev_name_len);
 105                                 pcur += 2;
 106
 107                                 memcpy(pcur, psta->dev_name, psta->dev_name_len);
 108                                 pcur += psta->dev_name_len;
 109                         }
 110
 111
 112                         tmplen = (u8)(pcur-pstart);
 113
 114                         *pstart = (tmplen-1);
 115
 116                         attr_len += tmplen;
 117
 118                         /* pstart += tmplen; */
 119                         pstart = pcur;
 120                 }
 121         }
 122         spin_unlock_bh(&pstapriv->asoc_list_lock);
 123
 124         if (attr_len > 0)
 125                 len = rtw_set_p2p_attr_content(pbuf, P2P_ATTR_GROUP_INFO, attr_len, pdata_attr);
 126
 127         kfree(pdata_attr);
 128         return len;
 129 }
 130
 131 static void issue_group_disc_req(struct wifidirect_info *pwdinfo, u8 *da)
 132 {
 133         struct xmit_frame                       *pmgntframe;
 134         struct pkt_attrib                       *pattrib;
 135         unsigned char                                   *pframe;
 136         struct rtw_ieee80211_hdr        *pwlanhdr;
 137         __le16 *fctrl;
 138         struct adapter *padapter = pwdinfo->padapter;
 139         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 140         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 141         unsigned char category = RTW_WLAN_CATEGORY_P2P;/* P2P action frame */
 142         __be32  p2poui = cpu_to_be32(P2POUI);
 143         u8      oui_subtype = P2P_GO_DISC_REQUEST;
 144         u8      dialogToken = 0;
 145
 146         DBG_88E("[%s]\n", __func__);
 147
 148         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
 149         if (pmgntframe == NULL)
 150                 return;
 151
 152         /* update attribute */
 153         pattrib = &pmgntframe->attrib;
 154         update_mgntframe_attrib(padapter, pattrib);
 155
 156         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 157
 158         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 159         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 160
 161         fctrl = &(pwlanhdr->frame_ctl);
 162         *(fctrl) = 0;
 163
 164         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
 165         memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
 166         memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
 167
 168         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 169         pmlmeext->mgnt_seq++;
 170         SetFrameSubType(pframe, WIFI_ACTION);
 171
 172         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
 173         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
 174
 175         /* Build P2P action frame header */
 176         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
 177         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
 178         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
 179         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
 180
 181         /* there is no IE in this P2P action frame */
 182
 183         pattrib->last_txcmdsz = pattrib->pktlen;
 184
 185         dump_mgntframe(padapter, pmgntframe);
 186 }
 187
 188 static void issue_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
 189 {
 190         struct xmit_frame                       *pmgntframe;
 191         struct pkt_attrib                       *pattrib;
 192         unsigned char                                   *pframe;
 193         struct rtw_ieee80211_hdr        *pwlanhdr;
 194         __le16 *fctrl;
 195         struct adapter *padapter = pwdinfo->padapter;
 196         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 197         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 198         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
 199         u8                      action = P2P_PUB_ACTION_ACTION;
 200         __be32                  p2poui = cpu_to_be32(P2POUI);
 201         u8                      oui_subtype = P2P_DEVDISC_RESP;
 202         u8 p2pie[8] = { 0x00 };
 203         u32 p2pielen = 0;
 204
 205         DBG_88E("[%s]\n", __func__);
 206
 207         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
 208         if (pmgntframe == NULL)
 209                 return;
 210
 211         /* update attribute */
 212         pattrib = &pmgntframe->attrib;
 213         update_mgntframe_attrib(padapter, pattrib);
 214
 215         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 216
 217         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 218         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 219
 220         fctrl = &(pwlanhdr->frame_ctl);
 221         *(fctrl) = 0;
 222
 223         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
 224         memcpy(pwlanhdr->addr2, pwdinfo->device_addr, ETH_ALEN);
 225         memcpy(pwlanhdr->addr3, pwdinfo->device_addr, ETH_ALEN);
 226
 227         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 228         pmlmeext->mgnt_seq++;
 229         SetFrameSubType(pframe, WIFI_ACTION);
 230
 231         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
 232         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
 233
 234         /* Build P2P public action frame header */
 235         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
 236         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
 237         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
 238         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
 239         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
 240
 241
 242         /* Build P2P IE */
 243         /*      P2P OUI */
 244         p2pielen = 0;
 245         p2pie[p2pielen++] = 0x50;
 246         p2pie[p2pielen++] = 0x6F;
 247         p2pie[p2pielen++] = 0x9A;
 248         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
 249
 250         /*  P2P_ATTR_STATUS */
 251         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
 252
 253         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen);
 254
 255         pattrib->last_txcmdsz = pattrib->pktlen;
 256
 257         dump_mgntframe(padapter, pmgntframe);
 258 }
 259
 260 static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8 *raddr, u8 *frame_body, u16 config_method)
 261 {
 262         struct adapter *padapter = pwdinfo->padapter;
 263         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
 264         u8                      action = P2P_PUB_ACTION_ACTION;
 265         u8                      dialogToken = frame_body[7];    /*      The Dialog Token of provisioning discovery request frame. */
 266         __be32                  p2poui = cpu_to_be32(P2POUI);
 267         u8                      oui_subtype = P2P_PROVISION_DISC_RESP;
 268         u8                      wpsie[100] = { 0x00 };
 269         u8                      wpsielen = 0;
 270         struct xmit_frame                       *pmgntframe;
 271         struct pkt_attrib                       *pattrib;
 272         unsigned char                                   *pframe;
 273         struct rtw_ieee80211_hdr        *pwlanhdr;
 274         __le16 *fctrl;
 275         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 276         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 277
 278         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
 279         if (pmgntframe == NULL)
 280                 return;
 281
 282         /* update attribute */
 283         pattrib = &pmgntframe->attrib;
 284         update_mgntframe_attrib(padapter, pattrib);
 285
 286         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 287
 288         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 289         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 290
 291         fctrl = &(pwlanhdr->frame_ctl);
 292         *(fctrl) = 0;
 293
 294         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
 295         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
 296         memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
 297
 298         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 299         pmlmeext->mgnt_seq++;
 300         SetFrameSubType(pframe, WIFI_ACTION);
 301
 302         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
 303         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
 304
 305         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
 306         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
 307         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
 308         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
 309         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
 310
 311         wpsielen = 0;
 312         /*      WPS OUI */
 313         RTW_PUT_BE32(wpsie, WPSOUI);
 314         wpsielen += 4;
 315
 316         /*      Config Method */
 317         /*      Type: */
 318         RTW_PUT_BE16(wpsie + wpsielen, WPS_ATTR_CONF_METHOD);
 319         wpsielen += 2;
 320
 321         /*      Length: */
 322         RTW_PUT_BE16(wpsie + wpsielen, 0x0002);
 323         wpsielen += 2;
 324
 325         /*      Value: */
 326         RTW_PUT_BE16(wpsie + wpsielen, config_method);
 327         wpsielen += 2;
 328
 329         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
 330
 331         pattrib->last_txcmdsz = pattrib->pktlen;
 332
 333         dump_mgntframe(padapter, pmgntframe);
 334
 335         return;
 336 }
 337
 338 static void issue_p2p_presence_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
 339 {
 340         struct xmit_frame                       *pmgntframe;
 341         struct pkt_attrib                       *pattrib;
 342         unsigned char                                   *pframe;
 343         struct rtw_ieee80211_hdr        *pwlanhdr;
 344         __le16 *fctrl;
 345         struct adapter *padapter = pwdinfo->padapter;
 346         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
 347         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 348         unsigned char category = RTW_WLAN_CATEGORY_P2P;/* P2P action frame */
 349         __be32  p2poui = cpu_to_be32(P2POUI);
 350         u8      oui_subtype = P2P_PRESENCE_RESPONSE;
 351         u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
 352         u8 noa_attr_content[32] = { 0x00 };
 353         u32 p2pielen = 0;
 354
 355         DBG_88E("[%s]\n", __func__);
 356
 357         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
 358         if (pmgntframe == NULL)
 359                 return;
 360
 361         /* update attribute */
 362         pattrib = &pmgntframe->attrib;
 363         update_mgntframe_attrib(padapter, pattrib);
 364
 365         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 366
 367         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 368         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 369
 370         fctrl = &(pwlanhdr->frame_ctl);
 371         *(fctrl) = 0;
 372
 373         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
 374         memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
 375         memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
 376
 377         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
 378         pmlmeext->mgnt_seq++;
 379         SetFrameSubType(pframe, WIFI_ACTION);
 380
 381         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
 382         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
 383
 384         /* Build P2P action frame header */
 385         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
 386         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
 387         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
 388         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
 389
 390
 391         /* Add P2P IE header */
 392         /*      P2P OUI */
 393         p2pielen = 0;
 394         p2pie[p2pielen++] = 0x50;
 395         p2pie[p2pielen++] = 0x6F;
 396         p2pie[p2pielen++] = 0x9A;
 397         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
 398
 399         /* Add Status attribute in P2P IE */
 400         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
 401
 402         /* Add NoA attribute in P2P IE */
 403         noa_attr_content[0] = 0x1;/* index */
 404         noa_attr_content[1] = 0x0;/* CTWindow and OppPS Parameters */
 405
 406         /* todo: Notice of Absence Descriptor(s) */
 407
 408         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_NOA, 2, noa_attr_content);
 409
 410
 411
 412         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &(pattrib->pktlen));
 413
 414
 415         pattrib->last_txcmdsz = pattrib->pktlen;
 416
 417         dump_mgntframe(padapter, pmgntframe);
 418 }
 419
 420 u32 build_beacon_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
 421 {
 422         u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
 423         u16 capability = 0;
 424         u32 len = 0, p2pielen = 0;
 425         __le16 le_tmp;
 426
 427         /*      P2P OUI */
 428         p2pielen = 0;
 429         p2pie[p2pielen++] = 0x50;
 430         p2pie[p2pielen++] = 0x6F;
 431         p2pie[p2pielen++] = 0x9A;
 432         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
 433
 434
 435         /*      According to the P2P Specification, the beacon frame should contain 3 P2P attributes */
 436         /*      1. P2P Capability */
 437         /*      2. P2P Device ID */
 438         /*      3. Notice of Absence (NOA) */
 439
 440         /*      P2P Capability ATTR */
 441         /*      Type: */
 442         /*      Length: */
 443         /*      Value: */
 444         /*      Device Capability Bitmap, 1 byte */
 445         /*      Be able to participate in additional P2P Groups and */
 446         /*      support the P2P Invitation Procedure */
 447         /*      Group Capability Bitmap, 1 byte */
 448         capability = P2P_DEVCAP_INVITATION_PROC|P2P_DEVCAP_CLIENT_DISCOVERABILITY;
 449         capability |=  ((P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS) << 8);
 450         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
 451                 capability |= (P2P_GRPCAP_GROUP_FORMATION<<8);
 452
 453         le_tmp = cpu_to_le16(capability);
 454         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8 *)&le_tmp);
 455
 456         /*  P2P Device ID ATTR */
 457         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_DEVICE_ID, ETH_ALEN, pwdinfo->device_addr);
 458
 459         /*  Notice of Absence ATTR */
 460         /*      Type: */
 461         /*      Length: */
 462         /*      Value: */
 463
 464         pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
 465         return len;
 466 }
 467
 468 u32 build_probe_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
 469 {
 470         u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
 471         u32 len = 0, p2pielen = 0;
 472
 473         /*      P2P OUI */
 474         p2pielen = 0;
 475         p2pie[p2pielen++] = 0x50;
 476         p2pie[p2pielen++] = 0x6F;
 477         p2pie[p2pielen++] = 0x9A;
 478         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
 479
 480         /*      Commented by Albert 20100907 */
 481         /*      According to the P2P Specification, the probe response frame should contain 5 P2P attributes */
 482         /*      1. P2P Capability */
 483         /*      2. Extended Listen Timing */
 484         /*      3. Notice of Absence (NOA)      (Only GO needs this) */
 485         /*      4. Device Info */
 486         /*      5. Group Info   (Only GO need this) */
 487
 488         /*      P2P Capability ATTR */
 489         /*      Type: */
 490         p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
 491
 492         /*      Length: */
 493         /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
 494         RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
 495         p2pielen += 2;
 496
 497         /*      Value: */
 498         /*      Device Capability Bitmap, 1 byte */
 499         p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
 500
 501         /*      Group Capability Bitmap, 1 byte */
 502         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
 503                 p2pie[p2pielen] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS);
 504
 505                 if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
 506                         p2pie[p2pielen] |= P2P_GRPCAP_GROUP_FORMATION;
 507
 508                 p2pielen++;
 509         } else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
 510                 /*      Group Capability Bitmap, 1 byte */
 511                 if (pwdinfo->persistent_supported)
 512                         p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
 513                 else
 514                         p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
 515         }
 516
 517         /*      Extended Listen Timing ATTR */
 518         /*      Type: */
 519         p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
 520
 521         /*      Length: */
 522         /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004); */
 523         RTW_PUT_LE16(p2pie + p2pielen, 0x0004);
 524         p2pielen += 2;
 525
 526         /*      Value: */
 527         /*      Availability Period */
 528         /* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
 529         RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
 530         p2pielen += 2;
 531
 532         /*      Availability Interval */
 533         /* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
 534         RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
 535         p2pielen += 2;
 536
 537
 538         /*  Notice of Absence ATTR */
 539         /*      Type: */
 540         /*      Length: */
 541         /*      Value: */
 542
 543         /*      Device Info ATTR */
 544         /*      Type: */
 545         p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
 546
 547         /*      Length: */
 548         /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
 549         /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
 550         /* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
 551         RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
 552         p2pielen += 2;
 553
 554         /*      Value: */
 555         /*      P2P Device Address */
 556         memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
 557         p2pielen += ETH_ALEN;
 558
 559         /*      Config Method */
 560         /*      This field should be big endian. Noted by P2P specification. */
 561         /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm); */
 562         RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->supported_wps_cm);
 563         p2pielen += 2;
 564
 565         /*      Primary Device Type */
 566         /*      Category ID */
 567         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
 568         RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA);
 569         p2pielen += 2;
 570
 571         /*      OUI */
 572         /* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
 573         RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
 574         p2pielen += 4;
 575
 576         /*      Sub Category ID */
 577         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
 578         RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER);
 579         p2pielen += 2;
 580
 581         /*      Number of Secondary Device Types */
 582         p2pie[p2pielen++] = 0x00;       /*      No Secondary Device Type List */
 583
 584         /*      Device Name */
 585         /*      Type: */
 586         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
 587         RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
 588         p2pielen += 2;
 589
 590         /*      Length: */
 591         /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
 592         RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
 593         p2pielen += 2;
 594
 595         /*      Value: */
 596         memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
 597         p2pielen += pwdinfo->device_name_len;
 598
 599         /*  Group Info ATTR */
 600         /*      Type: */
 601         /*      Length: */
 602         /*      Value: */
 603         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
 604                 p2pielen += go_add_group_info_attr(pwdinfo, p2pie + p2pielen);
 605
 606
 607         pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
 608
 609
 610         return len;
 611 }
 612
 613 u32 build_prov_disc_request_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 *pssid, u8 ussidlen, u8 *pdev_raddr)
 614 {
 615         u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
 616         u32 len = 0, p2pielen = 0;
 617
 618         /*      P2P OUI */
 619         p2pielen = 0;
 620         p2pie[p2pielen++] = 0x50;
 621         p2pie[p2pielen++] = 0x6F;
 622         p2pie[p2pielen++] = 0x9A;
 623         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
 624
 625         /*      Commented by Albert 20110301 */
 626         /*      According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes */
 627         /*      1. P2P Capability */
 628         /*      2. Device Info */
 629         /*      3. Group ID (When joining an operating P2P Group) */
 630
 631         /*      P2P Capability ATTR */
 632         /*      Type: */
 633         p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
 634
 635         /*      Length: */
 636         /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
 637         RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
 638         p2pielen += 2;
 639
 640         /*      Value: */
 641         /*      Device Capability Bitmap, 1 byte */
 642         p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
 643
 644         /*      Group Capability Bitmap, 1 byte */
 645         if (pwdinfo->persistent_supported)
 646                 p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
 647         else
 648                 p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
 649
 650
 651         /*      Device Info ATTR */
 652         /*      Type: */
 653         p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
 654
 655         /*      Length: */
 656         /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
 657         /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
 658         /* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
 659         RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
 660         p2pielen += 2;
 661
 662         /*      Value: */
 663         /*      P2P Device Address */
 664         memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
 665         p2pielen += ETH_ALEN;
 666
 667         /*      Config Method */
 668         /*      This field should be big endian. Noted by P2P specification. */
 669         if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC) {
 670                 /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC); */
 671                 RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_PBC);
 672         } else {
 673                 /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY); */
 674                 RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_DISPLAY);
 675         }
 676
 677         p2pielen += 2;
 678
 679         /*      Primary Device Type */
 680         /*      Category ID */
 681         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
 682         RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA);
 683         p2pielen += 2;
 684
 685         /*      OUI */
 686         /* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
 687         RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
 688         p2pielen += 4;
 689
 690         /*      Sub Category ID */
 691         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
 692         RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER);
 693         p2pielen += 2;
 694
 695         /*      Number of Secondary Device Types */
 696         p2pie[p2pielen++] = 0x00;       /*      No Secondary Device Type List */
 697
 698         /*      Device Name */
 699         /*      Type: */
 700         /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
 701         RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
 702         p2pielen += 2;
 703
 704         /*      Length: */
 705         /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
 706         RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
 707         p2pielen += 2;
 708
 709         /*      Value: */
 710         memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
 711         p2pielen += pwdinfo->device_name_len;
 712
 713         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
 714                 /*      Added by Albert 2011/05/19 */
 715                 /*      In this case, the pdev_raddr is the device address of the group owner. */
 716
 717                 /*      P2P Group ID ATTR */
 718                 /*      Type: */
 719                 p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
 720
 721                 /*      Length: */
 722                 /* u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + ussidlen); */
 723                 RTW_PUT_LE16(p2pie + p2pielen, ETH_ALEN + ussidlen);
 724                 p2pielen += 2;
 725
 726                 /*      Value: */
 727                 memcpy(p2pie + p2pielen, pdev_raddr, ETH_ALEN);
 728                 p2pielen += ETH_ALEN;
 729
 730                 memcpy(p2pie + p2pielen, pssid, ussidlen);
 731                 p2pielen += ussidlen;
 732         }
 733
 734         pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
 735
 736
 737         return len;
 738 }
 739
 740
 741 u32 build_assoc_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 status_code)
 742 {
 743         u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
 744         u32 len = 0, p2pielen = 0;
 745
 746         /*      P2P OUI */
 747         p2pielen = 0;
 748         p2pie[p2pielen++] = 0x50;
 749         p2pie[p2pielen++] = 0x6F;
 750         p2pie[p2pielen++] = 0x9A;
 751         p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */
 752
 753         /*  According to the P2P Specification, the Association response frame should contain 2 P2P attributes */
 754         /*      1. Status */
 755         /*      2. Extended Listen Timing (optional) */
 756
 757
 758         /*      Status ATTR */
 759         p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status_code);
 760
 761
 762         /*  Extended Listen Timing ATTR */
 763         /*      Type: */
 764         /*      Length: */
 765         /*      Value: */
 766
 767         pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
 768
 769         return len;
 770 }
 771
 772 u32 build_deauth_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
 773 {
 774         u32 len = 0;
 775
 776         return len;
 777 }
 778
 779 u32 process_probe_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
 780 {
 781         u8 *p;
 782         u32 ret = false;
 783         u8 *p2pie;
 784         u32     p2pielen = 0;
 785         int ssid_len = 0, rate_cnt = 0;
 786
 787         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt,
 788                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
 789
 790         if (rate_cnt <= 4) {
 791                 int i, g_rate = 0;
 792
 793                 for (i = 0; i < rate_cnt; i++) {
 794                         if (((*(p + 2 + i) & 0xff) != 0x02) &&
 795                             ((*(p + 2 + i) & 0xff) != 0x04) &&
 796                             ((*(p + 2 + i) & 0xff) != 0x0B) &&
 797                             ((*(p + 2 + i) & 0xff) != 0x16))
 798                                 g_rate = 1;
 799                 }
 800
 801                 if (g_rate == 0) {
 802                         /*      There is no OFDM rate included in SupportedRates IE of this probe request frame */
 803                         /*      The driver should response this probe request. */
 804                         return ret;
 805                 }
 806         } else {
 807                 /*      rate_cnt > 4 means the SupportRates IE contains the OFDM rate because the count of CCK rates are 4. */
 808                 /*      We should proceed the following check for this probe request. */
 809         }
 810
 811         /*      Added comments by Albert 20100906 */
 812         /*      There are several items we should check here. */
 813         /*      1. This probe request frame must contain the P2P IE. (Done) */
 814         /*      2. This probe request frame must contain the wildcard SSID. (Done) */
 815         /*      3. Wildcard BSSID. (Todo) */
 816         /*      4. Destination Address. (Done in mgt_dispatcher function) */
 817         /*      5. Requested Device Type in WSC IE. (Todo) */
 818         /*      6. Device ID attribute in P2P IE. (Todo) */
 819
 820         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len,
 821                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
 822
 823         ssid_len &= 0xff;       /*      Just last 1 byte is valid for ssid len of the probe request */
 824         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
 825                 p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_ , len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_ , NULL, &p2pielen);
 826                 if (p2pie) {
 827                         if ((p != NULL) && !memcmp((void *)(p+2), (void *)pwdinfo->p2p_wildcard_ssid , 7)) {
 828                                 /* todo: */
 829                                 /* Check Requested Device Type attributes in WSC IE. */
 830                                 /* Check Device ID attribute in P2P IE */
 831
 832                                 ret = true;
 833                         } else if ((p != NULL) && (ssid_len == 0)) {
 834                                 ret = true;
 835                         }
 836                 } else {
 837                         /* non -p2p device */
 838                 }
 839         }
 840
 841
 842         return ret;
 843 }
 844
 845 u32 process_assoc_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len, struct sta_info *psta)
 846 {
 847         u8 status_code = P2P_STATUS_SUCCESS;
 848         u8 *pbuf, *pattr_content = NULL;
 849         u32 attr_contentlen = 0;
 850         u16 cap_attr = 0;
 851         unsigned short  frame_type, ie_offset = 0;
 852         u8 *ies;
 853         u32 ies_len;
 854         u8 *p2p_ie;
 855         u32     p2p_ielen = 0;
 856         __be16 be_tmp;
 857         __le16 le_tmp;
 858
 859         if (!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
 860                 return P2P_STATUS_FAIL_REQUEST_UNABLE;
 861
 862         frame_type = GetFrameSubType(pframe);
 863         if (frame_type == WIFI_ASSOCREQ)
 864                 ie_offset = _ASOCREQ_IE_OFFSET_;
 865         else /*  WIFI_REASSOCREQ */
 866                 ie_offset = _REASOCREQ_IE_OFFSET_;
 867
 868         ies = pframe + WLAN_HDR_A3_LEN + ie_offset;
 869         ies_len = len - WLAN_HDR_A3_LEN - ie_offset;
 870
 871         p2p_ie = rtw_get_p2p_ie(ies , ies_len , NULL, &p2p_ielen);
 872
 873         if (!p2p_ie) {
 874                 DBG_88E("[%s] P2P IE not Found!!\n", __func__);
 875                 status_code =  P2P_STATUS_FAIL_INVALID_PARAM;
 876         } else {
 877                 DBG_88E("[%s] P2P IE Found!!\n", __func__);
 878         }
 879
 880         while (p2p_ie) {
 881                 /* Check P2P Capability ATTR */
 882                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8 *)&le_tmp, (uint *)&attr_contentlen)) {
 883                         DBG_88E("[%s] Got P2P Capability Attr!!\n", __func__);
 884                         cap_attr = le16_to_cpu(le_tmp);
 885                         psta->dev_cap = cap_attr&0xff;
 886                 }
 887
 888                 /* Check Extended Listen Timing ATTR */
 889
 890
 891                 /* Check P2P Device Info ATTR */
 892                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint *)&attr_contentlen)) {
 893                         DBG_88E("[%s] Got P2P DEVICE INFO Attr!!\n", __func__);
 894                         pattr_content = rtw_zmalloc(attr_contentlen);
 895                         pbuf = pattr_content;
 896                         if (pattr_content) {
 897                                 u8 num_of_secdev_type;
 898                                 u16 dev_name_len;
 899
 900                                 rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO , pattr_content, (uint *)&attr_contentlen);
 901
 902                                 memcpy(psta->dev_addr,  pattr_content, ETH_ALEN);/* P2P Device Address */
 903
 904                                 pattr_content += ETH_ALEN;
 905
 906                                 memcpy(&be_tmp, pattr_content, 2);/* Config Methods */
 907                                 psta->config_methods = be16_to_cpu(be_tmp);
 908
 909                                 pattr_content += 2;
 910
 911                                 memcpy(psta->primary_dev_type, pattr_content, 8);
 912
 913                                 pattr_content += 8;
 914
 915                                 num_of_secdev_type = *pattr_content;
 916                                 pattr_content += 1;
 917
 918                                 if (num_of_secdev_type == 0) {
 919                                         psta->num_of_secdev_type = 0;
 920                                 } else {
 921                                         u32 len;
 922
 923                                         psta->num_of_secdev_type = num_of_secdev_type;
 924
 925                                         len = (sizeof(psta->secdev_types_list) < (num_of_secdev_type*8)) ?
 926                                               (sizeof(psta->secdev_types_list)) : (num_of_secdev_type*8);
 927
 928                                         memcpy(psta->secdev_types_list, pattr_content, len);
 929
 930                                         pattr_content += (num_of_secdev_type*8);
 931                                 }
 932
 933
 934                                 psta->dev_name_len = 0;
 935                                 if (WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(__be16 *)pattr_content)) {
 936                                         dev_name_len = be16_to_cpu(*(__be16 *)(pattr_content+2));
 937
 938                                         psta->dev_name_len = (sizeof(psta->dev_name) < dev_name_len) ? sizeof(psta->dev_name) : dev_name_len;
 939
 940                                         memcpy(psta->dev_name, pattr_content+4, psta->dev_name_len);
 941                                 }
 942                                 kfree(pbuf);
 943                         }
 944                 }
 945
 946                 /* Get the next P2P IE */
 947                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
 948         }
 949
 950         return status_code;
 951 }
 952
 953 u32 process_p2p_devdisc_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
 954 {
 955         u8 *frame_body;
 956         u8 status, dialogToken;
 957         struct sta_info *psta = NULL;
 958         struct adapter *padapter = pwdinfo->padapter;
 959         struct sta_priv *pstapriv = &padapter->stapriv;
 960         u8 *p2p_ie;
 961         u32     p2p_ielen = 0;
 962
 963         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
 964
 965         dialogToken = frame_body[7];
 966         status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
 967
 968         p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
 969         if (p2p_ie) {
 970                 u8 groupid[38] = { 0x00 };
 971                 u8 dev_addr[ETH_ALEN] = { 0x00 };
 972                 u32     attr_contentlen = 0;
 973
 974                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) {
 975                         if (!memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) &&
 976                             !memcmp(pwdinfo->p2p_group_ssid, groupid+ETH_ALEN, pwdinfo->p2p_group_ssid_len)) {
 977                                 attr_contentlen = 0;
 978                                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_ID, dev_addr, &attr_contentlen)) {
 979                                         struct list_head *phead, *plist;
 980
 981                                         spin_lock_bh(&pstapriv->asoc_list_lock);
 982                                         phead = &pstapriv->asoc_list;
 983                                         plist = phead->next;
 984
 985                                         /* look up sta asoc_queue */
 986                                         while ((rtw_end_of_queue_search(phead, plist)) == false) {
 987                                                 psta = container_of(plist, struct sta_info, asoc_list);
 988
 989                                                 plist = plist->next;
 990
 991                                                 if (psta->is_p2p_device && (psta->dev_cap&P2P_DEVCAP_CLIENT_DISCOVERABILITY) &&
 992                                                     !memcmp(psta->dev_addr, dev_addr, ETH_ALEN)) {
 993                                                         /* issue GO Discoverability Request */
 994                                                         issue_group_disc_req(pwdinfo, psta->hwaddr);
 995                                                         status = P2P_STATUS_SUCCESS;
 996                                                         break;
 997                                                 } else {
 998                                                         status = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
 999                                                 }
1000                                         }
1001                                         spin_unlock_bh(&pstapriv->asoc_list_lock);
1002                                 } else {
1003                                         status = P2P_STATUS_FAIL_INVALID_PARAM;
1004                                 }
1005                         } else {
1006                                 status = P2P_STATUS_FAIL_INVALID_PARAM;
1007                         }
1008                 }
1009         }
1010
1011
1012         /* issue Device Discoverability Response */
1013         issue_p2p_devdisc_resp(pwdinfo, GetAddr2Ptr(pframe), status, dialogToken);
1014
1015         return (status == P2P_STATUS_SUCCESS) ? true : false;
1016 }
1017
1018 u32 process_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
1019 {
1020         return true;
1021 }
1022
1023 u8 process_p2p_provdisc_req(struct wifidirect_info *pwdinfo,  u8 *pframe, uint len)
1024 {
1025         u8 *frame_body;
1026         u8 *wpsie;
1027         uint    wps_ielen = 0, attr_contentlen = 0;
1028         u16     uconfig_method = 0;
1029         __be16 be_tmp;
1030
1031         frame_body = (pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
1032
1033         wpsie = rtw_get_wps_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen);
1034         if (wpsie) {
1035                 if (rtw_get_wps_attr_content(wpsie, wps_ielen, WPS_ATTR_CONF_METHOD, (u8 *)&be_tmp, &attr_contentlen)) {
1036                         uconfig_method = be16_to_cpu(be_tmp);
1037                         switch (uconfig_method) {
1038                         case WPS_CM_DISPLYA:
1039                                 memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
1040                                 break;
1041                         case WPS_CM_LABEL:
1042                                 memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "lab", 3);
1043                                 break;
1044                         case WPS_CM_PUSH_BUTTON:
1045                                 memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
1046                                 break;
1047                         case WPS_CM_KEYPAD:
1048                                 memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
1049                                 break;
1050                         }
1051                         issue_p2p_provision_resp(pwdinfo, GetAddr2Ptr(pframe), frame_body, uconfig_method);
1052                 }
1053         }
1054         DBG_88E("[%s] config method = %s\n", __func__, pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req);
1055         return true;
1056 }
1057
1058 u8 process_p2p_provdisc_resp(struct wifidirect_info *pwdinfo,  u8 *pframe)
1059 {
1060         return true;
1061 }
1062
1063 static u8 rtw_p2p_get_peer_ch_list(struct wifidirect_info *pwdinfo, u8 *ch_content, u8 ch_cnt, u8 *peer_ch_list)
1064 {
1065         u8 i = 0, j = 0;
1066         u8 temp = 0;
1067         u8 ch_no = 0;
1068         ch_content += 3;
1069         ch_cnt -= 3;
1070
1071         while (ch_cnt > 0) {
1072                 ch_content += 1;
1073                 ch_cnt -= 1;
1074                 temp = *ch_content;
1075                 for (i = 0 ; i < temp ; i++, j++)
1076                         peer_ch_list[j] = *(ch_content + 1 + i);
1077                 ch_content += (temp + 1);
1078                 ch_cnt -= (temp + 1);
1079                 ch_no += temp ;
1080         }
1081
1082         return ch_no;
1083 }
1084
1085 static u8 rtw_p2p_ch_inclusion(struct mlme_ext_priv *pmlmeext, u8 *peer_ch_list, u8 peer_ch_num, u8 *ch_list_inclusioned)
1086 {
1087         int     i = 0, j = 0, temp = 0;
1088         u8 ch_no = 0;
1089
1090         for (i = 0; i < peer_ch_num; i++) {
1091                 for (j = temp; j < pmlmeext->max_chan_nums; j++) {
1092                         if (*(peer_ch_list + i) == pmlmeext->channel_set[j].ChannelNum) {
1093                                 ch_list_inclusioned[ch_no++] = *(peer_ch_list + i);
1094                                 temp = j;
1095                                 break;
1096                         }
1097                 }
1098         }
1099
1100         return ch_no;
1101 }
1102
1103 u8 process_p2p_group_negotation_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
1104 {
1105         struct adapter *padapter = pwdinfo->padapter;
1106         u8      result = P2P_STATUS_SUCCESS;
1107         u32     p2p_ielen = 0, wps_ielen = 0;
1108         u8 *ies;
1109         u32 ies_len;
1110         u8 *p2p_ie;
1111         u8 *wpsie;
1112         u16             wps_devicepassword_id = 0x0000;
1113         uint    wps_devicepassword_id_len = 0;
1114         __be16 be_tmp;
1115
1116         wpsie = rtw_get_wps_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen);
1117         if (wpsie) {
1118                 /*      Commented by Kurt 20120113 */
1119                 /*      If some device wants to do p2p handshake without sending prov_disc_req */
1120                 /*      We have to get peer_req_cm from here. */
1121                 if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) {
1122                         rtw_get_wps_attr_content(wpsie, wps_ielen, WPS_ATTR_DEVICE_PWID, (u8 *)&be_tmp, &wps_devicepassword_id_len);
1123                         wps_devicepassword_id = be16_to_cpu(be_tmp);
1124
1125                         if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
1126                                 memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
1127                         else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
1128                                 memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
1129                         else
1130                                 memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
1131                 }
1132         } else {
1133                 DBG_88E("[%s] WPS IE not Found!!\n", __func__);
1134                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
1135                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
1136                 return result;
1137         }
1138
1139         if (pwdinfo->ui_got_wps_info == P2P_NO_WPSINFO) {
1140                 result = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
1141                 rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_INFOR_NOREADY);
1142                 return result;
1143         }
1144
1145         ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
1146         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
1147
1148         p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
1149
1150         if (!p2p_ie) {
1151                 DBG_88E("[%s] P2P IE not Found!!\n", __func__);
1152                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
1153                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
1154         }
1155
1156         while (p2p_ie) {
1157                 u8      attr_content = 0x00;
1158                 u32     attr_contentlen = 0;
1159                 u8      ch_content[50] = { 0x00 };
1160                 uint    ch_cnt = 0;
1161                 u8      peer_ch_list[50] = { 0x00 };
1162                 u8      peer_ch_num = 0;
1163                 u8      ch_list_inclusioned[50] = { 0x00 };
1164                 u8      ch_num_inclusioned = 0;
1165
1166                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING);
1167
1168                         if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen)) {
1169                         DBG_88E("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01);
1170                         pwdinfo->peer_intent = attr_content;    /*      include both intent and tie breaker values. */
1171
1172                         if (pwdinfo->intent == (pwdinfo->peer_intent >> 1)) {
1173                                 /*      Try to match the tie breaker value */
1174                                 if (pwdinfo->intent == P2P_MAX_INTENT) {
1175                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
1176                                         result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
1177                                 } else {
1178                                         if (attr_content & 0x01)
1179                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
1180                                         else
1181                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
1182                                 }
1183                         } else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1)) {
1184                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
1185                         } else {
1186                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
1187                         }
1188
1189                         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
1190                                 /*      Store the group id information. */
1191                                 memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
1192                                 memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
1193                         }
1194                 }
1195
1196
1197                 attr_contentlen = 0;
1198                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) {
1199                         if (attr_contentlen != ETH_ALEN)
1200                                 _rtw_memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
1201                 }
1202
1203                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, ch_content, &ch_cnt)) {
1204                         peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, ch_content, ch_cnt, peer_ch_list);
1205                         ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
1206
1207                         if (ch_num_inclusioned == 0) {
1208                                 DBG_88E("[%s] No common channel in channel list!\n", __func__);
1209                                 result = P2P_STATUS_FAIL_NO_COMMON_CH;
1210                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
1211                                 break;
1212                         }
1213
1214                         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
1215                                 if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
1216                                     ch_list_inclusioned, ch_num_inclusioned)) {
1217                                         u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
1218                                         attr_contentlen = 0;
1219
1220                                         if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
1221                                                 peer_operating_ch = operatingch_info[4];
1222
1223                                         if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
1224                                                                        ch_list_inclusioned, ch_num_inclusioned)) {
1225                                                 /**
1226                                                  *      Change our operating channel as peer's for compatibility.
1227                                                  */
1228                                                 pwdinfo->operating_channel = peer_operating_ch;
1229                                                 DBG_88E("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
1230                                         } else {
1231                                                 /*  Take first channel of ch_list_inclusioned as operating channel */
1232                                                 pwdinfo->operating_channel = ch_list_inclusioned[0];
1233                                                 DBG_88E("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
1234                                         }
1235                                 }
1236                         }
1237                 }
1238
1239                 /* Get the next P2P IE */
1240                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
1241         }
1242         return result;
1243 }
1244
1245 u8 process_p2p_group_negotation_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
1246 {
1247         struct adapter *padapter = pwdinfo->padapter;
1248         u8      result = P2P_STATUS_SUCCESS;
1249         u32     p2p_ielen, wps_ielen;
1250         u8 *ies;
1251         u32 ies_len;
1252         u8 *p2p_ie;
1253
1254         ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
1255         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
1256
1257         /*      Be able to know which one is the P2P GO and which one is P2P client. */
1258
1259         if (rtw_get_wps_ie(ies, ies_len, NULL, &wps_ielen)) {
1260         } else {
1261                 DBG_88E("[%s] WPS IE not Found!!\n", __func__);
1262                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
1263                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
1264         }
1265
1266         p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
1267         if (!p2p_ie) {
1268                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
1269                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
1270                 result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
1271         } else {
1272                 u8      attr_content = 0x00;
1273                 u32     attr_contentlen = 0;
1274                 u8      operatingch_info[5] = { 0x00 };
1275                 u8      groupid[38];
1276                 u8      peer_ch_list[50] = { 0x00 };
1277                 u8      peer_ch_num = 0;
1278                 u8      ch_list_inclusioned[50] = { 0x00 };
1279                 u8      ch_num_inclusioned = 0;
1280
1281                 while (p2p_ie) {        /*      Found the P2P IE. */
1282                         rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
1283                         if (attr_contentlen == 1) {
1284                                 DBG_88E("[%s] Status = %d\n", __func__, attr_content);
1285                                 if (attr_content == P2P_STATUS_SUCCESS) {
1286                                         /*      Do nothing. */
1287                                 } else {
1288                                         if (P2P_STATUS_FAIL_INFO_UNAVAILABLE == attr_content) {
1289                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INFOR_NOREADY);
1290                                         } else {
1291                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
1292                                         }
1293                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
1294                                         result = attr_content;
1295                                         break;
1296                                 }
1297                         }
1298
1299                         /*      Try to get the peer's interface address */
1300                         attr_contentlen = 0;
1301                         if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) {
1302                                 if (attr_contentlen != ETH_ALEN)
1303                                         _rtw_memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
1304                         }
1305
1306                         /*      Try to get the peer's intent and tie breaker value. */
1307                         attr_content = 0x00;
1308                         attr_contentlen = 0;
1309                         if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen)) {
1310                                 DBG_88E("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01);
1311                                 pwdinfo->peer_intent = attr_content;    /*      include both intent and tie breaker values. */
1312
1313                                 if (pwdinfo->intent == (pwdinfo->peer_intent >> 1)) {
1314                                         /*      Try to match the tie breaker value */
1315                                         if (pwdinfo->intent == P2P_MAX_INTENT) {
1316                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
1317                                                 result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
1318                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
1319                                         } else {
1320                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
1321                                                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
1322                                                 if (attr_content & 0x01)
1323                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
1324                                                 else
1325                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
1326                                         }
1327                                 } else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1)) {
1328                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
1329                                         rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
1330                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
1331                                 } else {
1332                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
1333                                         rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
1334                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
1335                                 }
1336
1337                                 if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
1338                                         /*      Store the group id information. */
1339                                         memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
1340                                         memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
1341                                 }
1342                         }
1343
1344                         /*      Try to get the operation channel information */
1345
1346                         attr_contentlen = 0;
1347                         if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) {
1348                                 DBG_88E("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]);
1349                                 pwdinfo->peer_operating_ch = operatingch_info[4];
1350                         }
1351
1352                         /*      Try to get the channel list information */
1353                         if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, pwdinfo->channel_list_attr, &pwdinfo->channel_list_attr_len)) {
1354                                 DBG_88E("[%s] channel list attribute found, len = %d\n", __func__,  pwdinfo->channel_list_attr_len);
1355
1356                                 peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len, peer_ch_list);
1357                                 ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
1358
1359                                 if (ch_num_inclusioned == 0) {
1360                                         DBG_88E("[%s] No common channel in channel list!\n", __func__);
1361                                         result = P2P_STATUS_FAIL_NO_COMMON_CH;
1362                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
1363                                         break;
1364                                 }
1365
1366                                 if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
1367                                         if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
1368                                             ch_list_inclusioned, ch_num_inclusioned)) {
1369                                                 u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
1370                                                 attr_contentlen = 0;
1371
1372                                                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
1373                                                         peer_operating_ch = operatingch_info[4];
1374
1375                                                 if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
1376                                                     ch_list_inclusioned, ch_num_inclusioned)) {
1377                                                         /**
1378                                                          *      Change our operating channel as peer's for compatibility.
1379                                                          */
1380                                                         pwdinfo->operating_channel = peer_operating_ch;
1381                                                         DBG_88E("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
1382                                                 } else {
1383                                                         /*  Take first channel of ch_list_inclusioned as operating channel */
1384                                                         pwdinfo->operating_channel = ch_list_inclusioned[0];
1385                                                         DBG_88E("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
1386                                                 }
1387                                         }
1388                                 }
1389                         } else {
1390                                 DBG_88E("[%s] channel list attribute not found!\n", __func__);
1391                         }
1392
1393                         /*      Try to get the group id information if peer is GO */
1394                         attr_contentlen = 0;
1395                         _rtw_memset(groupid, 0x00, 38);
1396                         if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) {
1397                                 memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
1398                                 memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
1399                         }
1400
1401                         /* Get the next P2P IE */
1402                         p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
1403                 }
1404         }
1405         return result;
1406 }
1407
1408 u8 process_p2p_group_negotation_confirm(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
1409 {
1410         u8 *ies;
1411         u32 ies_len;
1412         u8 *p2p_ie;
1413         u32     p2p_ielen = 0;
1414         u8      result = P2P_STATUS_SUCCESS;
1415         ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
1416         ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
1417
1418         p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
1419         while (p2p_ie) {        /*      Found the P2P IE. */
1420                 u8      attr_content = 0x00, operatingch_info[5] = { 0x00 };
1421                 u8      groupid[38] = { 0x00 };
1422                 u32     attr_contentlen = 0;
1423
1424                 pwdinfo->negotiation_dialog_token = 1;
1425                 rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
1426                 if (attr_contentlen == 1) {
1427                         DBG_88E("[%s] Status = %d\n", __func__, attr_content);
1428                         result = attr_content;
1429
1430                         if (attr_content == P2P_STATUS_SUCCESS) {
1431                                 u8      bcancelled = 0;
1432
1433                                 _cancel_timer(&pwdinfo->restore_p2p_state_timer, &bcancelled);
1434
1435                                 /*      Commented by Albert 20100911 */
1436                                 /*      Todo: Need to handle the case which both Intents are the same. */
1437                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
1438                                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
1439                                 if ((pwdinfo->intent) > (pwdinfo->peer_intent >> 1)) {
1440                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
1441                                 } else if ((pwdinfo->intent) < (pwdinfo->peer_intent >> 1)) {
1442                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
1443                                 } else {
1444                                         /*      Have to compare the Tie Breaker */
1445                                         if (pwdinfo->peer_intent & 0x01)
1446                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
1447                                         else
1448                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
1449                                 }
1450                         } else {
1451                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
1452                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
1453                                 break;
1454                         }
1455                 }
1456
1457                 /*      Try to get the group id information */
1458                 attr_contentlen = 0;
1459                 _rtw_memset(groupid, 0x00, 38);
1460                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) {
1461                         DBG_88E("[%s] Ssid = %s, ssidlen = %zu\n", __func__, &groupid[ETH_ALEN], strlen(&groupid[ETH_ALEN]));
1462                         memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
1463                         memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
1464                 }
1465
1466                 attr_contentlen = 0;
1467                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) {
1468                         DBG_88E("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]);
1469                         pwdinfo->peer_operating_ch = operatingch_info[4];
1470                 }
1471
1472                 /* Get the next P2P IE */
1473                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
1474         }
1475         return result;
1476 }
1477
1478 u8 process_p2p_presence_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
1479 {
1480         u8 *frame_body;
1481         u8 dialogToken = 0;
1482         u8 status = P2P_STATUS_SUCCESS;
1483
1484         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
1485
1486         dialogToken = frame_body[6];
1487
1488         /* todo: check NoA attribute */
1489
1490         issue_p2p_presence_resp(pwdinfo, GetAddr2Ptr(pframe), status, dialogToken);
1491
1492         return true;
1493 }
1494
1495 static void find_phase_handler(struct adapter *padapter)
1496 {
1497         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
1498         struct ndis_802_11_ssid ssid;
1499         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1500
1501
1502         _rtw_memset((unsigned char *)&ssid, 0, sizeof(struct ndis_802_11_ssid));
1503         memcpy(ssid.Ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN);
1504         ssid.SsidLength = P2P_WILDCARD_SSID_LEN;
1505
1506         rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
1507
1508         spin_lock_bh(&pmlmepriv->lock);
1509         rtw_sitesurvey_cmd(padapter, &ssid, 1, NULL, 0);
1510         spin_unlock_bh(&pmlmepriv->lock);
1511 }
1512
1513 void p2p_concurrent_handler(struct adapter *padapter);
1514
1515 static void restore_p2p_state_handler(struct adapter *padapter)
1516 {
1517         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
1518
1519         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
1520                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
1521         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
1522
1523         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
1524                 /*      In the P2P client mode, the driver should not switch back to its listen channel */
1525                 /*      because this P2P client should stay at the operating channel of P2P GO. */
1526                 set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
1527         }
1528 }
1529
1530 static void pre_tx_invitereq_handler(struct adapter *padapter)
1531 {
1532         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
1533         u8      val8 = 1;
1534
1535         set_channel_bwmode(padapter, pwdinfo->invitereq_info.peer_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
1536         padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
1537         issue_probereq_p2p(padapter, NULL);
1538         _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
1539
1540 }
1541
1542 static void pre_tx_provdisc_handler(struct adapter *padapter)
1543 {
1544         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
1545         u8      val8 = 1;
1546
1547         set_channel_bwmode(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
1548         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
1549         issue_probereq_p2p(padapter, NULL);
1550         _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
1551
1552 }
1553
1554 static void pre_tx_negoreq_handler(struct adapter *padapter)
1555 {
1556         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
1557         u8      val8 = 1;
1558
1559         set_channel_bwmode(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
1560         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
1561         issue_probereq_p2p(padapter, NULL);
1562         _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
1563
1564 }
1565
1566 void p2p_protocol_wk_hdl(struct adapter *padapter, int intCmdType)
1567 {
1568         switch (intCmdType) {
1569         case P2P_FIND_PHASE_WK:
1570                 find_phase_handler(padapter);
1571                 break;
1572         case P2P_RESTORE_STATE_WK:
1573                 restore_p2p_state_handler(padapter);
1574                 break;
1575         case P2P_PRE_TX_PROVDISC_PROCESS_WK:
1576                 pre_tx_provdisc_handler(padapter);
1577                 break;
1578         case P2P_PRE_TX_INVITEREQ_PROCESS_WK:
1579                 pre_tx_invitereq_handler(padapter);
1580                 break;
1581         case P2P_PRE_TX_NEGOREQ_PROCESS_WK:
1582                 pre_tx_negoreq_handler(padapter);
1583                 break;
1584         }
1585
1586 }
1587
1588 void process_p2p_ps_ie(struct adapter *padapter, u8 *IEs, u32 IELength)
1589 {
1590         u8 *ies;
1591         u32 ies_len;
1592         u8 *p2p_ie;
1593         u32     p2p_ielen = 0;
1594         u8      noa_attr[MAX_P2P_IE_LEN] = { 0x00 };/*  NoA length should be n*(13) + 2 */
1595         u32     attr_contentlen = 0;
1596
1597         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
1598         u8      find_p2p = false, find_p2p_ps = false;
1599         u8      noa_offset, noa_num, noa_index;
1600
1601
1602         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
1603                 return;
1604         if (IELength <= _BEACON_IE_OFFSET_)
1605                 return;
1606
1607         ies = IEs + _BEACON_IE_OFFSET_;
1608         ies_len = IELength - _BEACON_IE_OFFSET_;
1609
1610         p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
1611
1612         while (p2p_ie) {
1613                 find_p2p = true;
1614                 /*  Get Notice of Absence IE. */
1615                 if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_NOA, noa_attr, &attr_contentlen)) {
1616                         find_p2p_ps = true;
1617                         noa_index = noa_attr[0];
1618
1619                         if ((pwdinfo->p2p_ps_mode == P2P_PS_NONE) ||
1620                             (noa_index != pwdinfo->noa_index)) { /*  if index change, driver should reconfigure related setting. */
1621                                 pwdinfo->noa_index = noa_index;
1622                                 pwdinfo->opp_ps = noa_attr[1] >> 7;
1623                                 pwdinfo->ctwindow = noa_attr[1] & 0x7F;
1624
1625                                 noa_offset = 2;
1626                                 noa_num = 0;
1627                                 /*  NoA length should be n*(13) + 2 */
1628                                 if (attr_contentlen > 2) {
1629                                         while (noa_offset < attr_contentlen) {
1630                                                 /* memcpy(&wifidirect_info->noa_count[noa_num], &noa_attr[noa_offset], 1); */
1631                                                 pwdinfo->noa_count[noa_num] = noa_attr[noa_offset];
1632                                                 noa_offset += 1;
1633
1634                                                 memcpy(&pwdinfo->noa_duration[noa_num], &noa_attr[noa_offset], 4);
1635                                                 noa_offset += 4;
1636
1637                                                 memcpy(&pwdinfo->noa_interval[noa_num], &noa_attr[noa_offset], 4);
1638                                                 noa_offset += 4;
1639
1640                                                 memcpy(&pwdinfo->noa_start_time[noa_num], &noa_attr[noa_offset], 4);
1641                                                 noa_offset += 4;
1642
1643                                                 noa_num++;
1644                                         }
1645                                 }
1646                                 pwdinfo->noa_num = noa_num;
1647
1648                                 if (pwdinfo->opp_ps == 1) {
1649                                         pwdinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
1650                                         /*  driver should wait LPS for entering CTWindow */
1651                                         if (padapter->pwrctrlpriv.bFwCurrentInPSMode)
1652                                                 p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1);
1653                                 } else if (pwdinfo->noa_num > 0) {
1654                                         pwdinfo->p2p_ps_mode = P2P_PS_NOA;
1655                                         p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1);
1656                                 } else if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
1657                                         p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
1658                                 }
1659                         }
1660
1661                         break; /*  find target, just break. */
1662                 }
1663
1664                 /* Get the next P2P IE */
1665                 p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
1666         }
1667
1668         if (find_p2p) {
1669                 if ((pwdinfo->p2p_ps_mode > P2P_PS_NONE) && !find_p2p_ps)
1670                         p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
1671         }
1672
1673 }
1674
1675 void p2p_ps_wk_hdl(struct adapter *padapter, u8 p2p_ps_state)
1676 {
1677         struct pwrctrl_priv             *pwrpriv = &padapter->pwrctrlpriv;
1678         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
1679
1680
1681         /*  Pre action for p2p state */
1682         switch (p2p_ps_state) {
1683         case P2P_PS_DISABLE:
1684                 pwdinfo->p2p_ps_state = p2p_ps_state;
1685
1686                 rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
1687
1688                 pwdinfo->noa_index = 0;
1689                 pwdinfo->ctwindow = 0;
1690                 pwdinfo->opp_ps = 0;
1691                 pwdinfo->noa_num = 0;
1692                 pwdinfo->p2p_ps_mode = P2P_PS_NONE;
1693                 if (padapter->pwrctrlpriv.bFwCurrentInPSMode) {
1694                         if (pwrpriv->smart_ps == 0) {
1695                                 pwrpriv->smart_ps = 2;
1696                                 rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&(padapter->pwrctrlpriv.pwr_mode)));
1697                         }
1698                 }
1699                 break;
1700         case P2P_PS_ENABLE:
1701                 if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
1702                         pwdinfo->p2p_ps_state = p2p_ps_state;
1703
1704                         if (pwdinfo->ctwindow > 0) {
1705                                 if (pwrpriv->smart_ps != 0) {
1706                                         pwrpriv->smart_ps = 0;
1707                                         DBG_88E("%s(): Enter CTW, change SmartPS\n", __func__);
1708                                         rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&(padapter->pwrctrlpriv.pwr_mode)));
1709                                 }
1710                         }
1711                         rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
1712                 }
1713                 break;
1714         case P2P_PS_SCAN:
1715         case P2P_PS_SCAN_DONE:
1716         case P2P_PS_ALLSTASLEEP:
1717                 if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
1718                         pwdinfo->p2p_ps_state = p2p_ps_state;
1719                         rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
1720                 }
1721                 break;
1722         default:
1723                 break;
1724         }
1725
1726 }
1727
1728 u8 p2p_ps_wk_cmd(struct adapter *padapter, u8 p2p_ps_state, u8 enqueue)
1729 {
1730         struct cmd_obj  *ph2c;
1731         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
1732         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
1733         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1734         u8      res = _SUCCESS;
1735
1736
1737         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
1738                 return res;
1739
1740         if (enqueue) {
1741                 ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1742                 if (ph2c == NULL) {
1743                         res = _FAIL;
1744                         goto exit;
1745                 }
1746
1747                 pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
1748                 if (pdrvextra_cmd_parm == NULL) {
1749                         kfree(ph2c);
1750                         res = _FAIL;
1751                         goto exit;
1752                 }
1753
1754                 pdrvextra_cmd_parm->ec_id = P2P_PS_WK_CID;
1755                 pdrvextra_cmd_parm->type_size = p2p_ps_state;
1756                 pdrvextra_cmd_parm->pbuf = NULL;
1757
1758                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
1759
1760                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1761         } else {
1762                 p2p_ps_wk_hdl(padapter, p2p_ps_state);
1763         }
1764
1765 exit:
1766
1767
1768         return res;
1769 }
1770
1771 static void reset_ch_sitesurvey_timer_process (void *FunctionContext)
1772 {
1773         struct adapter *adapter = (struct adapter *)FunctionContext;
1774         struct  wifidirect_info         *pwdinfo = &adapter->wdinfo;
1775
1776         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
1777                 return;
1778
1779         DBG_88E("[%s] In\n", __func__);
1780         /*      Reset the operation channel information */
1781         pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
1782         pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
1783 }
1784
1785 static void reset_ch_sitesurvey_timer_process2 (void *FunctionContext)
1786 {
1787         struct adapter *adapter = (struct adapter *)FunctionContext;
1788         struct  wifidirect_info         *pwdinfo = &adapter->wdinfo;
1789
1790         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
1791                 return;
1792
1793         DBG_88E("[%s] In\n", __func__);
1794         /*      Reset the operation channel information */
1795         pwdinfo->p2p_info.operation_ch[0] = 0;
1796         pwdinfo->p2p_info.scan_op_ch_only = 0;
1797 }
1798
1799 static void restore_p2p_state_timer_process (void *FunctionContext)
1800 {
1801         struct adapter *adapter = (struct adapter *)FunctionContext;
1802         struct  wifidirect_info         *pwdinfo = &adapter->wdinfo;
1803
1804         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
1805                 return;
1806
1807         p2p_protocol_wk_cmd(adapter, P2P_RESTORE_STATE_WK);
1808 }
1809
1810 static void pre_tx_scan_timer_process(void *FunctionContext)
1811 {
1812         struct adapter *adapter = (struct adapter *)FunctionContext;
1813         struct  wifidirect_info *pwdinfo = &adapter->wdinfo;
1814         struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
1815
1816         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
1817                 return;
1818
1819         spin_lock_bh(&pmlmepriv->lock);
1820
1821         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
1822                 if (pwdinfo->tx_prov_disc_info.benable) {       /*      the provision discovery request frame is trigger to send or not */
1823                         p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_PROVDISC_PROCESS_WK);
1824                         /* issue_probereq_p2p(adapter, NULL); */
1825                         /* _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); */
1826                 }
1827         } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
1828                 if (pwdinfo->nego_req_info.benable)
1829                         p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_NEGOREQ_PROCESS_WK);
1830         } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) {
1831                 if (pwdinfo->invitereq_info.benable)
1832                         p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK);
1833         } else {
1834                 DBG_88E("[%s] p2p_state is %d, ignore!!\n", __func__, rtw_p2p_state(pwdinfo));
1835         }
1836
1837         spin_unlock_bh(&pmlmepriv->lock);
1838 }
1839
1840 static void find_phase_timer_process(void *FunctionContext)
1841 {
1842         struct adapter *adapter = (struct adapter *)FunctionContext;
1843         struct  wifidirect_info         *pwdinfo = &adapter->wdinfo;
1844
1845         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
1846                 return;
1847
1848         adapter->wdinfo.find_phase_state_exchange_cnt++;
1849
1850         p2p_protocol_wk_cmd(adapter, P2P_FIND_PHASE_WK);
1851 }
1852
1853 void reset_global_wifidirect_info(struct adapter *padapter)
1854 {
1855         struct wifidirect_info  *pwdinfo;
1856
1857         pwdinfo = &padapter->wdinfo;
1858         pwdinfo->persistent_supported = 0;
1859         pwdinfo->session_available = true;
1860         pwdinfo->wfd_tdls_enable = 0;
1861         pwdinfo->wfd_tdls_weaksec = 0;
1862 }
1863
1864 void rtw_init_wifidirect_timers(struct adapter *padapter)
1865 {
1866         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
1867
1868         _init_timer(&pwdinfo->find_phase_timer, padapter->pnetdev, find_phase_timer_process, padapter);
1869         _init_timer(&pwdinfo->restore_p2p_state_timer, padapter->pnetdev, restore_p2p_state_timer_process, padapter);
1870         _init_timer(&pwdinfo->pre_tx_scan_timer, padapter->pnetdev, pre_tx_scan_timer_process, padapter);
1871         _init_timer(&pwdinfo->reset_ch_sitesurvey, padapter->pnetdev, reset_ch_sitesurvey_timer_process, padapter);
1872         _init_timer(&pwdinfo->reset_ch_sitesurvey2, padapter->pnetdev, reset_ch_sitesurvey_timer_process2, padapter);
1873 }
1874
1875 void rtw_init_wifidirect_addrs(struct adapter *padapter, u8 *dev_addr, u8 *iface_addr)
1876 {
1877 #ifdef CONFIG_88EU_P2P
1878         struct wifidirect_info *pwdinfo = &padapter->wdinfo;
1879
1880         /*init device&interface address */
1881         if (dev_addr)
1882                 memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN);
1883         if (iface_addr)
1884                 memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN);
1885 #endif
1886 }
1887
1888 void init_wifidirect_info(struct adapter *padapter, enum P2P_ROLE role)
1889 {
1890         struct wifidirect_info  *pwdinfo;
1891
1892         pwdinfo = &padapter->wdinfo;
1893         pwdinfo->padapter = padapter;
1894
1895         /*      1, 6, 11 are the social channel defined in the WiFi Direct specification. */
1896         pwdinfo->social_chan[0] = 1;
1897         pwdinfo->social_chan[1] = 6;
1898         pwdinfo->social_chan[2] = 11;
1899         pwdinfo->social_chan[3] = 0;    /*      channel 0 for scanning ending in site survey function. */
1900
1901         /*      Use the channel 11 as the listen channel */
1902         pwdinfo->listen_channel = 11;
1903
1904         if (role == P2P_ROLE_DEVICE) {
1905                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
1906                 rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
1907                 pwdinfo->intent = 1;
1908                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_LISTEN);
1909         } else if (role == P2P_ROLE_CLIENT) {
1910                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
1911                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
1912                 pwdinfo->intent = 1;
1913                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
1914         } else if (role == P2P_ROLE_GO) {
1915                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
1916                 rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
1917                 pwdinfo->intent = 15;
1918                 rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
1919         }
1920
1921 /*      Use the OFDM rate in the P2P probe response frame. (6(B), 9(B), 12, 18, 24, 36, 48, 54) */
1922         pwdinfo->support_rate[0] = 0x8c;        /*      6(B) */
1923         pwdinfo->support_rate[1] = 0x92;        /*      9(B) */
1924         pwdinfo->support_rate[2] = 0x18;        /*      12 */
1925         pwdinfo->support_rate[3] = 0x24;        /*      18 */
1926         pwdinfo->support_rate[4] = 0x30;        /*      24 */
1927         pwdinfo->support_rate[5] = 0x48;        /*      36 */
1928         pwdinfo->support_rate[6] = 0x60;        /*      48 */
1929         pwdinfo->support_rate[7] = 0x6c;        /*      54 */
1930
1931         memcpy(pwdinfo->p2p_wildcard_ssid, "DIRECT-", 7);
1932
1933         _rtw_memset(pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN);
1934         pwdinfo->device_name_len = 0;
1935
1936         _rtw_memset(&pwdinfo->invitereq_info, 0x00, sizeof(struct tx_invite_req_info));
1937         pwdinfo->invitereq_info.token = 3;      /*      Token used for P2P invitation request frame. */
1938
1939         _rtw_memset(&pwdinfo->inviteresp_info, 0x00, sizeof(struct tx_invite_resp_info));
1940         pwdinfo->inviteresp_info.token = 0;
1941
1942         pwdinfo->profileindex = 0;
1943         _rtw_memset(&pwdinfo->profileinfo[0], 0x00, sizeof(struct profile_info) * P2P_MAX_PERSISTENT_GROUP_NUM);
1944
1945         rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
1946
1947         pwdinfo->listen_dwell = (u8) ((jiffies % 3) + 1);
1948
1949         _rtw_memset(&pwdinfo->tx_prov_disc_info, 0x00, sizeof(struct tx_provdisc_req_info));
1950         pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_NONE;
1951
1952         _rtw_memset(&pwdinfo->nego_req_info, 0x00, sizeof(struct tx_nego_req_info));
1953
1954         pwdinfo->device_password_id_for_nego = WPS_DPID_PBC;
1955         pwdinfo->negotiation_dialog_token = 1;
1956
1957         _rtw_memset(pwdinfo->nego_ssid, 0x00, WLAN_SSID_MAXLEN);
1958         pwdinfo->nego_ssidlen = 0;
1959
1960         pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO;
1961         pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD;
1962         pwdinfo->channel_list_attr_len = 0;
1963         _rtw_memset(pwdinfo->channel_list_attr, 0x00, 100);
1964
1965         _rtw_memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, 0x00, 4);
1966         _rtw_memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, '0', 3);
1967         _rtw_memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
1968         pwdinfo->wfd_tdls_enable = 0;
1969         _rtw_memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
1970         _rtw_memset(pwdinfo->p2p_peer_device_addr, 0x00, ETH_ALEN);
1971
1972         pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
1973         pwdinfo->rx_invitereq_info.operation_ch[1] = 0; /*      Used to indicate the scan end in site survey function */
1974         pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
1975         pwdinfo->p2p_info.operation_ch[0] = 0;
1976         pwdinfo->p2p_info.operation_ch[1] = 0;                  /*      Used to indicate the scan end in site survey function */
1977         pwdinfo->p2p_info.scan_op_ch_only = 0;
1978 }
1979
1980 int rtw_p2p_enable(struct adapter *padapter, enum P2P_ROLE role)
1981 {
1982         int ret = _SUCCESS;
1983         struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
1984
1985         if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT || role == P2P_ROLE_GO) {
1986                 /* leave IPS/Autosuspend */
1987                 if (_FAIL == rtw_pwr_wakeup(padapter)) {
1988                         ret = _FAIL;
1989                         goto exit;
1990                 }
1991
1992                 /*      Added by Albert 2011/03/22 */
1993                 /*      In the P2P mode, the driver should not support the b mode. */
1994                 /*      So, the Tx packet shouldn't use the CCK rate */
1995                 update_tx_basic_rate(padapter, WIRELESS_11AGN);
1996
1997                 /* Enable P2P function */
1998                 init_wifidirect_info(padapter, role);
1999
2000                 rtw_hal_set_odm_var(padapter, HAL_ODM_P2P_STATE, NULL, true);
2001         } else if (role == P2P_ROLE_DISABLE) {
2002                 if (_FAIL == rtw_pwr_wakeup(padapter)) {
2003                         ret = _FAIL;
2004                         goto exit;
2005                 }
2006
2007                 /* Disable P2P function */
2008                 if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
2009                         del_timer_sync(&pwdinfo->find_phase_timer);
2010                         del_timer_sync(&pwdinfo->restore_p2p_state_timer);
2011                         del_timer_sync(&pwdinfo->pre_tx_scan_timer);
2012                         del_timer_sync(&pwdinfo->reset_ch_sitesurvey);
2013                         del_timer_sync(&pwdinfo->reset_ch_sitesurvey2);
2014                         reset_ch_sitesurvey_timer_process(padapter);
2015                         reset_ch_sitesurvey_timer_process2(padapter);
2016                         rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
2017                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DISABLE);
2018                         _rtw_memset(&pwdinfo->rx_prov_disc_info, 0x00, sizeof(struct rx_provdisc_req_info));
2019                 }
2020
2021                 rtw_hal_set_odm_var(padapter, HAL_ODM_P2P_STATE, NULL, false);
2022
2023                 /* Restore to initial setting. */
2024                 update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
2025         }
2026
2027 exit:
2028         return ret;
2029 }
2030
2031 #else
2032 u8 p2p_ps_wk_cmd(struct adapter *padapter, u8 p2p_ps_state, u8 enqueue)
2033 {
2034         return _FAIL;
2035 }
2036
2037 void process_p2p_ps_ie(struct adapter *padapter, u8 *IEs, u32 IELength)
2038 {
2039 }
2040
2041 #endif /* CONFIG_88EU_P2P */