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
430	/*
431	 * In the 60 GHz, we have a smaller maximum frame length for management
432	 * frames.
433	 */
434	max_len = (freq > 56160) ? 928 : 1400;
435
436	/* TODO: fix the length limit to match with the maximum frame length */
437	if (wpabuf_len(resp_tlvs) > max_len) {
438		p2p_dbg(p2p, "SD response long enough to require fragmentation");
439		if (p2p->sd_resp) {
440			/*
441			 * TODO: Could consider storing the fragmented response
442			 * separately for each peer to avoid having to drop old
443			 * one if there is more than one pending SD query.
444			 * Though, that would eat more memory, so there are
445			 * also benefits to just using a single buffer.
446			 */
447			p2p_dbg(p2p, "Drop previous SD response");
448			wpabuf_free(p2p->sd_resp);
449		}
450		p2p->sd_resp = wpabuf_dup(resp_tlvs);
451		if (p2p->sd_resp == NULL) {
452			p2p_err(p2p, "Failed to allocate SD response fragmentation area");
453			return;
454		}
455		os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
456		p2p->sd_resp_dialog_token = dialog_token;
457		p2p->sd_resp_pos = 0;
458		p2p->sd_frag_id = 0;
459		resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
460					     1, p2p->srv_update_indic, NULL);
461	} else {
462		p2p_dbg(p2p, "SD response fits in initial response");
463		resp = p2p_build_sd_response(dialog_token,
464					     WLAN_STATUS_SUCCESS, 0,
465					     p2p->srv_update_indic, resp_tlvs);
466	}
467	if (resp == NULL)
468		return;
469
470	p2p->pending_action_state = P2P_NO_PENDING_ACTION;
471	if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr,
472			    p2p->cfg->dev_addr,
473			    wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
474		p2p_dbg(p2p, "Failed to send Action frame");
475
476	wpabuf_free(resp);
477}
478
479
480void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
481			     const u8 *data, size_t len, int rx_freq)
482{
483	const u8 *pos = data;
484	const u8 *end = data + len;
485	const u8 *next;
486	u8 dialog_token;
487	u16 status_code;
488	u16 comeback_delay;
489	u16 slen;
490	u16 update_indic;
491
492	if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
493	    os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
494		p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from "
495			MACSTR, MAC2STR(sa));
496		return;
497	}
498	p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
499	p2p_clear_timeout(p2p);
500
501	p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)",
502		MAC2STR(sa), (int) len);
503
504	if (len < 5 + 2) {
505		p2p_dbg(p2p, "Too short GAS Initial Response frame");
506		return;
507	}
508
509	dialog_token = *pos++;
510	/* TODO: check dialog_token match */
511	status_code = WPA_GET_LE16(pos);
512	pos += 2;
513	comeback_delay = WPA_GET_LE16(pos);
514	pos += 2;
515	p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u",
516		dialog_token, status_code, comeback_delay);
517	if (status_code) {
518		p2p_dbg(p2p, "Service Discovery failed: status code %u",
519			status_code);
520		return;
521	}
522
523	if (*pos != WLAN_EID_ADV_PROTO) {
524		p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos);
525		return;
526	}
527	pos++;
528
529	slen = *pos++;
530	if (slen > end - pos || slen < 2) {
531		p2p_dbg(p2p, "Invalid IE in GAS Initial Response");
532		return;
533	}
534	next = pos + slen;
535	pos++; /* skip QueryRespLenLimit and PAME-BI */
536
537	if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
538		p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
539			*pos);
540		return;
541	}
542
543	pos = next;
544	/* Query Response */
545	if (end - pos < 2) {
546		p2p_dbg(p2p, "Too short Query Response");
547		return;
548	}
549	slen = WPA_GET_LE16(pos);
550	pos += 2;
551	p2p_dbg(p2p, "Query Response Length: %d", slen);
552	if (slen > end - pos) {
553		p2p_dbg(p2p, "Not enough Query Response data");
554		return;
555	}
556	end = pos + slen;
557
558	if (comeback_delay) {
559		p2p_dbg(p2p, "Fragmented response - request fragments");
560		if (p2p->sd_rx_resp) {
561			p2p_dbg(p2p, "Drop old SD reassembly buffer");
562			wpabuf_free(p2p->sd_rx_resp);
563			p2p->sd_rx_resp = NULL;
564		}
565		p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
566		return;
567	}
568
569	/* ANQP Query Response */
570	if (end - pos < 4)
571		return;
572	if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
573		p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
574		return;
575	}
576	pos += 2;
577
578	slen = WPA_GET_LE16(pos);
579	pos += 2;
580	if (slen > end - pos || slen < 3 + 1) {
581		p2p_dbg(p2p, "Invalid ANQP Query Response length");
582		return;
583	}
584
585	if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
586		p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
587			WPA_GET_BE32(pos));
588		return;
589	}
590	pos += 4;
591
592	if (end - pos < 2)
593		return;
594	update_indic = WPA_GET_LE16(pos);
595	p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
596	pos += 2;
597
598	p2p->sd_peer = NULL;
599
600	if (p2p->sd_query) {
601		if (!p2p->sd_query->for_all_peers) {
602			struct p2p_sd_query *q;
603			p2p_dbg(p2p, "Remove completed SD query %p",
604				p2p->sd_query);
605			q = p2p->sd_query;
606			p2p_unlink_sd_query(p2p, p2p->sd_query);
607			p2p_free_sd_query(q);
608		}
609		p2p->sd_query = NULL;
610	}
611
612	if (p2p->cfg->sd_response)
613		p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
614				      pos, end - pos);
615	p2p_continue_find(p2p);
616}
617
618
619void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
620			     const u8 *data, size_t len, int rx_freq)
621{
622	struct wpabuf *resp;
623	u8 dialog_token;
624	size_t frag_len, max_len;
625	int more = 0;
626
627	wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
628	if (len < 1)
629		return;
630	dialog_token = *data;
631	p2p_dbg(p2p, "Dialog Token: %u", dialog_token);
632	if (dialog_token != p2p->sd_resp_dialog_token) {
633		p2p_dbg(p2p, "No pending SD response fragment for dialog token %u",
634			dialog_token);
635		return;
636	}
637
638	if (p2p->sd_resp == NULL) {
639		p2p_dbg(p2p, "No pending SD response fragment available");
640		return;
641	}
642	if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
643		p2p_dbg(p2p, "No pending SD response fragment for " MACSTR,
644			MAC2STR(sa));
645		return;
646	}
647
648	/*
649	 * In the 60 GHz, we have a smaller maximum frame length for management
650	 * frames.
651	 */
652	max_len = (rx_freq > 56160) ? 928 : 1400;
653	frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
654	if (frag_len > max_len) {
655		frag_len = max_len;
656		more = 1;
657	}
658	resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
659					   p2p->srv_update_indic,
660					   wpabuf_head_u8(p2p->sd_resp) +
661					   p2p->sd_resp_pos, frag_len,
662					   p2p->sd_frag_id, more,
663					   wpabuf_len(p2p->sd_resp));
664	if (resp == NULL)
665		return;
666	p2p_dbg(p2p, "Send GAS Comeback Response (frag_id %d more=%d frag_len=%d)",
667		p2p->sd_frag_id, more, (int) frag_len);
668	p2p->sd_frag_id++;
669	p2p->sd_resp_pos += frag_len;
670
671	if (more) {
672		p2p_dbg(p2p, "%d more bytes remain to be sent",
673			(int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
674	} else {
675		p2p_dbg(p2p, "All fragments of SD response sent");
676		wpabuf_free(p2p->sd_resp);
677		p2p->sd_resp = NULL;
678	}
679
680	p2p->pending_action_state = P2P_NO_PENDING_ACTION;
681	if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr,
682			    p2p->cfg->dev_addr,
683			    wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
684		p2p_dbg(p2p, "Failed to send Action frame");
685
686	wpabuf_free(resp);
687}
688
689
690void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
691			      const u8 *data, size_t len, int rx_freq)
692{
693	const u8 *pos = data;
694	const u8 *end = data + len;
695	const u8 *next;
696	u8 dialog_token;
697	u16 status_code;
698	u8 frag_id;
699	u8 more_frags;
700	u16 comeback_delay;
701	u16 slen;
702
703	wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
704
705	if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
706	    os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
707		p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from "
708			MACSTR, MAC2STR(sa));
709		return;
710	}
711	p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
712	p2p_clear_timeout(p2p);
713
714	p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)",
715		MAC2STR(sa), (int) len);
716
717	if (len < 6 + 2) {
718		p2p_dbg(p2p, "Too short GAS Comeback Response frame");
719		return;
720	}
721
722	dialog_token = *pos++;
723	/* TODO: check dialog_token match */
724	status_code = WPA_GET_LE16(pos);
725	pos += 2;
726	frag_id = *pos & 0x7f;
727	more_frags = (*pos & 0x80) >> 7;
728	pos++;
729	comeback_delay = WPA_GET_LE16(pos);
730	pos += 2;
731	p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
732		"comeback_delay=%u",
733		dialog_token, status_code, frag_id, more_frags,
734		comeback_delay);
735	/* TODO: check frag_id match */
736	if (status_code) {
737		p2p_dbg(p2p, "Service Discovery failed: status code %u",
738			status_code);
739		return;
740	}
741
742	if (*pos != WLAN_EID_ADV_PROTO) {
743		p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u",
744			*pos);
745		return;
746	}
747	pos++;
748
749	slen = *pos++;
750	if (slen > end - pos || slen < 2) {
751		p2p_dbg(p2p, "Invalid IE in GAS Comeback Response");
752		return;
753	}
754	next = pos + slen;
755	pos++; /* skip QueryRespLenLimit and PAME-BI */
756
757	if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
758		p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
759			*pos);
760		return;
761	}
762
763	pos = next;
764	/* Query Response */
765	if (end - pos < 2) {
766		p2p_dbg(p2p, "Too short Query Response");
767		return;
768	}
769	slen = WPA_GET_LE16(pos);
770	pos += 2;
771	p2p_dbg(p2p, "Query Response Length: %d", slen);
772	if (slen > end - pos) {
773		p2p_dbg(p2p, "Not enough Query Response data");
774		return;
775	}
776	if (slen == 0) {
777		p2p_dbg(p2p, "No Query Response data");
778		return;
779	}
780	end = pos + slen;
781
782	if (p2p->sd_rx_resp) {
783		 /*
784		  * ANQP header is only included in the first fragment; rest of
785		  * the fragments start with continue TLVs.
786		  */
787		goto skip_nqp_header;
788	}
789
790	/* ANQP Query Response */
791	if (end - pos < 4)
792		return;
793	if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
794		p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
795		return;
796	}
797	pos += 2;
798
799	slen = WPA_GET_LE16(pos);
800	pos += 2;
801	p2p_dbg(p2p, "ANQP Query Response length: %u", slen);
802	if (slen < 3 + 1) {
803		p2p_dbg(p2p, "Invalid ANQP Query Response length");
804		return;
805	}
806	if (end - pos < 4)
807		return;
808
809	if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
810		p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
811			WPA_GET_BE32(pos));
812		return;
813	}
814	pos += 4;
815
816	if (end - pos < 2)
817		return;
818	p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
819	p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic);
820	pos += 2;
821
822skip_nqp_header:
823	if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
824		return;
825	wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
826	p2p_dbg(p2p, "Current SD reassembly buffer length: %u",
827		(unsigned int) wpabuf_len(p2p->sd_rx_resp));
828
829	if (more_frags) {
830		p2p_dbg(p2p, "More fragments remains");
831		/* TODO: what would be a good size limit? */
832		if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
833			wpabuf_free(p2p->sd_rx_resp);
834			p2p->sd_rx_resp = NULL;
835			p2p_dbg(p2p, "Too long SD response - drop it");
836			return;
837		}
838		p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
839		return;
840	}
841
842	p2p->sd_peer = NULL;
843
844	if (p2p->sd_query) {
845		if (!p2p->sd_query->for_all_peers) {
846			struct p2p_sd_query *q;
847			p2p_dbg(p2p, "Remove completed SD query %p",
848				p2p->sd_query);
849			q = p2p->sd_query;
850			p2p_unlink_sd_query(p2p, p2p->sd_query);
851			p2p_free_sd_query(q);
852		}
853		p2p->sd_query = NULL;
854	}
855
856	if (p2p->cfg->sd_response)
857		p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
858				      p2p->sd_rx_update_indic,
859				      wpabuf_head(p2p->sd_rx_resp),
860				      wpabuf_len(p2p->sd_rx_resp));
861	wpabuf_free(p2p->sd_rx_resp);
862	p2p->sd_rx_resp = NULL;
863
864	p2p_continue_find(p2p);
865}
866
867
868void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
869		      const struct wpabuf *tlvs)
870{
871	struct p2p_sd_query *q;
872
873	q = os_zalloc(sizeof(*q));
874	if (q == NULL)
875		return NULL;
876
877	if (dst)
878		os_memcpy(q->peer, dst, ETH_ALEN);
879	else
880		q->for_all_peers = 1;
881
882	q->tlvs = wpabuf_dup(tlvs);
883	if (q->tlvs == NULL) {
884		p2p_free_sd_query(q);
885		return NULL;
886	}
887
888	q->next = p2p->sd_queries;
889	p2p->sd_queries = q;
890	p2p_dbg(p2p, "Added SD Query %p", q);
891
892	if (dst == NULL) {
893		struct p2p_device *dev;
894
895		p2p->num_p2p_sd_queries++;
896
897		/* Update all the devices for the newly added broadcast query */
898		dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) {
899			if (dev->sd_pending_bcast_queries <= 0)
900				dev->sd_pending_bcast_queries = 1;
901			else
902				dev->sd_pending_bcast_queries++;
903		}
904	}
905
906	return q;
907}
908
909
910#ifdef CONFIG_WIFI_DISPLAY
911void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
912			  const struct wpabuf *tlvs)
913{
914	struct p2p_sd_query *q;
915	q = p2p_sd_request(p2p, dst, tlvs);
916	if (q)
917		q->wsd = 1;
918	return q;
919}
920#endif /* CONFIG_WIFI_DISPLAY */
921
922
923void p2p_sd_service_update(struct p2p_data *p2p)
924{
925	p2p->srv_update_indic++;
926}
927
928
929int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
930{
931	if (p2p_unlink_sd_query(p2p, req)) {
932		p2p_dbg(p2p, "Cancel pending SD query %p", req);
933		p2p_free_sd_query(req);
934		return 0;
935	}
936	return -1;
937}
938