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