• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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
wfd_wsd_supported(struct wpabuf * wfd)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 (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 
p2p_pending_sd_req(struct p2p_data * p2p,struct p2p_device * dev)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 	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 
89 found:
90 	if (dev->sd_reqs > 100) {
91 		p2p_dbg(p2p, "Too many SD request attempts to " MACSTR_SEC
92 			" - skip remaining queries",
93 			MAC2STR_SEC(dev->info.p2p_device_addr));
94 		return NULL;
95 	}
96 	return q;
97 }
98 
99 
p2p_decrease_sd_bc_queries(struct p2p_data * p2p,int query_number)100 static 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 
p2p_unlink_sd_query(struct p2p_data * p2p,struct p2p_sd_query * query)117 static 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 
p2p_free_sd_query(struct p2p_sd_query * q)148 static 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 
p2p_free_sd_queries(struct p2p_data * p2p)157 void 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 
p2p_build_sd_query(u16 update_indic,struct wpabuf * tlvs)171 static 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 
p2p_send_gas_comeback_req(struct p2p_data * p2p,const u8 * dst,u8 dialog_token,int freq)194 static 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 
p2p_build_sd_response(u8 dialog_token,u16 status_code,u16 comeback_delay,u16 update_indic,const struct wpabuf * tlvs)212 static 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 
p2p_build_gas_comeback_resp(u8 dialog_token,u16 status_code,u16 update_indic,const u8 * data,size_t len,u8 frag_id,u8 more,u16 total_len)242 static 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 
p2p_start_sd(struct p2p_data * p2p,struct p2p_device * dev)272 int 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_SEC " to send SD Request",
284 			MAC2STR_SEC(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_SEC
295 			" due to it being the first no-ACK peer in this search iteration",
296 			MAC2STR_SEC(dev->info.p2p_device_addr));
297 		return -2;
298 	}
299 
300 	p2p_dbg(p2p, "Start Service Discovery with " MACSTR_SEC,
301 		MAC2STR_SEC(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 
p2p_rx_gas_initial_req(struct p2p_data * p2p,const u8 * sa,const u8 * data,size_t len,int rx_freq)328 void 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_SEC
356 		" (dialog token %u, freq %d)",
357 		MAC2STR_SEC(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 
p2p_sd_response(struct p2p_data * p2p,int freq,const u8 * dst,u8 dialog_token,const struct wpabuf * resp_tlvs)424 void 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 
p2p_rx_gas_initial_resp(struct p2p_data * p2p,const u8 * sa,const u8 * data,size_t len,int rx_freq)482 void 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_SEC, MAC2STR_SEC(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_SEC " (len=%d)",
504 		MAC2STR_SEC(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 
p2p_rx_gas_comeback_req(struct p2p_data * p2p,const u8 * sa,const u8 * data,size_t len,int rx_freq)621 void 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_SEC,
647 			MAC2STR_SEC(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 
p2p_rx_gas_comeback_resp(struct p2p_data * p2p,const u8 * sa,const u8 * data,size_t len,int rx_freq)694 void 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_SEC, MAC2STR_SEC(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_SEC " (len=%d)",
719 		MAC2STR_SEC(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 
826 skip_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 
p2p_sd_request(struct p2p_data * p2p,const u8 * dst,const struct wpabuf * tlvs)872 void * 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
p2p_sd_request_wfd(struct p2p_data * p2p,const u8 * dst,const struct wpabuf * tlvs)915 void * 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 
p2p_sd_service_update(struct p2p_data * p2p)927 void p2p_sd_service_update(struct p2p_data *p2p)
928 {
929 	p2p->srv_update_indic++;
930 }
931 
932 
p2p_sd_cancel_request(struct p2p_data * p2p,void * req)933 int 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