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
92 " - skip remaining queries",
93 MAC2STR(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 " 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
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
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
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, 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
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,
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
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, 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
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