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