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