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 
p2p_sd_dns_uncompress_label(char ** upos,char * uend,u8 * start,u8 ** spos,const u8 * end)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 */
51 			if (end - *spos < 2) {
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)++;
77 		if (len > end - *spos) {
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 
84 		if (len + 2 > uend - *upos)
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 */
p2p_sd_dns_uncompress(char * buf,size_t buf_len,const u8 * msg,size_t msg_len,size_t offset)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 *
wpas_p2p_service_get_bonjour(struct wpa_supplicant * wpa_s,const struct wpabuf * query)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 *
wpas_p2p_service_get_upnp(struct wpa_supplicant * wpa_s,u8 version,const char * service)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 
wpas_sd_add_empty(struct wpabuf * resp,u8 srv_proto,u8 srv_trans_id,u8 status)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 
wpas_sd_add_proto_not_avail(struct wpabuf * resp,u8 srv_proto,u8 srv_trans_id)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 
wpas_sd_add_bad_request(struct wpabuf * resp,u8 srv_proto,u8 srv_trans_id)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 
wpas_sd_add_not_found(struct wpabuf * resp,u8 srv_proto,u8 srv_trans_id)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 
wpas_sd_all_bonjour(struct wpa_supplicant * wpa_s,struct wpabuf * resp,u8 srv_trans_id)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 
match_bonjour_query(struct p2p_srv_bonjour * bsrv,const u8 * query,size_t query_len)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 
wpas_sd_req_bonjour(struct wpa_supplicant * wpa_s,struct wpabuf * resp,u8 srv_trans_id,const u8 * query,size_t query_len)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 
wpas_sd_all_upnp(struct wpa_supplicant * wpa_s,struct wpabuf * resp,u8 srv_trans_id)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 
wpas_sd_req_upnp(struct wpa_supplicant * wpa_s,struct wpabuf * resp,u8 srv_trans_id,const u8 * query,size_t query_len)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
wpas_sd_req_wfd(struct wpa_supplicant * wpa_s,struct wpabuf * resp,u8 srv_trans_id,const u8 * query,size_t query_len)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 
find_p2ps_substr(struct p2ps_advertisement * adv_data,const u8 * needle,size_t needle_len)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 
wpas_sd_req_asp(struct wpa_supplicant * wpa_s,struct wpabuf * resp,u8 srv_trans_id,const u8 * query,size_t query_len)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 
wpas_sd_all_asp(struct wpa_supplicant * wpa_s,struct wpabuf * resp,u8 srv_trans_id)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 
wpas_sd_request(void * ctx,int freq,const u8 * sa,u8 dialog_token,u16 update_indic,const u8 * tlvs,size_t tlvs_len)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 
725 	while (end - pos > 1) {
726 		wpa_printf(MSG_DEBUG, "P2P: Service Request TLV");
727 		slen = WPA_GET_LE16(pos);
728 		pos += 2;
729 		if (slen > end - pos || slen < 2) {
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);
753 			wpas_sd_all_asp(wpa_s, resp, srv_trans_id);
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) &&
762 			    dl_list_empty(&wpa_s->global->p2p_srv_bonjour) &&
763 			    !p2p_get_p2ps_adv_list(wpa_s->global->p2p)) {
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);
773 			wpas_sd_all_asp(wpa_s, resp, srv_trans_id);
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 
wpas_sd_p2ps_serv_response(struct wpa_supplicant * wpa_s,const u8 * sa,u8 srv_trans_id,const u8 * pos,const u8 * tlv_end)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 		/* Validity check fixed length+svc_str */
830 		if (6 >= tlv_end - pos)
831 			break;
832 		svc_len = pos[6];
833 		if (svc_len + 10 > tlv_end - pos)
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 		/* Validity 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 
wpas_sd_response(void * ctx,const u8 * sa,u16 update_indic,const u8 * tlvs,size_t tlvs_len)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 
920 	while (end - pos >= 2) {
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;
926 		if (slen > end - pos || slen < 3) {
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 
wpas_p2p_sd_request(struct wpa_supplicant * wpa_s,const u8 * dst,const struct wpabuf * tlvs)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 
wpas_p2p_sd_request_upnp(struct wpa_supplicant * wpa_s,const u8 * dst,u8 version,const char * query)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 
wpas_p2p_sd_request_asp(struct wpa_supplicant * wpa_s,const u8 * dst,u8 id,const char * svc_str,const char * info_substr)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 
wpas_p2p_sd_request_wfd(struct wpa_supplicant * wpa_s,const u8 * dst,const struct wpabuf * tlvs)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 
wfd_add_sd_req_role(struct wpabuf * tlvs,u8 id,u8 role,const char * subelems)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 
wpas_p2p_sd_request_wifi_display(struct wpa_supplicant * wpa_s,const u8 * dst,const char * role)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 
wpas_p2p_sd_cancel_request(struct wpa_supplicant * wpa_s,u64 req)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 
wpas_p2p_sd_response(struct wpa_supplicant * wpa_s,int freq,const u8 * dst,u8 dialog_token,const struct wpabuf * resp_tlvs)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 
wpas_p2p_sd_service_update(struct wpa_supplicant * wpa_s)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 
wpas_p2p_srv_bonjour_free(struct p2p_srv_bonjour * bsrv)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 
wpas_p2p_srv_upnp_free(struct p2p_srv_upnp * usrv)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 
wpas_p2p_service_flush(struct wpa_supplicant * wpa_s)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 
1157 	wpas_p2p_service_flush_asp(wpa_s);
1158 	wpas_p2p_sd_service_update(wpa_s);
1159 }
1160 
1161 
wpas_p2p_service_p2ps_id_exists(struct wpa_supplicant * wpa_s,u32 adv_id)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 
wpas_p2p_service_del_asp(struct wpa_supplicant * wpa_s,u32 adv_id)1174 int wpas_p2p_service_del_asp(struct wpa_supplicant *wpa_s, u32 adv_id)
1175 {
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;
1182 }
1183 
1184 
wpas_p2p_service_add_asp(struct wpa_supplicant * wpa_s,int auto_accept,u32 adv_id,const char * adv_str,u8 svc_state,u16 config_methods,const char * svc_info,const u8 * cpt_priority)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,
1188 			     u16 config_methods, const char *svc_info,
1189 			     const u8 *cpt_priority)
1190 {
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,
1195 				  svc_info, cpt_priority);
1196 	if (ret == 0)
1197 		wpas_p2p_sd_service_update(wpa_s);
1198 	return ret;
1199 }
1200 
1201 
wpas_p2p_service_flush_asp(struct wpa_supplicant * wpa_s)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 
wpas_p2p_service_add_bonjour(struct wpa_supplicant * wpa_s,struct wpabuf * query,struct wpabuf * resp)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 
wpas_p2p_service_del_bonjour(struct wpa_supplicant * wpa_s,const struct wpabuf * query)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 
wpas_p2p_service_add_upnp(struct wpa_supplicant * wpa_s,u8 version,const char * service)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 
wpas_p2p_service_del_upnp(struct wpa_supplicant * wpa_s,u8 version,const char * service)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 }
1274