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