Line data Source code
1 : /*
2 : * Wi-Fi Direct - P2P group operations
3 : * Copyright (c) 2009-2010, Atheros Communications
4 : *
5 : * This software may be distributed under the terms of the BSD license.
6 : * See README for more details.
7 : */
8 :
9 : #include "includes.h"
10 :
11 : #include "common.h"
12 : #include "common/ieee802_11_defs.h"
13 : #include "common/ieee802_11_common.h"
14 : #include "common/wpa_ctrl.h"
15 : #include "wps/wps_defs.h"
16 : #include "wps/wps_i.h"
17 : #include "p2p_i.h"
18 : #include "p2p.h"
19 :
20 :
21 : struct p2p_group_member {
22 : struct p2p_group_member *next;
23 : u8 addr[ETH_ALEN]; /* P2P Interface Address */
24 : u8 dev_addr[ETH_ALEN]; /* P2P Device Address */
25 : struct wpabuf *p2p_ie;
26 : struct wpabuf *wfd_ie;
27 : struct wpabuf *client_info;
28 : u8 dev_capab;
29 : };
30 :
31 : /**
32 : * struct p2p_group - Internal P2P module per-group data
33 : */
34 : struct p2p_group {
35 : struct p2p_data *p2p;
36 : struct p2p_group_config *cfg;
37 : struct p2p_group_member *members;
38 : unsigned int num_members;
39 : int group_formation;
40 : int beacon_update;
41 : struct wpabuf *noa;
42 : struct wpabuf *wfd_ie;
43 : };
44 :
45 :
46 403 : struct p2p_group * p2p_group_init(struct p2p_data *p2p,
47 : struct p2p_group_config *config)
48 : {
49 : struct p2p_group *group, **groups;
50 :
51 403 : group = os_zalloc(sizeof(*group));
52 403 : if (group == NULL)
53 0 : return NULL;
54 :
55 403 : groups = os_realloc_array(p2p->groups, p2p->num_groups + 1,
56 : sizeof(struct p2p_group *));
57 403 : if (groups == NULL) {
58 0 : os_free(group);
59 0 : return NULL;
60 : }
61 403 : groups[p2p->num_groups++] = group;
62 403 : p2p->groups = groups;
63 :
64 403 : group->p2p = p2p;
65 403 : group->cfg = config;
66 403 : group->group_formation = 1;
67 403 : group->beacon_update = 1;
68 403 : p2p_group_update_ies(group);
69 403 : group->cfg->idle_update(group->cfg->cb_ctx, 1);
70 :
71 403 : return group;
72 : }
73 :
74 :
75 536 : static void p2p_group_free_member(struct p2p_group_member *m)
76 : {
77 536 : wpabuf_free(m->wfd_ie);
78 536 : wpabuf_free(m->p2p_ie);
79 536 : wpabuf_free(m->client_info);
80 536 : os_free(m);
81 536 : }
82 :
83 :
84 403 : static void p2p_group_free_members(struct p2p_group *group)
85 : {
86 : struct p2p_group_member *m, *prev;
87 403 : m = group->members;
88 403 : group->members = NULL;
89 403 : group->num_members = 0;
90 1016 : while (m) {
91 210 : prev = m;
92 210 : m = m->next;
93 210 : p2p_group_free_member(prev);
94 : }
95 403 : }
96 :
97 :
98 442 : void p2p_group_deinit(struct p2p_group *group)
99 : {
100 : size_t g;
101 : struct p2p_data *p2p;
102 :
103 442 : if (group == NULL)
104 481 : return;
105 :
106 403 : p2p = group->p2p;
107 :
108 2356 : for (g = 0; g < p2p->num_groups; g++) {
109 2356 : if (p2p->groups[g] == group) {
110 806 : while (g + 1 < p2p->num_groups) {
111 0 : p2p->groups[g] = p2p->groups[g + 1];
112 0 : g++;
113 : }
114 403 : p2p->num_groups--;
115 403 : break;
116 : }
117 : }
118 :
119 403 : p2p_group_free_members(group);
120 403 : os_free(group->cfg);
121 403 : wpabuf_free(group->noa);
122 403 : wpabuf_free(group->wfd_ie);
123 403 : os_free(group);
124 : }
125 :
126 :
127 761 : static void p2p_client_info(struct wpabuf *ie, struct p2p_group_member *m)
128 : {
129 761 : if (m->client_info == NULL)
130 13 : return;
131 748 : if (wpabuf_tailroom(ie) < wpabuf_len(m->client_info) + 1)
132 0 : return;
133 748 : wpabuf_put_buf(ie, m->client_info);
134 : }
135 :
136 :
137 2463 : static void p2p_group_add_common_ies(struct p2p_group *group,
138 : struct wpabuf *ie)
139 : {
140 2463 : u8 dev_capab = group->p2p->dev_capab, group_capab = 0;
141 :
142 : /* P2P Capability */
143 2463 : dev_capab &= ~P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY;
144 2463 : group_capab |= P2P_GROUP_CAPAB_GROUP_OWNER;
145 2463 : if (group->cfg->persistent_group) {
146 824 : group_capab |= P2P_GROUP_CAPAB_PERSISTENT_GROUP;
147 824 : if (group->cfg->persistent_group == 2)
148 584 : group_capab |= P2P_GROUP_CAPAB_PERSISTENT_RECONN;
149 : }
150 2463 : if (group->p2p->cfg->p2p_intra_bss)
151 2463 : group_capab |= P2P_GROUP_CAPAB_INTRA_BSS_DIST;
152 2463 : if (group->group_formation)
153 955 : group_capab |= P2P_GROUP_CAPAB_GROUP_FORMATION;
154 2463 : if (group->p2p->cross_connect)
155 8 : group_capab |= P2P_GROUP_CAPAB_CROSS_CONN;
156 2463 : if (group->num_members >= group->cfg->max_clients)
157 0 : group_capab |= P2P_GROUP_CAPAB_GROUP_LIMIT;
158 2463 : if (group->cfg->ip_addr_alloc)
159 869 : group_capab |= P2P_GROUP_CAPAB_IP_ADDR_ALLOCATION;
160 2463 : p2p_buf_add_capability(ie, dev_capab, group_capab);
161 2463 : }
162 :
163 :
164 2463 : static void p2p_group_add_noa(struct wpabuf *ie, struct wpabuf *noa)
165 : {
166 2463 : if (noa == NULL)
167 4926 : return;
168 : /* Notice of Absence */
169 0 : wpabuf_put_u8(ie, P2P_ATTR_NOTICE_OF_ABSENCE);
170 0 : wpabuf_put_le16(ie, wpabuf_len(noa));
171 0 : wpabuf_put_buf(ie, noa);
172 : }
173 :
174 :
175 1659 : static struct wpabuf * p2p_group_encaps_probe_resp(struct wpabuf *subelems)
176 : {
177 : struct wpabuf *ie;
178 : const u8 *pos, *end;
179 : size_t len;
180 :
181 1659 : if (subelems == NULL)
182 0 : return NULL;
183 :
184 1659 : len = wpabuf_len(subelems) + 100;
185 :
186 1659 : ie = wpabuf_alloc(len);
187 1659 : if (ie == NULL)
188 0 : return NULL;
189 :
190 1659 : pos = wpabuf_head(subelems);
191 1659 : end = pos + wpabuf_len(subelems);
192 :
193 4979 : while (end > pos) {
194 1661 : size_t frag_len = end - pos;
195 1661 : if (frag_len > 251)
196 2 : frag_len = 251;
197 1661 : wpabuf_put_u8(ie, WLAN_EID_VENDOR_SPECIFIC);
198 1661 : wpabuf_put_u8(ie, 4 + frag_len);
199 1661 : wpabuf_put_be32(ie, P2P_IE_VENDOR_TYPE);
200 1661 : wpabuf_put_data(ie, pos, frag_len);
201 1661 : pos += frag_len;
202 : }
203 :
204 1659 : return ie;
205 : }
206 :
207 :
208 804 : static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group)
209 : {
210 : struct wpabuf *ie;
211 : u8 *len;
212 804 : size_t extra = 0;
213 :
214 : #ifdef CONFIG_WIFI_DISPLAY
215 804 : if (group->p2p->wfd_ie_beacon)
216 14 : extra = wpabuf_len(group->p2p->wfd_ie_beacon);
217 : #endif /* CONFIG_WIFI_DISPLAY */
218 :
219 1152 : if (group->p2p->vendor_elem &&
220 348 : group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO])
221 2 : extra += wpabuf_len(group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO]);
222 :
223 804 : ie = wpabuf_alloc(257 + extra);
224 804 : if (ie == NULL)
225 0 : return NULL;
226 :
227 : #ifdef CONFIG_WIFI_DISPLAY
228 804 : if (group->p2p->wfd_ie_beacon)
229 14 : wpabuf_put_buf(ie, group->p2p->wfd_ie_beacon);
230 : #endif /* CONFIG_WIFI_DISPLAY */
231 :
232 1152 : if (group->p2p->vendor_elem &&
233 348 : group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO])
234 2 : wpabuf_put_buf(ie,
235 2 : group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO]);
236 :
237 804 : len = p2p_buf_add_ie_hdr(ie);
238 804 : p2p_group_add_common_ies(group, ie);
239 804 : p2p_buf_add_device_id(ie, group->p2p->cfg->dev_addr);
240 804 : p2p_group_add_noa(ie, group->noa);
241 804 : p2p_buf_update_ie_hdr(ie, len);
242 :
243 804 : return ie;
244 : }
245 :
246 :
247 : #ifdef CONFIG_WIFI_DISPLAY
248 :
249 7 : struct wpabuf * p2p_group_get_wfd_ie(struct p2p_group *g)
250 : {
251 7 : return g->wfd_ie;
252 : }
253 :
254 :
255 417 : struct wpabuf * wifi_display_encaps(struct wpabuf *subelems)
256 : {
257 : struct wpabuf *ie;
258 : const u8 *pos, *end;
259 :
260 417 : if (subelems == NULL)
261 0 : return NULL;
262 :
263 417 : ie = wpabuf_alloc(wpabuf_len(subelems) + 100);
264 417 : if (ie == NULL)
265 0 : return NULL;
266 :
267 417 : pos = wpabuf_head(subelems);
268 417 : end = pos + wpabuf_len(subelems);
269 :
270 1147 : while (end > pos) {
271 313 : size_t frag_len = end - pos;
272 313 : if (frag_len > 251)
273 0 : frag_len = 251;
274 313 : wpabuf_put_u8(ie, WLAN_EID_VENDOR_SPECIFIC);
275 313 : wpabuf_put_u8(ie, 4 + frag_len);
276 313 : wpabuf_put_be32(ie, WFD_IE_VENDOR_TYPE);
277 313 : wpabuf_put_data(ie, pos, frag_len);
278 313 : pos += frag_len;
279 : }
280 :
281 417 : return ie;
282 : }
283 :
284 :
285 30 : static int wifi_display_add_dev_info_descr(struct wpabuf *buf,
286 : struct p2p_group_member *m)
287 : {
288 : const u8 *pos, *end;
289 30 : const u8 *dev_info = NULL;
290 30 : const u8 *assoc_bssid = NULL;
291 30 : const u8 *coupled_sink = NULL;
292 : u8 zero_addr[ETH_ALEN];
293 :
294 30 : if (m->wfd_ie == NULL)
295 2 : return 0;
296 :
297 28 : os_memset(zero_addr, 0, ETH_ALEN);
298 28 : pos = wpabuf_head_u8(m->wfd_ie);
299 28 : end = pos + wpabuf_len(m->wfd_ie);
300 104 : while (end - pos >= 3) {
301 : u8 id;
302 : u16 len;
303 :
304 54 : id = *pos++;
305 54 : len = WPA_GET_BE16(pos);
306 54 : pos += 2;
307 54 : if (len > end - pos)
308 6 : break;
309 :
310 48 : switch (id) {
311 : case WFD_SUBELEM_DEVICE_INFO:
312 28 : if (len < 6)
313 6 : break;
314 22 : dev_info = pos;
315 22 : break;
316 : case WFD_SUBELEM_ASSOCIATED_BSSID:
317 10 : if (len < ETH_ALEN)
318 6 : break;
319 4 : assoc_bssid = pos;
320 4 : break;
321 : case WFD_SUBELEM_COUPLED_SINK:
322 10 : if (len < 1 + ETH_ALEN)
323 6 : break;
324 4 : coupled_sink = pos;
325 4 : break;
326 : }
327 :
328 48 : pos += len;
329 : }
330 :
331 28 : if (dev_info == NULL)
332 6 : return 0;
333 :
334 22 : wpabuf_put_u8(buf, 23);
335 22 : wpabuf_put_data(buf, m->dev_addr, ETH_ALEN);
336 22 : if (assoc_bssid)
337 4 : wpabuf_put_data(buf, assoc_bssid, ETH_ALEN);
338 : else
339 18 : wpabuf_put_data(buf, zero_addr, ETH_ALEN);
340 22 : wpabuf_put_data(buf, dev_info, 2); /* WFD Device Info */
341 22 : wpabuf_put_data(buf, dev_info + 4, 2); /* WFD Device Max Throughput */
342 22 : if (coupled_sink) {
343 4 : wpabuf_put_data(buf, coupled_sink, 1 + ETH_ALEN);
344 : } else {
345 18 : wpabuf_put_u8(buf, 0);
346 18 : wpabuf_put_data(buf, zero_addr, ETH_ALEN);
347 : }
348 :
349 22 : return 1;
350 : }
351 :
352 :
353 : static struct wpabuf *
354 1659 : wifi_display_build_go_ie(struct p2p_group *group)
355 : {
356 : struct wpabuf *wfd_subelems, *wfd_ie;
357 : struct p2p_group_member *m;
358 : u8 *len;
359 1659 : unsigned int count = 0;
360 :
361 1659 : if (!group->p2p->wfd_ie_probe_resp)
362 1618 : return NULL;
363 :
364 82 : wfd_subelems = wpabuf_alloc(wpabuf_len(group->p2p->wfd_ie_probe_resp) +
365 41 : group->num_members * 24 + 100);
366 41 : if (wfd_subelems == NULL)
367 0 : return NULL;
368 41 : if (group->p2p->wfd_dev_info)
369 41 : wpabuf_put_buf(wfd_subelems, group->p2p->wfd_dev_info);
370 41 : if (group->p2p->wfd_assoc_bssid)
371 0 : wpabuf_put_buf(wfd_subelems,
372 0 : group->p2p->wfd_assoc_bssid);
373 41 : if (group->p2p->wfd_coupled_sink_info)
374 0 : wpabuf_put_buf(wfd_subelems,
375 0 : group->p2p->wfd_coupled_sink_info);
376 :
377 : /* Build WFD Session Info */
378 41 : wpabuf_put_u8(wfd_subelems, WFD_SUBELEM_SESSION_INFO);
379 41 : len = wpabuf_put(wfd_subelems, 2);
380 41 : m = group->members;
381 112 : while (m) {
382 30 : if (wifi_display_add_dev_info_descr(wfd_subelems, m))
383 22 : count++;
384 30 : m = m->next;
385 : }
386 :
387 41 : if (count == 0) {
388 : /* No Wi-Fi Display clients - do not include subelement */
389 23 : wfd_subelems->used -= 3;
390 : } else {
391 18 : WPA_PUT_BE16(len, (u8 *) wpabuf_put(wfd_subelems, 0) - len -
392 : 2);
393 18 : p2p_dbg(group->p2p, "WFD: WFD Session Info: %u descriptors",
394 : count);
395 : }
396 :
397 41 : wfd_ie = wifi_display_encaps(wfd_subelems);
398 41 : wpabuf_free(wfd_subelems);
399 :
400 41 : return wfd_ie;
401 : }
402 :
403 1659 : static void wifi_display_group_update(struct p2p_group *group)
404 : {
405 1659 : wpabuf_free(group->wfd_ie);
406 1659 : group->wfd_ie = wifi_display_build_go_ie(group);
407 1659 : }
408 :
409 : #endif /* CONFIG_WIFI_DISPLAY */
410 :
411 :
412 715 : void p2p_buf_add_group_info(struct p2p_group *group, struct wpabuf *buf,
413 : int max_clients)
414 : {
415 : u8 *group_info;
416 715 : int count = 0;
417 : struct p2p_group_member *m;
418 :
419 715 : p2p_dbg(group->p2p, "* P2P Group Info");
420 715 : group_info = wpabuf_put(buf, 0);
421 715 : wpabuf_put_u8(buf, P2P_ATTR_GROUP_INFO);
422 715 : wpabuf_put_le16(buf, 0); /* Length to be filled */
423 1476 : for (m = group->members; m; m = m->next) {
424 761 : p2p_client_info(buf, m);
425 761 : count++;
426 761 : if (max_clients >= 0 && count >= max_clients)
427 0 : break;
428 : }
429 715 : WPA_PUT_LE16(group_info + 1,
430 715 : (u8 *) wpabuf_put(buf, 0) - group_info - 3);
431 715 : }
432 :
433 :
434 5 : void p2p_group_buf_add_id(struct p2p_group *group, struct wpabuf *buf)
435 : {
436 5 : p2p_buf_add_group_id(buf, group->p2p->cfg->dev_addr, group->cfg->ssid,
437 5 : group->cfg->ssid_len);
438 5 : }
439 :
440 :
441 1659 : static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group)
442 : {
443 : struct wpabuf *p2p_subelems, *ie;
444 :
445 1659 : p2p_subelems = wpabuf_alloc(500);
446 1659 : if (p2p_subelems == NULL)
447 0 : return NULL;
448 :
449 1659 : p2p_group_add_common_ies(group, p2p_subelems);
450 1659 : p2p_group_add_noa(p2p_subelems, group->noa);
451 :
452 : /* P2P Device Info */
453 1659 : p2p_buf_add_device_info(p2p_subelems, group->p2p, NULL);
454 :
455 : /* P2P Group Info: Only when at least one P2P Client is connected */
456 1659 : if (group->members)
457 710 : p2p_buf_add_group_info(group, p2p_subelems, -1);
458 :
459 1659 : ie = p2p_group_encaps_probe_resp(p2p_subelems);
460 1659 : wpabuf_free(p2p_subelems);
461 :
462 2435 : if (group->p2p->vendor_elem &&
463 776 : group->p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P_GO]) {
464 : struct wpabuf *extra;
465 16 : extra = wpabuf_dup(group->p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P_GO]);
466 16 : ie = wpabuf_concat(extra, ie);
467 : }
468 :
469 : #ifdef CONFIG_WIFI_DISPLAY
470 1659 : if (group->wfd_ie) {
471 41 : struct wpabuf *wfd = wpabuf_dup(group->wfd_ie);
472 41 : ie = wpabuf_concat(wfd, ie);
473 : }
474 : #endif /* CONFIG_WIFI_DISPLAY */
475 :
476 1659 : return ie;
477 : }
478 :
479 :
480 1659 : void p2p_group_update_ies(struct p2p_group *group)
481 : {
482 : struct wpabuf *beacon_ie;
483 : struct wpabuf *probe_resp_ie;
484 :
485 : #ifdef CONFIG_WIFI_DISPLAY
486 1659 : wifi_display_group_update(group);
487 : #endif /* CONFIG_WIFI_DISPLAY */
488 :
489 1659 : probe_resp_ie = p2p_group_build_probe_resp_ie(group);
490 1659 : if (probe_resp_ie == NULL)
491 1659 : return;
492 1659 : wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Probe Response P2P IE",
493 : probe_resp_ie);
494 :
495 1659 : if (group->beacon_update) {
496 804 : beacon_ie = p2p_group_build_beacon_ie(group);
497 804 : if (beacon_ie)
498 804 : group->beacon_update = 0;
499 804 : wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Beacon P2P IE",
500 : beacon_ie);
501 : } else
502 855 : beacon_ie = NULL;
503 :
504 1659 : group->cfg->ie_update(group->cfg->cb_ctx, beacon_ie, probe_resp_ie);
505 : }
506 :
507 :
508 : /**
509 : * p2p_build_client_info - Build P2P Client Info Descriptor
510 : * @addr: MAC address of the peer device
511 : * @p2p_ie: P2P IE from (Re)Association Request
512 : * @dev_capab: Buffer for returning Device Capability
513 : * @dev_addr: Buffer for returning P2P Device Address
514 : * Returns: P2P Client Info Descriptor or %NULL on failure
515 : *
516 : * This function builds P2P Client Info Descriptor based on the information
517 : * available from (Re)Association Request frame. Group owner can use this to
518 : * build the P2P Group Info attribute for Probe Response frames.
519 : */
520 523 : static struct wpabuf * p2p_build_client_info(const u8 *addr,
521 : struct wpabuf *p2p_ie,
522 : u8 *dev_capab, u8 *dev_addr)
523 : {
524 : const u8 *spos;
525 : struct p2p_message msg;
526 : u8 *len_pos;
527 : struct wpabuf *buf;
528 :
529 523 : if (p2p_ie == NULL)
530 0 : return NULL;
531 :
532 523 : os_memset(&msg, 0, sizeof(msg));
533 1046 : if (p2p_parse_p2p_ie(p2p_ie, &msg) ||
534 1046 : msg.capability == NULL || msg.p2p_device_info == NULL)
535 0 : return NULL;
536 :
537 523 : buf = wpabuf_alloc(ETH_ALEN + 1 + 1 + msg.p2p_device_info_len);
538 523 : if (buf == NULL)
539 0 : return NULL;
540 :
541 523 : *dev_capab = msg.capability[0];
542 523 : os_memcpy(dev_addr, msg.p2p_device_addr, ETH_ALEN);
543 :
544 523 : spos = msg.p2p_device_info; /* P2P Device address */
545 :
546 : /* P2P Client Info Descriptor */
547 : /* Length to be set */
548 523 : len_pos = wpabuf_put(buf, 1);
549 : /* P2P Device address */
550 523 : wpabuf_put_data(buf, spos, ETH_ALEN);
551 : /* P2P Interface address */
552 523 : wpabuf_put_data(buf, addr, ETH_ALEN);
553 : /* Device Capability Bitmap */
554 523 : wpabuf_put_u8(buf, msg.capability[0]);
555 : /*
556 : * Config Methods, Primary Device Type, Number of Secondary Device
557 : * Types, Secondary Device Type List, Device Name copied from
558 : * Device Info
559 : */
560 523 : wpabuf_put_data(buf, spos + ETH_ALEN,
561 523 : msg.p2p_device_info_len - ETH_ALEN);
562 :
563 523 : *len_pos = wpabuf_len(buf) - 1;
564 :
565 :
566 523 : return buf;
567 : }
568 :
569 :
570 1253 : static int p2p_group_remove_member(struct p2p_group *group, const u8 *addr)
571 : {
572 : struct p2p_group_member *m, *prev;
573 :
574 1253 : if (group == NULL)
575 398 : return 0;
576 :
577 855 : m = group->members;
578 855 : prev = NULL;
579 1760 : while (m) {
580 376 : if (os_memcmp(m->addr, addr, ETH_ALEN) == 0)
581 326 : break;
582 50 : prev = m;
583 50 : m = m->next;
584 : }
585 :
586 855 : if (m == NULL)
587 529 : return 0;
588 :
589 326 : if (prev)
590 4 : prev->next = m->next;
591 : else
592 322 : group->members = m->next;
593 326 : p2p_group_free_member(m);
594 326 : group->num_members--;
595 :
596 326 : return 1;
597 : }
598 :
599 :
600 576 : int p2p_group_notif_assoc(struct p2p_group *group, const u8 *addr,
601 : const u8 *ie, size_t len)
602 : {
603 : struct p2p_group_member *m;
604 :
605 576 : if (group == NULL)
606 40 : return -1;
607 :
608 536 : p2p_add_device(group->p2p, addr, 0, NULL, 0, ie, len, 0);
609 :
610 536 : m = os_zalloc(sizeof(*m));
611 536 : if (m == NULL)
612 0 : return -1;
613 536 : os_memcpy(m->addr, addr, ETH_ALEN);
614 536 : m->p2p_ie = ieee802_11_vendor_ie_concat(ie, len, P2P_IE_VENDOR_TYPE);
615 536 : if (m->p2p_ie) {
616 523 : m->client_info = p2p_build_client_info(addr, m->p2p_ie,
617 : &m->dev_capab,
618 523 : m->dev_addr);
619 : }
620 : #ifdef CONFIG_WIFI_DISPLAY
621 536 : m->wfd_ie = ieee802_11_vendor_ie_concat(ie, len, WFD_IE_VENDOR_TYPE);
622 : #endif /* CONFIG_WIFI_DISPLAY */
623 :
624 536 : p2p_group_remove_member(group, addr);
625 :
626 536 : m->next = group->members;
627 536 : group->members = m;
628 536 : group->num_members++;
629 5360 : p2p_dbg(group->p2p, "Add client " MACSTR
630 : " to group (p2p=%d wfd=%d client_info=%d); num_members=%u/%u",
631 4288 : MAC2STR(addr), m->p2p_ie ? 1 : 0, m->wfd_ie ? 1 : 0,
632 536 : m->client_info ? 1 : 0,
633 536 : group->num_members, group->cfg->max_clients);
634 536 : if (group->num_members == group->cfg->max_clients)
635 0 : group->beacon_update = 1;
636 536 : p2p_group_update_ies(group);
637 536 : if (group->num_members == 1)
638 490 : group->cfg->idle_update(group->cfg->cb_ctx, 0);
639 :
640 536 : return 0;
641 : }
642 :
643 :
644 2629 : struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status)
645 : {
646 : struct wpabuf *resp;
647 : u8 *rlen;
648 2629 : size_t extra = 0;
649 :
650 : #ifdef CONFIG_WIFI_DISPLAY
651 2629 : if (group->wfd_ie)
652 74 : extra = wpabuf_len(group->wfd_ie);
653 : #endif /* CONFIG_WIFI_DISPLAY */
654 :
655 3921 : if (group->p2p->vendor_elem &&
656 1292 : group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP])
657 11 : extra += wpabuf_len(group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP]);
658 :
659 : /*
660 : * (Re)Association Response - P2P IE
661 : * Status attribute (shall be present when association request is
662 : * denied)
663 : * Extended Listen Timing (may be present)
664 : */
665 2629 : resp = wpabuf_alloc(20 + extra);
666 2629 : if (resp == NULL)
667 0 : return NULL;
668 :
669 : #ifdef CONFIG_WIFI_DISPLAY
670 2629 : if (group->wfd_ie)
671 74 : wpabuf_put_buf(resp, group->wfd_ie);
672 : #endif /* CONFIG_WIFI_DISPLAY */
673 :
674 3921 : if (group->p2p->vendor_elem &&
675 1292 : group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP])
676 11 : wpabuf_put_buf(resp,
677 11 : group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP]);
678 :
679 2629 : rlen = p2p_buf_add_ie_hdr(resp);
680 2629 : if (status != P2P_SC_SUCCESS)
681 0 : p2p_buf_add_status(resp, status);
682 2629 : p2p_buf_update_ie_hdr(resp, rlen);
683 :
684 2629 : return resp;
685 : }
686 :
687 :
688 717 : void p2p_group_notif_disassoc(struct p2p_group *group, const u8 *addr)
689 : {
690 717 : if (p2p_group_remove_member(group, addr)) {
691 2233 : p2p_dbg(group->p2p, "Remove client " MACSTR
692 : " from group; num_members=%u/%u",
693 1914 : MAC2STR(addr), group->num_members,
694 319 : group->cfg->max_clients);
695 319 : if (group->num_members == group->cfg->max_clients - 1)
696 0 : group->beacon_update = 1;
697 319 : p2p_group_update_ies(group);
698 319 : if (group->num_members == 0)
699 288 : group->cfg->idle_update(group->cfg->cb_ctx, 1);
700 : }
701 717 : }
702 :
703 :
704 : /**
705 : * p2p_match_dev_type_member - Match client device type with requested type
706 : * @m: Group member
707 : * @wps: WPS TLVs from Probe Request frame (concatenated WPS IEs)
708 : * Returns: 1 on match, 0 on mismatch
709 : *
710 : * This function can be used to match the Requested Device Type attribute in
711 : * WPS IE with the device types of a group member for deciding whether a GO
712 : * should reply to a Probe Request frame.
713 : */
714 3 : static int p2p_match_dev_type_member(struct p2p_group_member *m,
715 : struct wpabuf *wps)
716 : {
717 : const u8 *pos, *end;
718 : struct wps_parse_attr attr;
719 : u8 num_sec;
720 :
721 3 : if (m->client_info == NULL || wps == NULL)
722 0 : return 0;
723 :
724 3 : pos = wpabuf_head(m->client_info);
725 3 : end = pos + wpabuf_len(m->client_info);
726 :
727 3 : pos += 1 + 2 * ETH_ALEN + 1 + 2;
728 3 : if (end - pos < WPS_DEV_TYPE_LEN + 1)
729 0 : return 0;
730 :
731 3 : if (wps_parse_msg(wps, &attr))
732 0 : return 1; /* assume no Requested Device Type attributes */
733 :
734 3 : if (attr.num_req_dev_type == 0)
735 0 : return 1; /* no Requested Device Type attributes -> match */
736 :
737 3 : if (dev_type_list_match(pos, attr.req_dev_type, attr.num_req_dev_type))
738 0 : return 1; /* Match with client Primary Device Type */
739 :
740 3 : pos += WPS_DEV_TYPE_LEN;
741 3 : num_sec = *pos++;
742 3 : if (end - pos < num_sec * WPS_DEV_TYPE_LEN)
743 0 : return 0;
744 8 : while (num_sec > 0) {
745 3 : num_sec--;
746 3 : if (dev_type_list_match(pos, attr.req_dev_type,
747 3 : attr.num_req_dev_type))
748 1 : return 1; /* Match with client Secondary Device Type */
749 2 : pos += WPS_DEV_TYPE_LEN;
750 : }
751 :
752 : /* No matching device type found */
753 2 : return 0;
754 : }
755 :
756 :
757 509 : int p2p_group_match_dev_type(struct p2p_group *group, struct wpabuf *wps)
758 : {
759 : struct p2p_group_member *m;
760 :
761 509 : if (p2p_match_dev_type(group->p2p, wps))
762 506 : return 1; /* Match with own device type */
763 :
764 5 : for (m = group->members; m; m = m->next) {
765 3 : if (p2p_match_dev_type_member(m, wps))
766 1 : return 1; /* Match with group client device type */
767 : }
768 :
769 : /* No match with Requested Device Type */
770 2 : return 0;
771 : }
772 :
773 :
774 505 : int p2p_group_match_dev_id(struct p2p_group *group, struct wpabuf *p2p)
775 : {
776 : struct p2p_group_member *m;
777 : struct p2p_message msg;
778 :
779 505 : os_memset(&msg, 0, sizeof(msg));
780 505 : if (p2p_parse_p2p_ie(p2p, &msg))
781 0 : return 1; /* Failed to parse - assume no filter on Device ID */
782 :
783 505 : if (!msg.device_id)
784 501 : return 1; /* No filter on Device ID */
785 :
786 4 : if (os_memcmp(msg.device_id, group->p2p->cfg->dev_addr, ETH_ALEN) == 0)
787 0 : return 1; /* Match with our P2P Device Address */
788 :
789 7 : for (m = group->members; m; m = m->next) {
790 4 : if (os_memcmp(msg.device_id, m->dev_addr, ETH_ALEN) == 0)
791 1 : return 1; /* Match with group client P2P Device Address */
792 : }
793 :
794 : /* No match with Device ID */
795 3 : return 0;
796 : }
797 :
798 :
799 397 : void p2p_group_notif_formation_done(struct p2p_group *group)
800 : {
801 397 : if (group == NULL)
802 397 : return;
803 397 : group->group_formation = 0;
804 397 : group->beacon_update = 1;
805 397 : p2p_group_update_ies(group);
806 : }
807 :
808 :
809 0 : int p2p_group_notif_noa(struct p2p_group *group, const u8 *noa,
810 : size_t noa_len)
811 : {
812 0 : if (noa == NULL) {
813 0 : wpabuf_free(group->noa);
814 0 : group->noa = NULL;
815 : } else {
816 0 : if (group->noa) {
817 0 : if (wpabuf_size(group->noa) >= noa_len) {
818 0 : group->noa->used = 0;
819 0 : wpabuf_put_data(group->noa, noa, noa_len);
820 : } else {
821 0 : wpabuf_free(group->noa);
822 0 : group->noa = NULL;
823 : }
824 : }
825 :
826 0 : if (!group->noa) {
827 0 : group->noa = wpabuf_alloc_copy(noa, noa_len);
828 0 : if (group->noa == NULL)
829 0 : return -1;
830 : }
831 : }
832 :
833 0 : group->beacon_update = 1;
834 0 : p2p_group_update_ies(group);
835 0 : return 0;
836 : }
837 :
838 :
839 10 : static struct p2p_group_member * p2p_group_get_client(struct p2p_group *group,
840 : const u8 *dev_id)
841 : {
842 : struct p2p_group_member *m;
843 :
844 11 : for (m = group->members; m; m = m->next) {
845 2 : if (os_memcmp(dev_id, m->dev_addr, ETH_ALEN) == 0)
846 1 : return m;
847 : }
848 :
849 9 : return NULL;
850 : }
851 :
852 :
853 2 : const u8 * p2p_group_get_client_interface_addr(struct p2p_group *group,
854 : const u8 *dev_addr)
855 : {
856 : struct p2p_group_member *m;
857 :
858 2 : if (!group)
859 0 : return NULL;
860 2 : m = p2p_group_get_client(group, dev_addr);
861 2 : if (m)
862 1 : return m->addr;
863 1 : return NULL;
864 : }
865 :
866 :
867 373 : static struct p2p_group_member * p2p_group_get_client_iface(
868 : struct p2p_group *group, const u8 *interface_addr)
869 : {
870 : struct p2p_group_member *m;
871 :
872 379 : for (m = group->members; m; m = m->next) {
873 379 : if (os_memcmp(interface_addr, m->addr, ETH_ALEN) == 0)
874 373 : return m;
875 : }
876 :
877 0 : return NULL;
878 : }
879 :
880 :
881 368 : const u8 * p2p_group_get_dev_addr(struct p2p_group *group, const u8 *addr)
882 : {
883 : struct p2p_group_member *m;
884 :
885 368 : if (group == NULL)
886 0 : return NULL;
887 368 : m = p2p_group_get_client_iface(group, addr);
888 368 : if (m && !is_zero_ether_addr(m->dev_addr))
889 353 : return m->dev_addr;
890 15 : return NULL;
891 : }
892 :
893 :
894 0 : static struct wpabuf * p2p_build_go_disc_req(void)
895 : {
896 : struct wpabuf *buf;
897 :
898 0 : buf = wpabuf_alloc(100);
899 0 : if (buf == NULL)
900 0 : return NULL;
901 :
902 0 : p2p_buf_add_action_hdr(buf, P2P_GO_DISC_REQ, 0);
903 :
904 0 : return buf;
905 : }
906 :
907 :
908 8 : int p2p_group_go_discover(struct p2p_group *group, const u8 *dev_id,
909 : const u8 *searching_dev, int rx_freq)
910 : {
911 : struct p2p_group_member *m;
912 : struct wpabuf *req;
913 8 : struct p2p_data *p2p = group->p2p;
914 : int freq;
915 :
916 8 : m = p2p_group_get_client(group, dev_id);
917 8 : if (m == NULL || m->client_info == NULL) {
918 48 : p2p_dbg(group->p2p, "Requested client was not in this group "
919 48 : MACSTR, MAC2STR(group->cfg->interface_addr));
920 8 : return -1;
921 : }
922 :
923 0 : if (!(m->dev_capab & P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY)) {
924 0 : p2p_dbg(group->p2p, "Requested client does not support client discoverability");
925 0 : return -1;
926 : }
927 :
928 0 : p2p_dbg(group->p2p, "Schedule GO Discoverability Request to be sent to "
929 0 : MACSTR, MAC2STR(dev_id));
930 :
931 0 : req = p2p_build_go_disc_req();
932 0 : if (req == NULL)
933 0 : return -1;
934 :
935 : /* TODO: Should really use group operating frequency here */
936 0 : freq = rx_freq;
937 :
938 0 : p2p->pending_action_state = P2P_PENDING_GO_DISC_REQ;
939 0 : if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, m->addr,
940 0 : group->cfg->interface_addr,
941 0 : group->cfg->interface_addr,
942 0 : wpabuf_head(req), wpabuf_len(req), 200) < 0)
943 : {
944 0 : p2p_dbg(p2p, "Failed to send Action frame");
945 : }
946 :
947 0 : wpabuf_free(req);
948 :
949 0 : return 0;
950 : }
951 :
952 :
953 7 : const u8 * p2p_group_get_interface_addr(struct p2p_group *group)
954 : {
955 7 : return group->cfg->interface_addr;
956 : }
957 :
958 :
959 5 : u8 p2p_group_presence_req(struct p2p_group *group,
960 : const u8 *client_interface_addr,
961 : const u8 *noa, size_t noa_len)
962 : {
963 : struct p2p_group_member *m;
964 : u8 curr_noa[50];
965 : int curr_noa_len;
966 :
967 5 : m = p2p_group_get_client_iface(group, client_interface_addr);
968 5 : if (m == NULL || m->client_info == NULL) {
969 0 : p2p_dbg(group->p2p, "Client was not in this group");
970 0 : return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
971 : }
972 :
973 5 : wpa_hexdump(MSG_DEBUG, "P2P: Presence Request NoA", noa, noa_len);
974 :
975 5 : if (group->p2p->cfg->get_noa)
976 10 : curr_noa_len = group->p2p->cfg->get_noa(
977 5 : group->p2p->cfg->cb_ctx, group->cfg->interface_addr,
978 : curr_noa, sizeof(curr_noa));
979 : else
980 0 : curr_noa_len = -1;
981 5 : if (curr_noa_len < 0)
982 5 : p2p_dbg(group->p2p, "Failed to fetch current NoA");
983 0 : else if (curr_noa_len == 0)
984 0 : p2p_dbg(group->p2p, "No NoA being advertized");
985 : else
986 0 : wpa_hexdump(MSG_DEBUG, "P2P: Current NoA", curr_noa,
987 : curr_noa_len);
988 :
989 : /* TODO: properly process request and store copy */
990 5 : if (curr_noa_len > 0 || curr_noa_len == -1)
991 5 : return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
992 :
993 0 : return P2P_SC_SUCCESS;
994 : }
995 :
996 :
997 24 : unsigned int p2p_get_group_num_members(struct p2p_group *group)
998 : {
999 24 : if (!group)
1000 0 : return 0;
1001 :
1002 24 : return group->num_members;
1003 : }
1004 :
1005 :
1006 5 : int p2p_client_limit_reached(struct p2p_group *group)
1007 : {
1008 5 : if (!group || !group->cfg)
1009 0 : return 1;
1010 :
1011 5 : return group->num_members >= group->cfg->max_clients;
1012 : }
1013 :
1014 :
1015 7 : const u8 * p2p_iterate_group_members(struct p2p_group *group, void **next)
1016 : {
1017 7 : struct p2p_group_member *iter = *next;
1018 :
1019 7 : if (!iter)
1020 5 : iter = group->members;
1021 : else
1022 2 : iter = iter->next;
1023 :
1024 7 : *next = iter;
1025 :
1026 7 : if (!iter)
1027 5 : return NULL;
1028 :
1029 2 : return iter->dev_addr;
1030 : }
1031 :
1032 :
1033 755 : int p2p_group_is_client_connected(struct p2p_group *group, const u8 *dev_addr)
1034 : {
1035 : struct p2p_group_member *m;
1036 :
1037 808 : for (m = group->members; m; m = m->next) {
1038 653 : if (os_memcmp(m->dev_addr, dev_addr, ETH_ALEN) == 0)
1039 600 : return 1;
1040 : }
1041 :
1042 155 : return 0;
1043 : }
1044 :
1045 :
1046 79 : int p2p_group_is_group_id_match(struct p2p_group *group, const u8 *group_id,
1047 : size_t group_id_len)
1048 : {
1049 79 : if (group_id_len != ETH_ALEN + group->cfg->ssid_len)
1050 0 : return 0;
1051 79 : if (os_memcmp(group_id, group->p2p->cfg->dev_addr, ETH_ALEN) != 0)
1052 0 : return 0;
1053 158 : return os_memcmp(group_id + ETH_ALEN, group->cfg->ssid,
1054 79 : group->cfg->ssid_len) == 0;
1055 : }
1056 :
1057 :
1058 4 : void p2p_group_force_beacon_update_ies(struct p2p_group *group)
1059 : {
1060 4 : group->beacon_update = 1;
1061 4 : p2p_group_update_ies(group);
1062 4 : }
1063 :
1064 :
1065 5 : int p2p_group_get_freq(struct p2p_group *group)
1066 : {
1067 5 : return group->cfg->freq;
1068 : }
1069 :
1070 :
1071 600 : const struct p2p_group_config * p2p_group_get_config(struct p2p_group *group)
1072 : {
1073 600 : return group->cfg;
1074 : }
1075 :
1076 :
1077 1525 : void p2p_loop_on_all_groups(struct p2p_data *p2p,
1078 : int (*group_callback)(struct p2p_group *group,
1079 : void *user_data),
1080 : void *user_data)
1081 : {
1082 : unsigned int i;
1083 :
1084 2099 : for (i = 0; i < p2p->num_groups; i++) {
1085 755 : if (!group_callback(p2p->groups[i], user_data))
1086 181 : break;
1087 : }
1088 1525 : }
1089 :
1090 :
1091 8118 : int p2p_group_get_common_freqs(struct p2p_group *group, int *common_freqs,
1092 : unsigned int *num)
1093 :
1094 : {
1095 : struct p2p_channels intersect, res;
1096 : struct p2p_group_member *m;
1097 :
1098 8118 : if (!group || !common_freqs || !num)
1099 0 : return -1;
1100 :
1101 8118 : os_memset(&intersect, 0, sizeof(intersect));
1102 8118 : os_memset(&res, 0, sizeof(res));
1103 :
1104 8118 : p2p_channels_union(&intersect, &group->p2p->cfg->channels,
1105 : &intersect);
1106 :
1107 8118 : p2p_channels_dump(group->p2p,
1108 : "Group common freqs before iterating members",
1109 : &intersect);
1110 :
1111 8285 : for (m = group->members; m; m = m->next) {
1112 : struct p2p_device *dev;
1113 :
1114 167 : dev = p2p_get_device(group->p2p, m->dev_addr);
1115 167 : if (!dev || dev->channels.reg_classes == 0)
1116 39 : continue;
1117 :
1118 128 : p2p_channels_intersect(&intersect, &dev->channels, &res);
1119 128 : intersect = res;
1120 : }
1121 :
1122 8118 : p2p_channels_dump(group->p2p, "Group common channels", &intersect);
1123 :
1124 8118 : os_memset(common_freqs, 0, *num * sizeof(int));
1125 8118 : *num = p2p_channels_to_freqs(&intersect, common_freqs, *num);
1126 :
1127 8118 : return 0;
1128 : }
|