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