1 /*
2  * Wi-Fi Direct - P2P service discovery
3  * Copyright (c) 2009, Atheros Communications
4  *
5  * This software may be distributed under the terms of the BSD license.
6  * See README for more details.
7  */
8 
9 #include "includes.h"
10 
11 #include "common.h"
12 #include "common/ieee802_11_defs.h"
13 #include "common/gas.h"
14 #include "p2p_i.h"
15 #include "p2p.h"
16 
17 
18 #ifdef CONFIG_WIFI_DISPLAY
19 static int wfd_wsd_supported(struct wpabuf *wfd)
20 {
21 	const u8 *pos, *end;
22 	u8 subelem;
23 	u16 len;
24 
25 	if (wfd == NULL)
26 		return 0;
27 
28 	pos = wpabuf_head(wfd);
29 	end = pos + wpabuf_len(wfd);
30 
31 	while (pos + 3 <= end) {
32 		subelem = *pos++;
33 		len = WPA_GET_BE16(pos);
34 		pos += 2;
35 		if (pos + len > end)
36 			break;
37 
38 		if (subelem == WFD_SUBELEM_DEVICE_INFO && len >= 6) {
39 			u16 info = WPA_GET_BE16(pos);
40 			return !!(info & 0x0040);
41 		}
42 
43 		pos += len;
44 	}
45 
46 	return 0;
47 }
48 #endif /* CONFIG_WIFI_DISPLAY */
49 
50 struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p,
51 					 struct p2p_device *dev)
52 {
53 	struct p2p_sd_query *q;
54 	int wsd = 0;
55 
56 	if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY))
57 		return NULL; /* peer does not support SD */
58 #ifdef CONFIG_WIFI_DISPLAY
59 	if (wfd_wsd_supported(dev->info.wfd_subelems))
60 		wsd = 1;
61 #endif /* CONFIG_WIFI_DISPLAY */
62 
63 	for (q = p2p->sd_queries; q; q = q->next) {
64 		/* Use WSD only if the peer indicates support or it */
65 		if (q->wsd && !wsd)
66 			continue;
67 		if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO))
68 			return q;
69 		if (!q->for_all_peers &&
70 		    os_memcmp(q->peer, dev->info.p2p_device_addr, ETH_ALEN) ==
71 		    0)
72 			return q;
73 	}
74 
75 	return NULL;
76 }
77 
78 
79 static int p2p_unlink_sd_query(struct p2p_data *p2p,
80 			       struct p2p_sd_query *query)
81 {
82 	struct p2p_sd_query *q, *prev;
83 	q = p2p->sd_queries;
84 	prev = NULL;
85 	while (q) {
86 		if (q == query) {
87 			if (prev)
88 				prev->next = q->next;
89 			else
90 				p2p->sd_queries = q->next;
91 			if (p2p->sd_query == query)
92 				p2p->sd_query = NULL;
93 			return 1;
94 		}
95 		prev = q;
96 		q = q->next;
97 	}
98 	return 0;
99 }
100 
101 
102 static void p2p_free_sd_query(struct p2p_sd_query *q)
103 {
104 	if (q == NULL)
105 		return;
106 	wpabuf_free(q->tlvs);
107 	os_free(q);
108 }
109 
110 
111 void p2p_free_sd_queries(struct p2p_data *p2p)
112 {
113 	struct p2p_sd_query *q, *prev;
114 	q = p2p->sd_queries;
115 	p2p->sd_queries = NULL;
116 	while (q) {
117 		prev = q;
118 		q = q->next;
119 		p2p_free_sd_query(prev);
120 	}
121 }
122 
123 
124 static struct wpabuf * p2p_build_sd_query(u16 update_indic,
125 					  struct wpabuf *tlvs)
126 {
127 	struct wpabuf *buf;
128 	u8 *len_pos;
129 
130 	buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs));
131 	if (buf == NULL)
132 		return NULL;
133 
134 	/* ANQP Query Request Frame */
135 	len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
136 	wpabuf_put_be24(buf, OUI_WFA);
137 	wpabuf_put_u8(buf, P2P_OUI_TYPE);
138 	wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
139 	wpabuf_put_buf(buf, tlvs);
140 	gas_anqp_set_element_len(buf, len_pos);
141 
142 	gas_anqp_set_len(buf);
143 
144 	return buf;
145 }
146 
147 
148 static void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst,
149 				      u8 dialog_token, int freq)
150 {
151 	struct wpabuf *req;
152 
153 	req = gas_build_comeback_req(dialog_token);
154 	if (req == NULL)
155 		return;
156 
157 	p2p->pending_action_state = P2P_NO_PENDING_ACTION;
158 	if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst,
159 			    wpabuf_head(req), wpabuf_len(req), 200) < 0)
160 		p2p_dbg(p2p, "Failed to send Action frame");
161 
162 	wpabuf_free(req);
163 }
164 
165 
166 static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
167 					     u16 comeback_delay,
168 					     u16 update_indic,
169 					     const struct wpabuf *tlvs)
170 {
171 	struct wpabuf *buf;
172 	u8 *len_pos;
173 
174 	buf = gas_anqp_build_initial_resp(dialog_token, status_code,
175 					  comeback_delay,
176 					  100 + (tlvs ? wpabuf_len(tlvs) : 0));
177 	if (buf == NULL)
178 		return NULL;
179 
180 	if (tlvs) {
181 		/* ANQP Query Response Frame */
182 		len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
183 		wpabuf_put_be24(buf, OUI_WFA);
184 		wpabuf_put_u8(buf, P2P_OUI_TYPE);
185 		 /* Service Update Indicator */
186 		wpabuf_put_le16(buf, update_indic);
187 		wpabuf_put_buf(buf, tlvs);
188 		gas_anqp_set_element_len(buf, len_pos);
189 	}
190 
191 	gas_anqp_set_len(buf);
192 
193 	return buf;
194 }
195 
196 
197 static struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token,
198 						   u16 status_code,
199 						   u16 update_indic,
200 						   const u8 *data, size_t len,
201 						   u8 frag_id, u8 more,
202 						   u16 total_len)
203 {
204 	struct wpabuf *buf;
205 
206 	buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id,
207 					   more, 0, 100 + len);
208 	if (buf == NULL)
209 		return NULL;
210 
211 	if (frag_id == 0) {
212 		/* ANQP Query Response Frame */
213 		wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */
214 		wpabuf_put_le16(buf, 3 + 1 + 2 + total_len);
215 		wpabuf_put_be24(buf, OUI_WFA);
216 		wpabuf_put_u8(buf, P2P_OUI_TYPE);
217 		/* Service Update Indicator */
218 		wpabuf_put_le16(buf, update_indic);
219 	}
220 
221 	wpabuf_put_data(buf, data, len);
222 	gas_anqp_set_len(buf);
223 
224 	return buf;
225 }
226 
227 
228 int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev)
229 {
230 	struct wpabuf *req;
231 	int ret = 0;
232 	struct p2p_sd_query *query;
233 	int freq;
234 
235 	freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq;
236 	if (freq <= 0) {
237 		p2p_dbg(p2p, "No Listen/Operating frequency known for the peer "
238 			MACSTR " to send SD Request",
239 			MAC2STR(dev->info.p2p_device_addr));
240 		return -1;
241 	}
242 
243 	query = p2p_pending_sd_req(p2p, dev);
244 	if (query == NULL)
245 		return -1;
246 
247 	p2p_dbg(p2p, "Start Service Discovery with " MACSTR,
248 		MAC2STR(dev->info.p2p_device_addr));
249 
250 	req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs);
251 	if (req == NULL)
252 		return -1;
253 
254 	p2p->sd_peer = dev;
255 	p2p->sd_query = query;
256 	p2p->pending_action_state = P2P_PENDING_SD;
257 
258 	if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr,
259 			    p2p->cfg->dev_addr, dev->info.p2p_device_addr,
260 			    wpabuf_head(req), wpabuf_len(req), 5000) < 0) {
261 		p2p_dbg(p2p, "Failed to send Action frame");
262 		ret = -1;
263 	}
264 
265 	wpabuf_free(req);
266 
267 	return ret;
268 }
269 
270 
271 void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
272 			    const u8 *data, size_t len, int rx_freq)
273 {
274 	const u8 *pos = data;
275 	const u8 *end = data + len;
276 	const u8 *next;
277 	u8 dialog_token;
278 	u16 slen;
279 	int freq;
280 	u16 update_indic;
281 
282 
283 	if (p2p->cfg->sd_request == NULL)
284 		return;
285 
286 	if (rx_freq > 0)
287 		freq = rx_freq;
288 	else
289 		freq = p2p_channel_to_freq(p2p->cfg->reg_class,
290 					   p2p->cfg->channel);
291 	if (freq < 0)
292 		return;
293 
294 	if (len < 1 + 2)
295 		return;
296 
297 	dialog_token = *pos++;
298 	p2p_dbg(p2p, "GAS Initial Request from " MACSTR
299 		" (dialog token %u, freq %d)",
300 		MAC2STR(sa), dialog_token, rx_freq);
301 
302 	if (*pos != WLAN_EID_ADV_PROTO) {
303 		p2p_dbg(p2p, "Unexpected IE in GAS Initial Request: %u", *pos);
304 		return;
305 	}
306 	pos++;
307 
308 	slen = *pos++;
309 	next = pos + slen;
310 	if (next > end || slen < 2) {
311 		p2p_dbg(p2p, "Invalid IE in GAS Initial Request");
312 		return;
313 	}
314 	pos++; /* skip QueryRespLenLimit and PAME-BI */
315 
316 	if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
317 		p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
318 			*pos);
319 		return;
320 	}
321 
322 	pos = next;
323 	/* Query Request */
324 	if (pos + 2 > end)
325 		return;
326 	slen = WPA_GET_LE16(pos);
327 	pos += 2;
328 	if (pos + slen > end)
329 		return;
330 	end = pos + slen;
331 
332 	/* ANQP Query Request */
333 	if (pos + 4 > end)
334 		return;
335 	if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
336 		p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
337 		return;
338 	}
339 	pos += 2;
340 
341 	slen = WPA_GET_LE16(pos);
342 	pos += 2;
343 	if (pos + slen > end || slen < 3 + 1) {
344 		p2p_dbg(p2p, "Invalid ANQP Query Request length");
345 		return;
346 	}
347 
348 	if (WPA_GET_BE24(pos) != OUI_WFA) {
349 		p2p_dbg(p2p, "Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
350 		return;
351 	}
352 	pos += 3;
353 
354 	if (*pos != P2P_OUI_TYPE) {
355 		p2p_dbg(p2p, "Unsupported ANQP vendor type %u", *pos);
356 		return;
357 	}
358 	pos++;
359 
360 	if (pos + 2 > end)
361 		return;
362 	update_indic = WPA_GET_LE16(pos);
363 	p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
364 	pos += 2;
365 
366 	p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token,
367 			     update_indic, pos, end - pos);
368 	/* the response will be indicated with a call to p2p_sd_response() */
369 }
370 
371 
372 void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
373 		     u8 dialog_token, const struct wpabuf *resp_tlvs)
374 {
375 	struct wpabuf *resp;
376 
377 	/* TODO: fix the length limit to match with the maximum frame length */
378 	if (wpabuf_len(resp_tlvs) > 1400) {
379 		p2p_dbg(p2p, "SD response long enough to require fragmentation");
380 		if (p2p->sd_resp) {
381 			/*
382 			 * TODO: Could consider storing the fragmented response
383 			 * separately for each peer to avoid having to drop old
384 			 * one if there is more than one pending SD query.
385 			 * Though, that would eat more memory, so there are
386 			 * also benefits to just using a single buffer.
387 			 */
388 			p2p_dbg(p2p, "Drop previous SD response");
389 			wpabuf_free(p2p->sd_resp);
390 		}
391 		p2p->sd_resp = wpabuf_dup(resp_tlvs);
392 		if (p2p->sd_resp == NULL) {
393 			p2p_err(p2p, "Failed to allocate SD response fragmentation area");
394 			return;
395 		}
396 		os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
397 		p2p->sd_resp_dialog_token = dialog_token;
398 		p2p->sd_resp_pos = 0;
399 		p2p->sd_frag_id = 0;
400 		resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
401 					     1, p2p->srv_update_indic, NULL);
402 	} else {
403 		p2p_dbg(p2p, "SD response fits in initial response");
404 		resp = p2p_build_sd_response(dialog_token,
405 					     WLAN_STATUS_SUCCESS, 0,
406 					     p2p->srv_update_indic, resp_tlvs);
407 	}
408 	if (resp == NULL)
409 		return;
410 
411 	p2p->pending_action_state = P2P_NO_PENDING_ACTION;
412 	if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr,
413 			    p2p->cfg->dev_addr,
414 			    wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
415 		p2p_dbg(p2p, "Failed to send Action frame");
416 
417 	wpabuf_free(resp);
418 }
419 
420 
421 void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
422 			     const u8 *data, size_t len, int rx_freq)
423 {
424 	const u8 *pos = data;
425 	const u8 *end = data + len;
426 	const u8 *next;
427 	u8 dialog_token;
428 	u16 status_code;
429 	u16 comeback_delay;
430 	u16 slen;
431 	u16 update_indic;
432 
433 	if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
434 	    os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
435 		p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from "
436 			MACSTR, MAC2STR(sa));
437 		return;
438 	}
439 	p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
440 	p2p_clear_timeout(p2p);
441 
442 	p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)",
443 		MAC2STR(sa), (int) len);
444 
445 	if (len < 5 + 2) {
446 		p2p_dbg(p2p, "Too short GAS Initial Response frame");
447 		return;
448 	}
449 
450 	dialog_token = *pos++;
451 	/* TODO: check dialog_token match */
452 	status_code = WPA_GET_LE16(pos);
453 	pos += 2;
454 	comeback_delay = WPA_GET_LE16(pos);
455 	pos += 2;
456 	p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u",
457 		dialog_token, status_code, comeback_delay);
458 	if (status_code) {
459 		p2p_dbg(p2p, "Service Discovery failed: status code %u",
460 			status_code);
461 		return;
462 	}
463 
464 	if (*pos != WLAN_EID_ADV_PROTO) {
465 		p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos);
466 		return;
467 	}
468 	pos++;
469 
470 	slen = *pos++;
471 	next = pos + slen;
472 	if (next > end || slen < 2) {
473 		p2p_dbg(p2p, "Invalid IE in GAS Initial Response");
474 		return;
475 	}
476 	pos++; /* skip QueryRespLenLimit and PAME-BI */
477 
478 	if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
479 		p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
480 			*pos);
481 		return;
482 	}
483 
484 	pos = next;
485 	/* Query Response */
486 	if (pos + 2 > end) {
487 		p2p_dbg(p2p, "Too short Query Response");
488 		return;
489 	}
490 	slen = WPA_GET_LE16(pos);
491 	pos += 2;
492 	p2p_dbg(p2p, "Query Response Length: %d", slen);
493 	if (pos + slen > end) {
494 		p2p_dbg(p2p, "Not enough Query Response data");
495 		return;
496 	}
497 	end = pos + slen;
498 
499 	if (comeback_delay) {
500 		p2p_dbg(p2p, "Fragmented response - request fragments");
501 		if (p2p->sd_rx_resp) {
502 			p2p_dbg(p2p, "Drop old SD reassembly buffer");
503 			wpabuf_free(p2p->sd_rx_resp);
504 			p2p->sd_rx_resp = NULL;
505 		}
506 		p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
507 		return;
508 	}
509 
510 	/* ANQP Query Response */
511 	if (pos + 4 > end)
512 		return;
513 	if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
514 		p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
515 		return;
516 	}
517 	pos += 2;
518 
519 	slen = WPA_GET_LE16(pos);
520 	pos += 2;
521 	if (pos + slen > end || slen < 3 + 1) {
522 		p2p_dbg(p2p, "Invalid ANQP Query Response length");
523 		return;
524 	}
525 
526 	if (WPA_GET_BE24(pos) != OUI_WFA) {
527 		p2p_dbg(p2p, "Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
528 		return;
529 	}
530 	pos += 3;
531 
532 	if (*pos != P2P_OUI_TYPE) {
533 		p2p_dbg(p2p, "Unsupported ANQP vendor type %u", *pos);
534 		return;
535 	}
536 	pos++;
537 
538 	if (pos + 2 > end)
539 		return;
540 	update_indic = WPA_GET_LE16(pos);
541 	p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
542 	pos += 2;
543 
544 	p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
545 	p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
546 	p2p->sd_peer = NULL;
547 
548 	if (p2p->sd_query) {
549 		if (!p2p->sd_query->for_all_peers) {
550 			struct p2p_sd_query *q;
551 			p2p_dbg(p2p, "Remove completed SD query %p",
552 				p2p->sd_query);
553 			q = p2p->sd_query;
554 			p2p_unlink_sd_query(p2p, p2p->sd_query);
555 			p2p_free_sd_query(q);
556 		}
557 		p2p->sd_query = NULL;
558 	}
559 
560 	if (p2p->cfg->sd_response)
561 		p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
562 				      pos, end - pos);
563 	p2p_continue_find(p2p);
564 }
565 
566 
567 void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
568 			     const u8 *data, size_t len, int rx_freq)
569 {
570 	struct wpabuf *resp;
571 	u8 dialog_token;
572 	size_t frag_len;
573 	int more = 0;
574 
575 	wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
576 	if (len < 1)
577 		return;
578 	dialog_token = *data;
579 	p2p_dbg(p2p, "Dialog Token: %u", dialog_token);
580 	if (dialog_token != p2p->sd_resp_dialog_token) {
581 		p2p_dbg(p2p, "No pending SD response fragment for dialog token %u",
582 			dialog_token);
583 		return;
584 	}
585 
586 	if (p2p->sd_resp == NULL) {
587 		p2p_dbg(p2p, "No pending SD response fragment available");
588 		return;
589 	}
590 	if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
591 		p2p_dbg(p2p, "No pending SD response fragment for " MACSTR,
592 			MAC2STR(sa));
593 		return;
594 	}
595 
596 	frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
597 	if (frag_len > 1400) {
598 		frag_len = 1400;
599 		more = 1;
600 	}
601 	resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
602 					   p2p->srv_update_indic,
603 					   wpabuf_head_u8(p2p->sd_resp) +
604 					   p2p->sd_resp_pos, frag_len,
605 					   p2p->sd_frag_id, more,
606 					   wpabuf_len(p2p->sd_resp));
607 	if (resp == NULL)
608 		return;
609 	p2p_dbg(p2p, "Send GAS Comeback Response (frag_id %d more=%d frag_len=%d)",
610 		p2p->sd_frag_id, more, (int) frag_len);
611 	p2p->sd_frag_id++;
612 	p2p->sd_resp_pos += frag_len;
613 
614 	if (more) {
615 		p2p_dbg(p2p, "%d more bytes remain to be sent",
616 			(int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
617 	} else {
618 		p2p_dbg(p2p, "All fragments of SD response sent");
619 		wpabuf_free(p2p->sd_resp);
620 		p2p->sd_resp = NULL;
621 	}
622 
623 	p2p->pending_action_state = P2P_NO_PENDING_ACTION;
624 	if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr,
625 			    p2p->cfg->dev_addr,
626 			    wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
627 		p2p_dbg(p2p, "Failed to send Action frame");
628 
629 	wpabuf_free(resp);
630 }
631 
632 
633 void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
634 			      const u8 *data, size_t len, int rx_freq)
635 {
636 	const u8 *pos = data;
637 	const u8 *end = data + len;
638 	const u8 *next;
639 	u8 dialog_token;
640 	u16 status_code;
641 	u8 frag_id;
642 	u8 more_frags;
643 	u16 comeback_delay;
644 	u16 slen;
645 
646 	wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
647 
648 	if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
649 	    os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
650 		p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from "
651 			MACSTR, MAC2STR(sa));
652 		return;
653 	}
654 	p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
655 	p2p_clear_timeout(p2p);
656 
657 	p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)",
658 		MAC2STR(sa), (int) len);
659 
660 	if (len < 6 + 2) {
661 		p2p_dbg(p2p, "Too short GAS Comeback Response frame");
662 		return;
663 	}
664 
665 	dialog_token = *pos++;
666 	/* TODO: check dialog_token match */
667 	status_code = WPA_GET_LE16(pos);
668 	pos += 2;
669 	frag_id = *pos & 0x7f;
670 	more_frags = (*pos & 0x80) >> 7;
671 	pos++;
672 	comeback_delay = WPA_GET_LE16(pos);
673 	pos += 2;
674 	p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
675 		"comeback_delay=%u",
676 		dialog_token, status_code, frag_id, more_frags,
677 		comeback_delay);
678 	/* TODO: check frag_id match */
679 	if (status_code) {
680 		p2p_dbg(p2p, "Service Discovery failed: status code %u",
681 			status_code);
682 		return;
683 	}
684 
685 	if (*pos != WLAN_EID_ADV_PROTO) {
686 		p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u",
687 			*pos);
688 		return;
689 	}
690 	pos++;
691 
692 	slen = *pos++;
693 	next = pos + slen;
694 	if (next > end || slen < 2) {
695 		p2p_dbg(p2p, "Invalid IE in GAS Comeback Response");
696 		return;
697 	}
698 	pos++; /* skip QueryRespLenLimit and PAME-BI */
699 
700 	if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
701 		p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
702 			*pos);
703 		return;
704 	}
705 
706 	pos = next;
707 	/* Query Response */
708 	if (pos + 2 > end) {
709 		p2p_dbg(p2p, "Too short Query Response");
710 		return;
711 	}
712 	slen = WPA_GET_LE16(pos);
713 	pos += 2;
714 	p2p_dbg(p2p, "Query Response Length: %d", slen);
715 	if (pos + slen > end) {
716 		p2p_dbg(p2p, "Not enough Query Response data");
717 		return;
718 	}
719 	if (slen == 0) {
720 		p2p_dbg(p2p, "No Query Response data");
721 		return;
722 	}
723 	end = pos + slen;
724 
725 	if (p2p->sd_rx_resp) {
726 		 /*
727 		  * ANQP header is only included in the first fragment; rest of
728 		  * the fragments start with continue TLVs.
729 		  */
730 		goto skip_nqp_header;
731 	}
732 
733 	/* ANQP Query Response */
734 	if (pos + 4 > end)
735 		return;
736 	if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
737 		p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
738 		return;
739 	}
740 	pos += 2;
741 
742 	slen = WPA_GET_LE16(pos);
743 	pos += 2;
744 	p2p_dbg(p2p, "ANQP Query Response length: %u", slen);
745 	if (slen < 3 + 1) {
746 		p2p_dbg(p2p, "Invalid ANQP Query Response length");
747 		return;
748 	}
749 	if (pos + 4 > end)
750 		return;
751 
752 	if (WPA_GET_BE24(pos) != OUI_WFA) {
753 		p2p_dbg(p2p, "Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
754 		return;
755 	}
756 	pos += 3;
757 
758 	if (*pos != P2P_OUI_TYPE) {
759 		p2p_dbg(p2p, "Unsupported ANQP vendor type %u", *pos);
760 		return;
761 	}
762 	pos++;
763 
764 	if (pos + 2 > end)
765 		return;
766 	p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
767 	p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic);
768 	pos += 2;
769 
770 skip_nqp_header:
771 	if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
772 		return;
773 	wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
774 	p2p_dbg(p2p, "Current SD reassembly buffer length: %u",
775 		(unsigned int) wpabuf_len(p2p->sd_rx_resp));
776 
777 	if (more_frags) {
778 		p2p_dbg(p2p, "More fragments remains");
779 		/* TODO: what would be a good size limit? */
780 		if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
781 			wpabuf_free(p2p->sd_rx_resp);
782 			p2p->sd_rx_resp = NULL;
783 			p2p_dbg(p2p, "Too long SD response - drop it");
784 			return;
785 		}
786 		p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
787 		return;
788 	}
789 
790 	p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
791 	p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
792 	p2p->sd_peer = NULL;
793 
794 	if (p2p->sd_query) {
795 		if (!p2p->sd_query->for_all_peers) {
796 			struct p2p_sd_query *q;
797 			p2p_dbg(p2p, "Remove completed SD query %p",
798 				p2p->sd_query);
799 			q = p2p->sd_query;
800 			p2p_unlink_sd_query(p2p, p2p->sd_query);
801 			p2p_free_sd_query(q);
802 		}
803 		p2p->sd_query = NULL;
804 	}
805 
806 	if (p2p->cfg->sd_response)
807 		p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
808 				      p2p->sd_rx_update_indic,
809 				      wpabuf_head(p2p->sd_rx_resp),
810 				      wpabuf_len(p2p->sd_rx_resp));
811 	wpabuf_free(p2p->sd_rx_resp);
812 	p2p->sd_rx_resp = NULL;
813 
814 	p2p_continue_find(p2p);
815 }
816 
817 
818 void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
819 		      const struct wpabuf *tlvs)
820 {
821 	struct p2p_sd_query *q;
822 
823 	q = os_zalloc(sizeof(*q));
824 	if (q == NULL)
825 		return NULL;
826 
827 	if (dst)
828 		os_memcpy(q->peer, dst, ETH_ALEN);
829 	else
830 		q->for_all_peers = 1;
831 
832 	q->tlvs = wpabuf_dup(tlvs);
833 	if (q->tlvs == NULL) {
834 		p2p_free_sd_query(q);
835 		return NULL;
836 	}
837 
838 	q->next = p2p->sd_queries;
839 	p2p->sd_queries = q;
840 	p2p_dbg(p2p, "Added SD Query %p", q);
841 
842 	if (dst == NULL) {
843 		struct p2p_device *dev;
844 		dl_list_for_each(dev, &p2p->devices, struct p2p_device, list)
845 			dev->flags &= ~P2P_DEV_SD_INFO;
846 	}
847 
848 	return q;
849 }
850 
851 
852 #ifdef CONFIG_WIFI_DISPLAY
853 void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
854 			  const struct wpabuf *tlvs)
855 {
856 	struct p2p_sd_query *q;
857 	q = p2p_sd_request(p2p, dst, tlvs);
858 	if (q)
859 		q->wsd = 1;
860 	return q;
861 }
862 #endif /* CONFIG_WIFI_DISPLAY */
863 
864 
865 void p2p_sd_service_update(struct p2p_data *p2p)
866 {
867 	p2p->srv_update_indic++;
868 }
869 
870 
871 int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
872 {
873 	if (p2p_unlink_sd_query(p2p, req)) {
874 		p2p_dbg(p2p, "Cancel pending SD query %p", req);
875 		p2p_free_sd_query(req);
876 		return 0;
877 	}
878 	return -1;
879 }
880