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