]>
Commit | Line | Data |
---|---|---|
0f893285 JM |
1 | /* |
2 | * wpa_supplicant - P2P service discovery | |
3 | * Copyright (c) 2009-2010, Atheros Communications | |
4 | * Copyright (c) 2010-2014, Jouni Malinen <j@w1.fi> | |
5 | * | |
6 | * This software may be distributed under the terms of the BSD license. | |
7 | * See README for more details. | |
8 | */ | |
9 | ||
10 | #include "utils/includes.h" | |
11 | ||
12 | #include "utils/common.h" | |
13 | #include "p2p/p2p.h" | |
14 | #include "wpa_supplicant_i.h" | |
15 | #include "notify.h" | |
16 | #include "p2p_supplicant.h" | |
17 | ||
18 | ||
19 | /* | |
20 | * DNS Header section is used only to calculate compression pointers, so the | |
21 | * contents of this data does not matter, but the length needs to be reserved | |
22 | * in the virtual packet. | |
23 | */ | |
24 | #define DNS_HEADER_LEN 12 | |
25 | ||
26 | /* | |
27 | * 27-octet in-memory packet from P2P specification containing two implied | |
28 | * queries for _tcp.lcoal. PTR IN and _udp.local. PTR IN | |
29 | */ | |
30 | #define P2P_SD_IN_MEMORY_LEN 27 | |
31 | ||
32 | static int p2p_sd_dns_uncompress_label(char **upos, char *uend, u8 *start, | |
33 | u8 **spos, const u8 *end) | |
34 | { | |
35 | while (*spos < end) { | |
36 | u8 val = ((*spos)[0] & 0xc0) >> 6; | |
37 | int len; | |
38 | ||
39 | if (val == 1 || val == 2) { | |
40 | /* These are reserved values in RFC 1035 */ | |
41 | wpa_printf(MSG_DEBUG, "P2P: Invalid domain name " | |
42 | "sequence starting with 0x%x", val); | |
43 | return -1; | |
44 | } | |
45 | ||
46 | if (val == 3) { | |
47 | u16 offset; | |
48 | u8 *spos_tmp; | |
49 | ||
50 | /* Offset */ | |
f758ae76 | 51 | if (end - *spos < 2) { |
0f893285 JM |
52 | wpa_printf(MSG_DEBUG, "P2P: No room for full " |
53 | "DNS offset field"); | |
54 | return -1; | |
55 | } | |
56 | ||
57 | offset = (((*spos)[0] & 0x3f) << 8) | (*spos)[1]; | |
58 | if (offset >= *spos - start) { | |
59 | wpa_printf(MSG_DEBUG, "P2P: Invalid DNS " | |
60 | "pointer offset %u", offset); | |
61 | return -1; | |
62 | } | |
63 | ||
64 | (*spos) += 2; | |
65 | spos_tmp = start + offset; | |
66 | return p2p_sd_dns_uncompress_label(upos, uend, start, | |
67 | &spos_tmp, | |
68 | *spos - 2); | |
69 | } | |
70 | ||
71 | /* Label */ | |
72 | len = (*spos)[0] & 0x3f; | |
73 | if (len == 0) | |
74 | return 0; | |
75 | ||
76 | (*spos)++; | |
f758ae76 | 77 | if (len > end - *spos) { |
0f893285 JM |
78 | wpa_printf(MSG_DEBUG, "P2P: Invalid domain name " |
79 | "sequence - no room for label with length " | |
80 | "%u", len); | |
81 | return -1; | |
82 | } | |
83 | ||
f758ae76 | 84 | if (len + 2 > uend - *upos) |
0f893285 JM |
85 | return -2; |
86 | ||
87 | os_memcpy(*upos, *spos, len); | |
88 | *spos += len; | |
89 | *upos += len; | |
90 | (*upos)[0] = '.'; | |
91 | (*upos)++; | |
92 | (*upos)[0] = '\0'; | |
93 | } | |
94 | ||
95 | return 0; | |
96 | } | |
97 | ||
98 | ||
99 | /* Uncompress domain names per RFC 1035 using the P2P SD in-memory packet. | |
100 | * Returns -1 on parsing error (invalid input sequence), -2 if output buffer is | |
101 | * not large enough */ | |
102 | static int p2p_sd_dns_uncompress(char *buf, size_t buf_len, const u8 *msg, | |
103 | size_t msg_len, size_t offset) | |
104 | { | |
105 | /* 27-octet in-memory packet from P2P specification */ | |
106 | const char *prefix = "\x04_tcp\x05local\x00\x00\x0C\x00\x01" | |
107 | "\x04_udp\xC0\x11\x00\x0C\x00\x01"; | |
108 | u8 *tmp, *end, *spos; | |
109 | char *upos, *uend; | |
110 | int ret = 0; | |
111 | ||
112 | if (buf_len < 2) | |
113 | return -1; | |
114 | if (offset > msg_len) | |
115 | return -1; | |
116 | ||
117 | tmp = os_malloc(DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN + msg_len); | |
118 | if (tmp == NULL) | |
119 | return -1; | |
120 | spos = tmp + DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN; | |
121 | end = spos + msg_len; | |
122 | spos += offset; | |
123 | ||
124 | os_memset(tmp, 0, DNS_HEADER_LEN); | |
125 | os_memcpy(tmp + DNS_HEADER_LEN, prefix, P2P_SD_IN_MEMORY_LEN); | |
126 | os_memcpy(tmp + DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN, msg, msg_len); | |
127 | ||
128 | upos = buf; | |
129 | uend = buf + buf_len; | |
130 | ||
131 | ret = p2p_sd_dns_uncompress_label(&upos, uend, tmp, &spos, end); | |
132 | if (ret) { | |
133 | os_free(tmp); | |
134 | return ret; | |
135 | } | |
136 | ||
137 | if (upos == buf) { | |
138 | upos[0] = '.'; | |
139 | upos[1] = '\0'; | |
140 | } else if (upos[-1] == '.') | |
141 | upos[-1] = '\0'; | |
142 | ||
143 | os_free(tmp); | |
144 | return 0; | |
145 | } | |
146 | ||
147 | ||
148 | static struct p2p_srv_bonjour * | |
149 | wpas_p2p_service_get_bonjour(struct wpa_supplicant *wpa_s, | |
150 | const struct wpabuf *query) | |
151 | { | |
152 | struct p2p_srv_bonjour *bsrv; | |
153 | size_t len; | |
154 | ||
155 | len = wpabuf_len(query); | |
156 | dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, | |
157 | struct p2p_srv_bonjour, list) { | |
158 | if (len == wpabuf_len(bsrv->query) && | |
159 | os_memcmp(wpabuf_head(query), wpabuf_head(bsrv->query), | |
160 | len) == 0) | |
161 | return bsrv; | |
162 | } | |
163 | return NULL; | |
164 | } | |
165 | ||
166 | ||
167 | static struct p2p_srv_upnp * | |
168 | wpas_p2p_service_get_upnp(struct wpa_supplicant *wpa_s, u8 version, | |
169 | const char *service) | |
170 | { | |
171 | struct p2p_srv_upnp *usrv; | |
172 | ||
173 | dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, | |
174 | struct p2p_srv_upnp, list) { | |
175 | if (version == usrv->version && | |
176 | os_strcmp(service, usrv->service) == 0) | |
177 | return usrv; | |
178 | } | |
179 | return NULL; | |
180 | } | |
181 | ||
182 | ||
183 | static void wpas_sd_add_empty(struct wpabuf *resp, u8 srv_proto, | |
184 | u8 srv_trans_id, u8 status) | |
185 | { | |
186 | u8 *len_pos; | |
187 | ||
188 | if (wpabuf_tailroom(resp) < 5) | |
189 | return; | |
190 | ||
191 | /* Length (to be filled) */ | |
192 | len_pos = wpabuf_put(resp, 2); | |
193 | wpabuf_put_u8(resp, srv_proto); | |
194 | wpabuf_put_u8(resp, srv_trans_id); | |
195 | /* Status Code */ | |
196 | wpabuf_put_u8(resp, status); | |
197 | /* Response Data: empty */ | |
198 | WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); | |
199 | } | |
200 | ||
201 | ||
202 | static void wpas_sd_add_proto_not_avail(struct wpabuf *resp, u8 srv_proto, | |
203 | u8 srv_trans_id) | |
204 | { | |
205 | wpas_sd_add_empty(resp, srv_proto, srv_trans_id, | |
206 | P2P_SD_PROTO_NOT_AVAILABLE); | |
207 | } | |
208 | ||
209 | ||
210 | static void wpas_sd_add_bad_request(struct wpabuf *resp, u8 srv_proto, | |
211 | u8 srv_trans_id) | |
212 | { | |
213 | wpas_sd_add_empty(resp, srv_proto, srv_trans_id, P2P_SD_BAD_REQUEST); | |
214 | } | |
215 | ||
216 | ||
217 | static void wpas_sd_add_not_found(struct wpabuf *resp, u8 srv_proto, | |
218 | u8 srv_trans_id) | |
219 | { | |
220 | wpas_sd_add_empty(resp, srv_proto, srv_trans_id, | |
221 | P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); | |
222 | } | |
223 | ||
224 | ||
225 | static void wpas_sd_all_bonjour(struct wpa_supplicant *wpa_s, | |
226 | struct wpabuf *resp, u8 srv_trans_id) | |
227 | { | |
228 | struct p2p_srv_bonjour *bsrv; | |
229 | u8 *len_pos; | |
230 | ||
231 | wpa_printf(MSG_DEBUG, "P2P: SD Request for all Bonjour services"); | |
232 | ||
233 | if (dl_list_empty(&wpa_s->global->p2p_srv_bonjour)) { | |
234 | wpa_printf(MSG_DEBUG, "P2P: Bonjour protocol not available"); | |
235 | return; | |
236 | } | |
237 | ||
238 | dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, | |
239 | struct p2p_srv_bonjour, list) { | |
240 | if (wpabuf_tailroom(resp) < | |
241 | 5 + wpabuf_len(bsrv->query) + wpabuf_len(bsrv->resp)) | |
242 | return; | |
243 | /* Length (to be filled) */ | |
244 | len_pos = wpabuf_put(resp, 2); | |
245 | wpabuf_put_u8(resp, P2P_SERV_BONJOUR); | |
246 | wpabuf_put_u8(resp, srv_trans_id); | |
247 | /* Status Code */ | |
248 | wpabuf_put_u8(resp, P2P_SD_SUCCESS); | |
249 | wpa_hexdump_ascii(MSG_DEBUG, "P2P: Matching Bonjour service", | |
250 | wpabuf_head(bsrv->resp), | |
251 | wpabuf_len(bsrv->resp)); | |
252 | /* Response Data */ | |
253 | wpabuf_put_buf(resp, bsrv->query); /* Key */ | |
254 | wpabuf_put_buf(resp, bsrv->resp); /* Value */ | |
255 | WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - | |
256 | 2); | |
257 | } | |
258 | } | |
259 | ||
260 | ||
261 | static int match_bonjour_query(struct p2p_srv_bonjour *bsrv, const u8 *query, | |
262 | size_t query_len) | |
263 | { | |
264 | char str_rx[256], str_srv[256]; | |
265 | ||
266 | if (query_len < 3 || wpabuf_len(bsrv->query) < 3) | |
267 | return 0; /* Too short to include DNS Type and Version */ | |
268 | if (os_memcmp(query + query_len - 3, | |
269 | wpabuf_head_u8(bsrv->query) + wpabuf_len(bsrv->query) - 3, | |
270 | 3) != 0) | |
271 | return 0; /* Mismatch in DNS Type or Version */ | |
272 | if (query_len == wpabuf_len(bsrv->query) && | |
273 | os_memcmp(query, wpabuf_head(bsrv->query), query_len - 3) == 0) | |
274 | return 1; /* Binary match */ | |
275 | ||
276 | if (p2p_sd_dns_uncompress(str_rx, sizeof(str_rx), query, query_len - 3, | |
277 | 0)) | |
278 | return 0; /* Failed to uncompress query */ | |
279 | if (p2p_sd_dns_uncompress(str_srv, sizeof(str_srv), | |
280 | wpabuf_head(bsrv->query), | |
281 | wpabuf_len(bsrv->query) - 3, 0)) | |
282 | return 0; /* Failed to uncompress service */ | |
283 | ||
284 | return os_strcmp(str_rx, str_srv) == 0; | |
285 | } | |
286 | ||
287 | ||
288 | static void wpas_sd_req_bonjour(struct wpa_supplicant *wpa_s, | |
289 | struct wpabuf *resp, u8 srv_trans_id, | |
290 | const u8 *query, size_t query_len) | |
291 | { | |
292 | struct p2p_srv_bonjour *bsrv; | |
293 | u8 *len_pos; | |
294 | int matches = 0; | |
295 | ||
296 | wpa_hexdump_ascii(MSG_DEBUG, "P2P: SD Request for Bonjour", | |
297 | query, query_len); | |
298 | if (dl_list_empty(&wpa_s->global->p2p_srv_bonjour)) { | |
299 | wpa_printf(MSG_DEBUG, "P2P: Bonjour protocol not available"); | |
300 | wpas_sd_add_proto_not_avail(resp, P2P_SERV_BONJOUR, | |
301 | srv_trans_id); | |
302 | return; | |
303 | } | |
304 | ||
305 | if (query_len == 0) { | |
306 | wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); | |
307 | return; | |
308 | } | |
309 | ||
310 | dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, | |
311 | struct p2p_srv_bonjour, list) { | |
312 | if (!match_bonjour_query(bsrv, query, query_len)) | |
313 | continue; | |
314 | ||
315 | if (wpabuf_tailroom(resp) < | |
316 | 5 + query_len + wpabuf_len(bsrv->resp)) | |
317 | return; | |
318 | ||
319 | matches++; | |
320 | ||
321 | /* Length (to be filled) */ | |
322 | len_pos = wpabuf_put(resp, 2); | |
323 | wpabuf_put_u8(resp, P2P_SERV_BONJOUR); | |
324 | wpabuf_put_u8(resp, srv_trans_id); | |
325 | ||
326 | /* Status Code */ | |
327 | wpabuf_put_u8(resp, P2P_SD_SUCCESS); | |
328 | wpa_hexdump_ascii(MSG_DEBUG, "P2P: Matching Bonjour service", | |
329 | wpabuf_head(bsrv->resp), | |
330 | wpabuf_len(bsrv->resp)); | |
331 | ||
332 | /* Response Data */ | |
333 | wpabuf_put_data(resp, query, query_len); /* Key */ | |
334 | wpabuf_put_buf(resp, bsrv->resp); /* Value */ | |
335 | ||
336 | WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); | |
337 | } | |
338 | ||
339 | if (matches == 0) { | |
340 | wpa_printf(MSG_DEBUG, "P2P: Requested Bonjour service not " | |
341 | "available"); | |
342 | if (wpabuf_tailroom(resp) < 5) | |
343 | return; | |
344 | ||
345 | /* Length (to be filled) */ | |
346 | len_pos = wpabuf_put(resp, 2); | |
347 | wpabuf_put_u8(resp, P2P_SERV_BONJOUR); | |
348 | wpabuf_put_u8(resp, srv_trans_id); | |
349 | ||
350 | /* Status Code */ | |
351 | wpabuf_put_u8(resp, P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); | |
352 | /* Response Data: empty */ | |
353 | WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - | |
354 | 2); | |
355 | } | |
356 | } | |
357 | ||
358 | ||
359 | static void wpas_sd_all_upnp(struct wpa_supplicant *wpa_s, | |
360 | struct wpabuf *resp, u8 srv_trans_id) | |
361 | { | |
362 | struct p2p_srv_upnp *usrv; | |
363 | u8 *len_pos; | |
364 | ||
365 | wpa_printf(MSG_DEBUG, "P2P: SD Request for all UPnP services"); | |
366 | ||
367 | if (dl_list_empty(&wpa_s->global->p2p_srv_upnp)) { | |
368 | wpa_printf(MSG_DEBUG, "P2P: UPnP protocol not available"); | |
369 | return; | |
370 | } | |
371 | ||
372 | dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, | |
373 | struct p2p_srv_upnp, list) { | |
374 | if (wpabuf_tailroom(resp) < 5 + 1 + os_strlen(usrv->service)) | |
375 | return; | |
376 | ||
377 | /* Length (to be filled) */ | |
378 | len_pos = wpabuf_put(resp, 2); | |
379 | wpabuf_put_u8(resp, P2P_SERV_UPNP); | |
380 | wpabuf_put_u8(resp, srv_trans_id); | |
381 | ||
382 | /* Status Code */ | |
383 | wpabuf_put_u8(resp, P2P_SD_SUCCESS); | |
384 | /* Response Data */ | |
385 | wpabuf_put_u8(resp, usrv->version); | |
386 | wpa_printf(MSG_DEBUG, "P2P: Matching UPnP Service: %s", | |
387 | usrv->service); | |
388 | wpabuf_put_str(resp, usrv->service); | |
389 | WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - | |
390 | 2); | |
391 | } | |
392 | } | |
393 | ||
394 | ||
395 | static void wpas_sd_req_upnp(struct wpa_supplicant *wpa_s, | |
396 | struct wpabuf *resp, u8 srv_trans_id, | |
397 | const u8 *query, size_t query_len) | |
398 | { | |
399 | struct p2p_srv_upnp *usrv; | |
400 | u8 *len_pos; | |
401 | u8 version; | |
402 | char *str; | |
403 | int count = 0; | |
404 | ||
405 | wpa_hexdump_ascii(MSG_DEBUG, "P2P: SD Request for UPnP", | |
406 | query, query_len); | |
407 | ||
408 | if (dl_list_empty(&wpa_s->global->p2p_srv_upnp)) { | |
409 | wpa_printf(MSG_DEBUG, "P2P: UPnP protocol not available"); | |
410 | wpas_sd_add_proto_not_avail(resp, P2P_SERV_UPNP, | |
411 | srv_trans_id); | |
412 | return; | |
413 | } | |
414 | ||
415 | if (query_len == 0) { | |
416 | wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); | |
417 | return; | |
418 | } | |
419 | ||
420 | if (wpabuf_tailroom(resp) < 5) | |
421 | return; | |
422 | ||
423 | /* Length (to be filled) */ | |
424 | len_pos = wpabuf_put(resp, 2); | |
425 | wpabuf_put_u8(resp, P2P_SERV_UPNP); | |
426 | wpabuf_put_u8(resp, srv_trans_id); | |
427 | ||
428 | version = query[0]; | |
429 | str = os_malloc(query_len); | |
430 | if (str == NULL) | |
431 | return; | |
432 | os_memcpy(str, query + 1, query_len - 1); | |
433 | str[query_len - 1] = '\0'; | |
434 | ||
435 | dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, | |
436 | struct p2p_srv_upnp, list) { | |
437 | if (version != usrv->version) | |
438 | continue; | |
439 | ||
440 | if (os_strcmp(str, "ssdp:all") != 0 && | |
441 | os_strstr(usrv->service, str) == NULL) | |
442 | continue; | |
443 | ||
444 | if (wpabuf_tailroom(resp) < 2) | |
445 | break; | |
446 | if (count == 0) { | |
447 | /* Status Code */ | |
448 | wpabuf_put_u8(resp, P2P_SD_SUCCESS); | |
449 | /* Response Data */ | |
450 | wpabuf_put_u8(resp, version); | |
451 | } else | |
452 | wpabuf_put_u8(resp, ','); | |
453 | ||
454 | count++; | |
455 | ||
456 | wpa_printf(MSG_DEBUG, "P2P: Matching UPnP Service: %s", | |
457 | usrv->service); | |
458 | if (wpabuf_tailroom(resp) < os_strlen(usrv->service)) | |
459 | break; | |
460 | wpabuf_put_str(resp, usrv->service); | |
461 | } | |
462 | os_free(str); | |
463 | ||
464 | if (count == 0) { | |
465 | wpa_printf(MSG_DEBUG, "P2P: Requested UPnP service not " | |
466 | "available"); | |
467 | /* Status Code */ | |
468 | wpabuf_put_u8(resp, P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); | |
469 | /* Response Data: empty */ | |
470 | } | |
471 | ||
472 | WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); | |
473 | } | |
474 | ||
475 | ||
476 | #ifdef CONFIG_WIFI_DISPLAY | |
477 | static void wpas_sd_req_wfd(struct wpa_supplicant *wpa_s, | |
478 | struct wpabuf *resp, u8 srv_trans_id, | |
479 | const u8 *query, size_t query_len) | |
480 | { | |
481 | const u8 *pos; | |
482 | u8 role; | |
483 | u8 *len_pos; | |
484 | ||
485 | wpa_hexdump(MSG_DEBUG, "P2P: SD Request for WFD", query, query_len); | |
486 | ||
487 | if (!wpa_s->global->wifi_display) { | |
488 | wpa_printf(MSG_DEBUG, "P2P: WFD protocol not available"); | |
489 | wpas_sd_add_proto_not_avail(resp, P2P_SERV_WIFI_DISPLAY, | |
490 | srv_trans_id); | |
491 | return; | |
492 | } | |
493 | ||
494 | if (query_len < 1) { | |
495 | wpa_printf(MSG_DEBUG, "P2P: Missing WFD Requested Device " | |
496 | "Role"); | |
497 | return; | |
498 | } | |
499 | ||
500 | if (wpabuf_tailroom(resp) < 5) | |
501 | return; | |
502 | ||
503 | pos = query; | |
504 | role = *pos++; | |
505 | wpa_printf(MSG_DEBUG, "P2P: WSD for device role 0x%x", role); | |
506 | ||
507 | /* TODO: role specific handling */ | |
508 | ||
509 | /* Length (to be filled) */ | |
510 | len_pos = wpabuf_put(resp, 2); | |
511 | wpabuf_put_u8(resp, P2P_SERV_WIFI_DISPLAY); | |
512 | wpabuf_put_u8(resp, srv_trans_id); | |
513 | wpabuf_put_u8(resp, P2P_SD_SUCCESS); /* Status Code */ | |
514 | ||
515 | while (pos < query + query_len) { | |
516 | if (*pos < MAX_WFD_SUBELEMS && | |
517 | wpa_s->global->wfd_subelem[*pos] && | |
518 | wpabuf_tailroom(resp) >= | |
519 | wpabuf_len(wpa_s->global->wfd_subelem[*pos])) { | |
520 | wpa_printf(MSG_DEBUG, "P2P: Add WSD response " | |
521 | "subelement %u", *pos); | |
522 | wpabuf_put_buf(resp, wpa_s->global->wfd_subelem[*pos]); | |
523 | } | |
524 | pos++; | |
525 | } | |
526 | ||
527 | WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); | |
528 | } | |
529 | #endif /* CONFIG_WIFI_DISPLAY */ | |
530 | ||
531 | ||
532 | static int find_p2ps_substr(struct p2ps_advertisement *adv_data, | |
533 | const u8 *needle, size_t needle_len) | |
534 | { | |
535 | const u8 *haystack = (const u8 *) adv_data->svc_info; | |
536 | size_t haystack_len, i; | |
537 | ||
538 | /* Allow search term to be empty */ | |
539 | if (!needle || !needle_len) | |
540 | return 1; | |
541 | ||
542 | if (!haystack) | |
543 | return 0; | |
544 | ||
545 | haystack_len = os_strlen(adv_data->svc_info); | |
546 | for (i = 0; i < haystack_len; i++) { | |
547 | if (haystack_len - i < needle_len) | |
548 | break; | |
549 | if (os_memcmp(haystack + i, needle, needle_len) == 0) | |
550 | return 1; | |
551 | } | |
552 | ||
553 | return 0; | |
554 | } | |
555 | ||
556 | ||
557 | static void wpas_sd_req_asp(struct wpa_supplicant *wpa_s, | |
558 | struct wpabuf *resp, u8 srv_trans_id, | |
559 | const u8 *query, size_t query_len) | |
560 | { | |
561 | struct p2ps_advertisement *adv_data; | |
562 | const u8 *svc = &query[1]; | |
563 | const u8 *info = NULL; | |
564 | size_t svc_len = query[0]; | |
565 | size_t info_len = 0; | |
566 | int prefix = 0; | |
567 | u8 *count_pos = NULL; | |
568 | u8 *len_pos = NULL; | |
569 | ||
570 | wpa_hexdump(MSG_DEBUG, "P2P: SD Request for ASP", query, query_len); | |
571 | ||
572 | if (!wpa_s->global->p2p) { | |
573 | wpa_printf(MSG_DEBUG, "P2P: ASP protocol not available"); | |
574 | wpas_sd_add_proto_not_avail(resp, P2P_SERV_P2PS, srv_trans_id); | |
575 | return; | |
576 | } | |
577 | ||
578 | /* Info block is optional */ | |
579 | if (svc_len + 1 < query_len) { | |
580 | info = &svc[svc_len]; | |
581 | info_len = *info++; | |
582 | } | |
583 | ||
584 | /* Range check length of svc string and info block */ | |
585 | if (svc_len + (info_len ? info_len + 2 : 1) > query_len) { | |
586 | wpa_printf(MSG_DEBUG, "P2P: ASP bad request"); | |
587 | wpas_sd_add_bad_request(resp, P2P_SERV_P2PS, srv_trans_id); | |
588 | return; | |
589 | } | |
590 | ||
591 | /* Detect and correct for prefix search */ | |
592 | if (svc_len && svc[svc_len - 1] == '*') { | |
593 | prefix = 1; | |
594 | svc_len--; | |
595 | } | |
596 | ||
597 | for (adv_data = p2p_get_p2ps_adv_list(wpa_s->global->p2p); | |
598 | adv_data; adv_data = adv_data->next) { | |
599 | /* If not a prefix match, reject length mismatches */ | |
600 | if (!prefix && svc_len != os_strlen(adv_data->svc_name)) | |
601 | continue; | |
602 | ||
603 | /* Search each service for request */ | |
604 | if (os_memcmp(adv_data->svc_name, svc, svc_len) == 0 && | |
605 | find_p2ps_substr(adv_data, info, info_len)) { | |
606 | size_t len = os_strlen(adv_data->svc_name); | |
607 | size_t svc_info_len = 0; | |
608 | ||
609 | if (adv_data->svc_info) | |
610 | svc_info_len = os_strlen(adv_data->svc_info); | |
611 | ||
612 | if (len > 0xff || svc_info_len > 0xffff) | |
613 | return; | |
614 | ||
615 | /* Length & Count to be filled as we go */ | |
616 | if (!len_pos && !count_pos) { | |
617 | if (wpabuf_tailroom(resp) < | |
618 | len + svc_info_len + 16) | |
619 | return; | |
620 | ||
621 | len_pos = wpabuf_put(resp, 2); | |
622 | wpabuf_put_u8(resp, P2P_SERV_P2PS); | |
623 | wpabuf_put_u8(resp, srv_trans_id); | |
624 | /* Status Code */ | |
625 | wpabuf_put_u8(resp, P2P_SD_SUCCESS); | |
626 | count_pos = wpabuf_put(resp, 1); | |
627 | *count_pos = 0; | |
628 | } else if (wpabuf_tailroom(resp) < | |
629 | len + svc_info_len + 10) | |
630 | return; | |
631 | ||
632 | if (svc_info_len) { | |
633 | wpa_printf(MSG_DEBUG, | |
634 | "P2P: Add Svc: %s info: %s", | |
635 | adv_data->svc_name, | |
636 | adv_data->svc_info); | |
637 | } else { | |
638 | wpa_printf(MSG_DEBUG, "P2P: Add Svc: %s", | |
639 | adv_data->svc_name); | |
640 | } | |
641 | ||
642 | /* Advertisement ID */ | |
643 | wpabuf_put_le32(resp, adv_data->id); | |
644 | ||
645 | /* Config Methods */ | |
646 | wpabuf_put_be16(resp, adv_data->config_methods); | |
647 | ||
648 | /* Service Name */ | |
649 | wpabuf_put_u8(resp, (u8) len); | |
650 | wpabuf_put_data(resp, adv_data->svc_name, len); | |
651 | ||
652 | /* Service State */ | |
653 | wpabuf_put_u8(resp, adv_data->state); | |
654 | ||
655 | /* Service Information */ | |
656 | wpabuf_put_le16(resp, (u16) svc_info_len); | |
657 | wpabuf_put_data(resp, adv_data->svc_info, svc_info_len); | |
658 | ||
659 | /* Update length and count */ | |
660 | (*count_pos)++; | |
661 | WPA_PUT_LE16(len_pos, | |
662 | (u8 *) wpabuf_put(resp, 0) - len_pos - 2); | |
663 | } | |
664 | } | |
665 | ||
666 | /* Return error if no matching svc found */ | |
667 | if (count_pos == NULL) { | |
668 | wpa_printf(MSG_DEBUG, "P2P: ASP service not found"); | |
669 | wpas_sd_add_not_found(resp, P2P_SERV_P2PS, srv_trans_id); | |
670 | } | |
671 | } | |
672 | ||
673 | ||
6dd51ecb MS |
674 | static void wpas_sd_all_asp(struct wpa_supplicant *wpa_s, |
675 | struct wpabuf *resp, u8 srv_trans_id) | |
676 | { | |
677 | /* Query data to add all P2PS advertisements: | |
678 | * - Service name length: 1 | |
679 | * - Service name: '*' | |
680 | * - Service Information Request Length: 0 | |
681 | */ | |
682 | const u8 q[] = { 1, (const u8) '*', 0 }; | |
683 | ||
684 | if (p2p_get_p2ps_adv_list(wpa_s->global->p2p)) | |
685 | wpas_sd_req_asp(wpa_s, resp, srv_trans_id, q, sizeof(q)); | |
686 | } | |
687 | ||
688 | ||
0f893285 JM |
689 | void wpas_sd_request(void *ctx, int freq, const u8 *sa, u8 dialog_token, |
690 | u16 update_indic, const u8 *tlvs, size_t tlvs_len) | |
691 | { | |
692 | struct wpa_supplicant *wpa_s = ctx; | |
693 | const u8 *pos = tlvs; | |
694 | const u8 *end = tlvs + tlvs_len; | |
695 | const u8 *tlv_end; | |
696 | u16 slen; | |
697 | struct wpabuf *resp; | |
698 | u8 srv_proto, srv_trans_id; | |
699 | size_t buf_len; | |
700 | char *buf; | |
701 | ||
702 | wpa_hexdump(MSG_MSGDUMP, "P2P: Service Discovery Request TLVs", | |
703 | tlvs, tlvs_len); | |
704 | buf_len = 2 * tlvs_len + 1; | |
705 | buf = os_malloc(buf_len); | |
706 | if (buf) { | |
707 | wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len); | |
708 | wpa_msg_ctrl(wpa_s, MSG_INFO, P2P_EVENT_SERV_DISC_REQ "%d " | |
709 | MACSTR " %u %u %s", | |
710 | freq, MAC2STR(sa), dialog_token, update_indic, | |
711 | buf); | |
712 | os_free(buf); | |
713 | } | |
714 | ||
715 | if (wpa_s->p2p_sd_over_ctrl_iface) { | |
716 | wpas_notify_p2p_sd_request(wpa_s, freq, sa, dialog_token, | |
717 | update_indic, tlvs, tlvs_len); | |
718 | return; /* to be processed by an external program */ | |
719 | } | |
720 | ||
721 | resp = wpabuf_alloc(10000); | |
722 | if (resp == NULL) | |
723 | return; | |
724 | ||
f758ae76 | 725 | while (end - pos > 1) { |
0f893285 JM |
726 | wpa_printf(MSG_DEBUG, "P2P: Service Request TLV"); |
727 | slen = WPA_GET_LE16(pos); | |
728 | pos += 2; | |
f758ae76 | 729 | if (slen > end - pos || slen < 2) { |
0f893285 JM |
730 | wpa_printf(MSG_DEBUG, "P2P: Unexpected Query Data " |
731 | "length"); | |
732 | wpabuf_free(resp); | |
733 | return; | |
734 | } | |
735 | tlv_end = pos + slen; | |
736 | ||
737 | srv_proto = *pos++; | |
738 | wpa_printf(MSG_DEBUG, "P2P: Service Protocol Type %u", | |
739 | srv_proto); | |
740 | srv_trans_id = *pos++; | |
741 | wpa_printf(MSG_DEBUG, "P2P: Service Transaction ID %u", | |
742 | srv_trans_id); | |
743 | ||
744 | wpa_hexdump(MSG_MSGDUMP, "P2P: Query Data", | |
745 | pos, tlv_end - pos); | |
746 | ||
747 | ||
748 | if (wpa_s->force_long_sd) { | |
749 | wpa_printf(MSG_DEBUG, "P2P: SD test - force long " | |
750 | "response"); | |
751 | wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); | |
752 | wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); | |
6dd51ecb | 753 | wpas_sd_all_asp(wpa_s, resp, srv_trans_id); |
0f893285 JM |
754 | goto done; |
755 | } | |
756 | ||
757 | switch (srv_proto) { | |
758 | case P2P_SERV_ALL_SERVICES: | |
759 | wpa_printf(MSG_DEBUG, "P2P: Service Discovery Request " | |
760 | "for all services"); | |
761 | if (dl_list_empty(&wpa_s->global->p2p_srv_upnp) && | |
6dd51ecb MS |
762 | dl_list_empty(&wpa_s->global->p2p_srv_bonjour) && |
763 | !p2p_get_p2ps_adv_list(wpa_s->global->p2p)) { | |
0f893285 JM |
764 | wpa_printf(MSG_DEBUG, "P2P: No service " |
765 | "discovery protocols available"); | |
766 | wpas_sd_add_proto_not_avail( | |
767 | resp, P2P_SERV_ALL_SERVICES, | |
768 | srv_trans_id); | |
769 | break; | |
770 | } | |
771 | wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); | |
772 | wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); | |
6dd51ecb | 773 | wpas_sd_all_asp(wpa_s, resp, srv_trans_id); |
0f893285 JM |
774 | break; |
775 | case P2P_SERV_BONJOUR: | |
776 | wpas_sd_req_bonjour(wpa_s, resp, srv_trans_id, | |
777 | pos, tlv_end - pos); | |
778 | break; | |
779 | case P2P_SERV_UPNP: | |
780 | wpas_sd_req_upnp(wpa_s, resp, srv_trans_id, | |
781 | pos, tlv_end - pos); | |
782 | break; | |
783 | #ifdef CONFIG_WIFI_DISPLAY | |
784 | case P2P_SERV_WIFI_DISPLAY: | |
785 | wpas_sd_req_wfd(wpa_s, resp, srv_trans_id, | |
786 | pos, tlv_end - pos); | |
787 | break; | |
788 | #endif /* CONFIG_WIFI_DISPLAY */ | |
789 | case P2P_SERV_P2PS: | |
790 | wpas_sd_req_asp(wpa_s, resp, srv_trans_id, | |
791 | pos, tlv_end - pos); | |
792 | break; | |
793 | default: | |
794 | wpa_printf(MSG_DEBUG, "P2P: Unavailable service " | |
795 | "protocol %u", srv_proto); | |
796 | wpas_sd_add_proto_not_avail(resp, srv_proto, | |
797 | srv_trans_id); | |
798 | break; | |
799 | } | |
800 | ||
801 | pos = tlv_end; | |
802 | } | |
803 | ||
804 | done: | |
805 | wpas_notify_p2p_sd_request(wpa_s, freq, sa, dialog_token, | |
806 | update_indic, tlvs, tlvs_len); | |
807 | ||
808 | wpas_p2p_sd_response(wpa_s, freq, sa, dialog_token, resp); | |
809 | ||
810 | wpabuf_free(resp); | |
811 | } | |
812 | ||
813 | ||
814 | static void wpas_sd_p2ps_serv_response(struct wpa_supplicant *wpa_s, | |
815 | const u8 *sa, u8 srv_trans_id, | |
816 | const u8 *pos, const u8 *tlv_end) | |
817 | { | |
818 | u8 left = *pos++; | |
819 | u32 adv_id; | |
820 | u8 svc_status; | |
821 | u16 config_methods; | |
822 | char svc_str[256]; | |
823 | ||
824 | while (left-- && pos < tlv_end) { | |
825 | char *buf = NULL; | |
826 | size_t buf_len; | |
827 | u8 svc_len; | |
828 | ||
829 | /* Sanity check fixed length+svc_str */ | |
f758ae76 | 830 | if (6 >= tlv_end - pos) |
0f893285 JM |
831 | break; |
832 | svc_len = pos[6]; | |
f758ae76 | 833 | if (svc_len + 10 > tlv_end - pos) |
0f893285 JM |
834 | break; |
835 | ||
836 | /* Advertisement ID */ | |
837 | adv_id = WPA_GET_LE32(pos); | |
838 | pos += sizeof(u32); | |
839 | ||
840 | /* Config Methods */ | |
841 | config_methods = WPA_GET_BE16(pos); | |
842 | pos += sizeof(u16); | |
843 | ||
844 | /* Service Name */ | |
845 | pos++; /* svc_len */ | |
846 | os_memcpy(svc_str, pos, svc_len); | |
847 | svc_str[svc_len] = '\0'; | |
848 | pos += svc_len; | |
849 | ||
850 | /* Service Status */ | |
851 | svc_status = *pos++; | |
852 | ||
853 | /* Service Information Length */ | |
854 | buf_len = WPA_GET_LE16(pos); | |
855 | pos += sizeof(u16); | |
856 | ||
857 | /* Sanity check buffer length */ | |
858 | if (buf_len > (unsigned int) (tlv_end - pos)) | |
859 | break; | |
860 | ||
861 | if (buf_len) { | |
862 | buf = os_zalloc(2 * buf_len + 1); | |
863 | if (buf) { | |
864 | utf8_escape((const char *) pos, buf_len, buf, | |
865 | 2 * buf_len + 1); | |
866 | } | |
867 | } | |
868 | ||
869 | pos += buf_len; | |
870 | ||
871 | if (buf) { | |
872 | wpa_msg_global(wpa_s, MSG_INFO, P2P_EVENT_SERV_ASP_RESP | |
873 | MACSTR " %x %x %x %x %s '%s'", | |
874 | MAC2STR(sa), srv_trans_id, adv_id, | |
875 | svc_status, config_methods, svc_str, | |
876 | buf); | |
877 | os_free(buf); | |
878 | } else { | |
879 | wpa_msg_global(wpa_s, MSG_INFO, P2P_EVENT_SERV_ASP_RESP | |
880 | MACSTR " %x %x %x %x %s", | |
881 | MAC2STR(sa), srv_trans_id, adv_id, | |
882 | svc_status, config_methods, svc_str); | |
883 | } | |
884 | } | |
885 | } | |
886 | ||
887 | ||
888 | void wpas_sd_response(void *ctx, const u8 *sa, u16 update_indic, | |
889 | const u8 *tlvs, size_t tlvs_len) | |
890 | { | |
891 | struct wpa_supplicant *wpa_s = ctx; | |
892 | const u8 *pos = tlvs; | |
893 | const u8 *end = tlvs + tlvs_len; | |
894 | const u8 *tlv_end; | |
895 | u16 slen; | |
896 | size_t buf_len; | |
897 | char *buf; | |
898 | ||
899 | wpa_hexdump(MSG_MSGDUMP, "P2P: Service Discovery Response TLVs", | |
900 | tlvs, tlvs_len); | |
901 | if (tlvs_len > 1500) { | |
902 | /* TODO: better way for handling this */ | |
903 | wpa_msg_ctrl(wpa_s, MSG_INFO, | |
904 | P2P_EVENT_SERV_DISC_RESP MACSTR | |
905 | " %u <long response: %u bytes>", | |
906 | MAC2STR(sa), update_indic, | |
907 | (unsigned int) tlvs_len); | |
908 | } else { | |
909 | buf_len = 2 * tlvs_len + 1; | |
910 | buf = os_malloc(buf_len); | |
911 | if (buf) { | |
912 | wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len); | |
913 | wpa_msg_ctrl(wpa_s, MSG_INFO, | |
914 | P2P_EVENT_SERV_DISC_RESP MACSTR " %u %s", | |
915 | MAC2STR(sa), update_indic, buf); | |
916 | os_free(buf); | |
917 | } | |
918 | } | |
919 | ||
f758ae76 | 920 | while (end - pos >= 2) { |
0f893285 JM |
921 | u8 srv_proto, srv_trans_id, status; |
922 | ||
923 | wpa_printf(MSG_DEBUG, "P2P: Service Response TLV"); | |
924 | slen = WPA_GET_LE16(pos); | |
925 | pos += 2; | |
f758ae76 | 926 | if (slen > end - pos || slen < 3) { |
0f893285 JM |
927 | wpa_printf(MSG_DEBUG, "P2P: Unexpected Response Data " |
928 | "length"); | |
929 | return; | |
930 | } | |
931 | tlv_end = pos + slen; | |
932 | ||
933 | srv_proto = *pos++; | |
934 | wpa_printf(MSG_DEBUG, "P2P: Service Protocol Type %u", | |
935 | srv_proto); | |
936 | srv_trans_id = *pos++; | |
937 | wpa_printf(MSG_DEBUG, "P2P: Service Transaction ID %u", | |
938 | srv_trans_id); | |
939 | status = *pos++; | |
940 | wpa_printf(MSG_DEBUG, "P2P: Status Code ID %u", | |
941 | status); | |
942 | ||
943 | wpa_hexdump(MSG_MSGDUMP, "P2P: Response Data", | |
944 | pos, tlv_end - pos); | |
945 | ||
946 | if (srv_proto == P2P_SERV_P2PS && pos < tlv_end) { | |
947 | wpas_sd_p2ps_serv_response(wpa_s, sa, srv_trans_id, | |
948 | pos, tlv_end); | |
949 | } | |
950 | ||
951 | pos = tlv_end; | |
952 | } | |
953 | ||
954 | wpas_notify_p2p_sd_response(wpa_s, sa, update_indic, tlvs, tlvs_len); | |
955 | } | |
956 | ||
957 | ||
958 | u64 wpas_p2p_sd_request(struct wpa_supplicant *wpa_s, const u8 *dst, | |
959 | const struct wpabuf *tlvs) | |
960 | { | |
961 | if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) | |
962 | return 0; | |
963 | return (uintptr_t) p2p_sd_request(wpa_s->global->p2p, dst, tlvs); | |
964 | } | |
965 | ||
966 | ||
967 | u64 wpas_p2p_sd_request_upnp(struct wpa_supplicant *wpa_s, const u8 *dst, | |
968 | u8 version, const char *query) | |
969 | { | |
970 | struct wpabuf *tlvs; | |
971 | u64 ret; | |
972 | ||
973 | tlvs = wpabuf_alloc(2 + 1 + 1 + 1 + os_strlen(query)); | |
974 | if (tlvs == NULL) | |
975 | return 0; | |
976 | wpabuf_put_le16(tlvs, 1 + 1 + 1 + os_strlen(query)); | |
977 | wpabuf_put_u8(tlvs, P2P_SERV_UPNP); /* Service Protocol Type */ | |
978 | wpabuf_put_u8(tlvs, 1); /* Service Transaction ID */ | |
979 | wpabuf_put_u8(tlvs, version); | |
980 | wpabuf_put_str(tlvs, query); | |
981 | ret = wpas_p2p_sd_request(wpa_s, dst, tlvs); | |
982 | wpabuf_free(tlvs); | |
983 | return ret; | |
984 | } | |
985 | ||
986 | ||
987 | u64 wpas_p2p_sd_request_asp(struct wpa_supplicant *wpa_s, const u8 *dst, u8 id, | |
988 | const char *svc_str, const char *info_substr) | |
989 | { | |
990 | struct wpabuf *tlvs; | |
991 | size_t plen, svc_len, substr_len = 0; | |
992 | u64 ret; | |
993 | ||
994 | svc_len = os_strlen(svc_str); | |
995 | if (info_substr) | |
996 | substr_len = os_strlen(info_substr); | |
997 | ||
998 | if (svc_len > 0xff || substr_len > 0xff) | |
999 | return 0; | |
1000 | ||
1001 | plen = 1 + 1 + 1 + svc_len + 1 + substr_len; | |
1002 | tlvs = wpabuf_alloc(2 + plen); | |
1003 | if (tlvs == NULL) | |
1004 | return 0; | |
1005 | ||
1006 | wpabuf_put_le16(tlvs, plen); | |
1007 | wpabuf_put_u8(tlvs, P2P_SERV_P2PS); | |
1008 | wpabuf_put_u8(tlvs, id); /* Service Transaction ID */ | |
1009 | wpabuf_put_u8(tlvs, (u8) svc_len); /* Service String Length */ | |
1010 | wpabuf_put_data(tlvs, svc_str, svc_len); | |
1011 | wpabuf_put_u8(tlvs, (u8) substr_len); /* Info Substring Length */ | |
1012 | wpabuf_put_data(tlvs, info_substr, substr_len); | |
1013 | ret = wpas_p2p_sd_request(wpa_s, dst, tlvs); | |
1014 | wpabuf_free(tlvs); | |
1015 | ||
1016 | return ret; | |
1017 | } | |
1018 | ||
1019 | ||
1020 | #ifdef CONFIG_WIFI_DISPLAY | |
1021 | ||
1022 | static u64 wpas_p2p_sd_request_wfd(struct wpa_supplicant *wpa_s, const u8 *dst, | |
1023 | const struct wpabuf *tlvs) | |
1024 | { | |
1025 | if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) | |
1026 | return 0; | |
1027 | return (uintptr_t) p2p_sd_request_wfd(wpa_s->global->p2p, dst, tlvs); | |
1028 | } | |
1029 | ||
1030 | ||
1031 | #define MAX_WFD_SD_SUBELEMS 20 | |
1032 | ||
1033 | static void wfd_add_sd_req_role(struct wpabuf *tlvs, u8 id, u8 role, | |
1034 | const char *subelems) | |
1035 | { | |
1036 | u8 *len; | |
1037 | const char *pos; | |
1038 | int val; | |
1039 | int count = 0; | |
1040 | ||
1041 | len = wpabuf_put(tlvs, 2); | |
1042 | wpabuf_put_u8(tlvs, P2P_SERV_WIFI_DISPLAY); /* Service Protocol Type */ | |
1043 | wpabuf_put_u8(tlvs, id); /* Service Transaction ID */ | |
1044 | ||
1045 | wpabuf_put_u8(tlvs, role); | |
1046 | ||
1047 | pos = subelems; | |
1048 | while (*pos) { | |
1049 | val = atoi(pos); | |
1050 | if (val >= 0 && val < 256) { | |
1051 | wpabuf_put_u8(tlvs, val); | |
1052 | count++; | |
1053 | if (count == MAX_WFD_SD_SUBELEMS) | |
1054 | break; | |
1055 | } | |
1056 | pos = os_strchr(pos + 1, ','); | |
1057 | if (pos == NULL) | |
1058 | break; | |
1059 | pos++; | |
1060 | } | |
1061 | ||
1062 | WPA_PUT_LE16(len, (u8 *) wpabuf_put(tlvs, 0) - len - 2); | |
1063 | } | |
1064 | ||
1065 | ||
1066 | u64 wpas_p2p_sd_request_wifi_display(struct wpa_supplicant *wpa_s, | |
1067 | const u8 *dst, const char *role) | |
1068 | { | |
1069 | struct wpabuf *tlvs; | |
1070 | u64 ret; | |
1071 | const char *subelems; | |
1072 | u8 id = 1; | |
1073 | ||
1074 | subelems = os_strchr(role, ' '); | |
1075 | if (subelems == NULL) | |
1076 | return 0; | |
1077 | subelems++; | |
1078 | ||
1079 | tlvs = wpabuf_alloc(4 * (2 + 1 + 1 + 1 + MAX_WFD_SD_SUBELEMS)); | |
1080 | if (tlvs == NULL) | |
1081 | return 0; | |
1082 | ||
1083 | if (os_strstr(role, "[source]")) | |
1084 | wfd_add_sd_req_role(tlvs, id++, 0x00, subelems); | |
1085 | if (os_strstr(role, "[pri-sink]")) | |
1086 | wfd_add_sd_req_role(tlvs, id++, 0x01, subelems); | |
1087 | if (os_strstr(role, "[sec-sink]")) | |
1088 | wfd_add_sd_req_role(tlvs, id++, 0x02, subelems); | |
1089 | if (os_strstr(role, "[source+sink]")) | |
1090 | wfd_add_sd_req_role(tlvs, id++, 0x03, subelems); | |
1091 | ||
1092 | ret = wpas_p2p_sd_request_wfd(wpa_s, dst, tlvs); | |
1093 | wpabuf_free(tlvs); | |
1094 | return ret; | |
1095 | } | |
1096 | ||
1097 | #endif /* CONFIG_WIFI_DISPLAY */ | |
1098 | ||
1099 | ||
1100 | int wpas_p2p_sd_cancel_request(struct wpa_supplicant *wpa_s, u64 req) | |
1101 | { | |
1102 | if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) | |
1103 | return -1; | |
1104 | return p2p_sd_cancel_request(wpa_s->global->p2p, | |
1105 | (void *) (uintptr_t) req); | |
1106 | } | |
1107 | ||
1108 | ||
1109 | void wpas_p2p_sd_response(struct wpa_supplicant *wpa_s, int freq, | |
1110 | const u8 *dst, u8 dialog_token, | |
1111 | const struct wpabuf *resp_tlvs) | |
1112 | { | |
1113 | if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) | |
1114 | return; | |
1115 | p2p_sd_response(wpa_s->global->p2p, freq, dst, dialog_token, | |
1116 | resp_tlvs); | |
1117 | } | |
1118 | ||
1119 | ||
1120 | void wpas_p2p_sd_service_update(struct wpa_supplicant *wpa_s) | |
1121 | { | |
1122 | if (wpa_s->global->p2p) | |
1123 | p2p_sd_service_update(wpa_s->global->p2p); | |
1124 | } | |
1125 | ||
1126 | ||
1127 | static void wpas_p2p_srv_bonjour_free(struct p2p_srv_bonjour *bsrv) | |
1128 | { | |
1129 | dl_list_del(&bsrv->list); | |
1130 | wpabuf_free(bsrv->query); | |
1131 | wpabuf_free(bsrv->resp); | |
1132 | os_free(bsrv); | |
1133 | } | |
1134 | ||
1135 | ||
1136 | static void wpas_p2p_srv_upnp_free(struct p2p_srv_upnp *usrv) | |
1137 | { | |
1138 | dl_list_del(&usrv->list); | |
1139 | os_free(usrv->service); | |
1140 | os_free(usrv); | |
1141 | } | |
1142 | ||
1143 | ||
1144 | void wpas_p2p_service_flush(struct wpa_supplicant *wpa_s) | |
1145 | { | |
1146 | struct p2p_srv_bonjour *bsrv, *bn; | |
1147 | struct p2p_srv_upnp *usrv, *un; | |
1148 | ||
1149 | dl_list_for_each_safe(bsrv, bn, &wpa_s->global->p2p_srv_bonjour, | |
1150 | struct p2p_srv_bonjour, list) | |
1151 | wpas_p2p_srv_bonjour_free(bsrv); | |
1152 | ||
1153 | dl_list_for_each_safe(usrv, un, &wpa_s->global->p2p_srv_upnp, | |
1154 | struct p2p_srv_upnp, list) | |
1155 | wpas_p2p_srv_upnp_free(usrv); | |
1156 | ||
c40a8918 | 1157 | wpas_p2p_service_flush_asp(wpa_s); |
0f893285 JM |
1158 | wpas_p2p_sd_service_update(wpa_s); |
1159 | } | |
1160 | ||
1161 | ||
1162 | int wpas_p2p_service_p2ps_id_exists(struct wpa_supplicant *wpa_s, u32 adv_id) | |
1163 | { | |
1164 | if (adv_id == 0) | |
1165 | return 1; | |
1166 | ||
1167 | if (p2p_service_p2ps_id(wpa_s->global->p2p, adv_id)) | |
1168 | return 1; | |
1169 | ||
1170 | return 0; | |
1171 | } | |
1172 | ||
1173 | ||
1174 | int wpas_p2p_service_del_asp(struct wpa_supplicant *wpa_s, u32 adv_id) | |
1175 | { | |
2dc422e2 MS |
1176 | int ret; |
1177 | ||
1178 | ret = p2p_service_del_asp(wpa_s->global->p2p, adv_id); | |
1179 | if (ret == 0) | |
1180 | wpas_p2p_sd_service_update(wpa_s); | |
1181 | return ret; | |
0f893285 JM |
1182 | } |
1183 | ||
1184 | ||
1185 | int wpas_p2p_service_add_asp(struct wpa_supplicant *wpa_s, | |
1186 | int auto_accept, u32 adv_id, | |
1187 | const char *adv_str, u8 svc_state, | |
e2b7fbf2 MS |
1188 | u16 config_methods, const char *svc_info, |
1189 | const u8 *cpt_priority) | |
0f893285 | 1190 | { |
2dc422e2 MS |
1191 | int ret; |
1192 | ||
1193 | ret = p2p_service_add_asp(wpa_s->global->p2p, auto_accept, adv_id, | |
1194 | adv_str, svc_state, config_methods, | |
e2b7fbf2 | 1195 | svc_info, cpt_priority); |
2dc422e2 MS |
1196 | if (ret == 0) |
1197 | wpas_p2p_sd_service_update(wpa_s); | |
1198 | return ret; | |
0f893285 JM |
1199 | } |
1200 | ||
1201 | ||
c40a8918 MS |
1202 | void wpas_p2p_service_flush_asp(struct wpa_supplicant *wpa_s) |
1203 | { | |
1204 | p2p_service_flush_asp(wpa_s->global->p2p); | |
1205 | } | |
1206 | ||
1207 | ||
0f893285 JM |
1208 | int wpas_p2p_service_add_bonjour(struct wpa_supplicant *wpa_s, |
1209 | struct wpabuf *query, struct wpabuf *resp) | |
1210 | { | |
1211 | struct p2p_srv_bonjour *bsrv; | |
1212 | ||
1213 | bsrv = os_zalloc(sizeof(*bsrv)); | |
1214 | if (bsrv == NULL) | |
1215 | return -1; | |
1216 | bsrv->query = query; | |
1217 | bsrv->resp = resp; | |
1218 | dl_list_add(&wpa_s->global->p2p_srv_bonjour, &bsrv->list); | |
1219 | ||
1220 | wpas_p2p_sd_service_update(wpa_s); | |
1221 | return 0; | |
1222 | } | |
1223 | ||
1224 | ||
1225 | int wpas_p2p_service_del_bonjour(struct wpa_supplicant *wpa_s, | |
1226 | const struct wpabuf *query) | |
1227 | { | |
1228 | struct p2p_srv_bonjour *bsrv; | |
1229 | ||
1230 | bsrv = wpas_p2p_service_get_bonjour(wpa_s, query); | |
1231 | if (bsrv == NULL) | |
1232 | return -1; | |
1233 | wpas_p2p_srv_bonjour_free(bsrv); | |
1234 | wpas_p2p_sd_service_update(wpa_s); | |
1235 | return 0; | |
1236 | } | |
1237 | ||
1238 | ||
1239 | int wpas_p2p_service_add_upnp(struct wpa_supplicant *wpa_s, u8 version, | |
1240 | const char *service) | |
1241 | { | |
1242 | struct p2p_srv_upnp *usrv; | |
1243 | ||
1244 | if (wpas_p2p_service_get_upnp(wpa_s, version, service)) | |
1245 | return 0; /* Already listed */ | |
1246 | usrv = os_zalloc(sizeof(*usrv)); | |
1247 | if (usrv == NULL) | |
1248 | return -1; | |
1249 | usrv->version = version; | |
1250 | usrv->service = os_strdup(service); | |
1251 | if (usrv->service == NULL) { | |
1252 | os_free(usrv); | |
1253 | return -1; | |
1254 | } | |
1255 | dl_list_add(&wpa_s->global->p2p_srv_upnp, &usrv->list); | |
1256 | ||
1257 | wpas_p2p_sd_service_update(wpa_s); | |
1258 | return 0; | |
1259 | } | |
1260 | ||
1261 | ||
1262 | int wpas_p2p_service_del_upnp(struct wpa_supplicant *wpa_s, u8 version, | |
1263 | const char *service) | |
1264 | { | |
1265 | struct p2p_srv_upnp *usrv; | |
1266 | ||
1267 | usrv = wpas_p2p_service_get_upnp(wpa_s, version, service); | |
1268 | if (usrv == NULL) | |
1269 | return -1; | |
1270 | wpas_p2p_srv_upnp_free(usrv); | |
1271 | wpas_p2p_sd_service_update(wpa_s); | |
1272 | return 0; | |
1273 | } |