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