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 if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
453 os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
454 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
455 "P2P: Ignore unexpected GAS Initial Response from "
456 MACSTR, MAC2STR(sa));
457 return;
458 }
459 p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
460 p2p_clear_timeout(p2p);
461
462 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
463 "P2P: Received GAS Initial Response from " MACSTR " (len=%d)",
464 MAC2STR(sa), (int) len);
465
466 if (len < 5 + 2) {
467 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
468 "P2P: Too short GAS Initial Response frame");
469 return;
470 }
471
472 dialog_token = *pos++;
473 /* TODO: check dialog_token match */
474 status_code = WPA_GET_LE16(pos);
475 pos += 2;
476 comeback_delay = WPA_GET_LE16(pos);
477 pos += 2;
478 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
479 "P2P: dialog_token=%u status_code=%u comeback_delay=%u",
480 dialog_token, status_code, comeback_delay);
481 if (status_code) {
482 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
483 "P2P: Service Discovery failed: status code %u",
484 status_code);
485 return;
486 }
487
488 if (*pos != WLAN_EID_ADV_PROTO) {
489 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
490 "P2P: Unexpected IE in GAS Initial Response: %u",
491 *pos);
492 return;
493 }
494 pos++;
495
496 slen = *pos++;
497 next = pos + slen;
498 if (next > end || slen < 2) {
499 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
500 "P2P: Invalid IE in GAS Initial Response");
501 return;
502 }
503 pos++; /* skip QueryRespLenLimit and PAME-BI */
504
505 if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
506 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
507 "P2P: Unsupported GAS advertisement protocol id %u",
508 *pos);
509 return;
510 }
511
512 pos = next;
513 /* Query Response */
514 if (pos + 2 > end) {
515 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
516 "Response");
517 return;
518 }
519 slen = WPA_GET_LE16(pos);
520 pos += 2;
521 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
522 slen);
523 if (pos + slen > end) {
524 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
525 "Response data");
526 return;
527 }
528 end = pos + slen;
529
530 if (comeback_delay) {
531 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Fragmented "
532 "response - request fragments");
533 if (p2p->sd_rx_resp) {
534 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop "
535 "old SD reassembly buffer");
536 wpabuf_free(p2p->sd_rx_resp);
537 p2p->sd_rx_resp = NULL;
538 }
539 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
540 return;
541 }
542
543 /* ANQP Query Response */
544 if (pos + 4 > end)
545 return;
546 if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
547 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
548 "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 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
557 "P2P: Invalid ANQP Query Response length");
558 return;
559 }
560
561 if (WPA_GET_BE24(pos) != OUI_WFA) {
562 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
563 "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
564 return;
565 }
566 pos += 3;
567
568 if (*pos != P2P_OUI_TYPE) {
569 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
570 "P2P: Unsupported ANQP vendor type %u", *pos);
571 return;
572 }
573 pos++;
574
575 if (pos + 2 > end)
576 return;
577 update_indic = WPA_GET_LE16(pos);
578 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
579 "P2P: Service Update Indicator: %u", update_indic);
580 pos += 2;
581
582 p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
583 p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
584 p2p->sd_peer = NULL;
585
586 if (p2p->sd_query) {
587 if (!p2p->sd_query->for_all_peers) {
588 struct p2p_sd_query *q;
589 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
590 "P2P: Remove completed SD query %p",
591 p2p->sd_query);
592 q = p2p->sd_query;
593 p2p_unlink_sd_query(p2p, p2p->sd_query);
594 p2p_free_sd_query(q);
595 }
596 p2p->sd_query = NULL;
597 }
598
599 if (p2p->cfg->sd_response)
600 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
601 pos, end - pos);
602 p2p_continue_find(p2p);
603 }
604
605
p2p_rx_gas_comeback_req(struct p2p_data * p2p,const u8 * sa,const u8 * data,size_t len,int rx_freq)606 void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
607 const u8 *data, size_t len, int rx_freq)
608 {
609 struct wpabuf *resp;
610 u8 dialog_token;
611 size_t frag_len;
612 int more = 0;
613
614 wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
615 if (len < 1)
616 return;
617 dialog_token = *data;
618 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Dialog Token: %u",
619 dialog_token);
620 if (dialog_token != p2p->sd_resp_dialog_token) {
621 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
622 "response fragment for dialog token %u", dialog_token);
623 return;
624 }
625
626 if (p2p->sd_resp == NULL) {
627 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
628 "response fragment available");
629 return;
630 }
631 if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
632 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
633 "response fragment for " MACSTR, MAC2STR(sa));
634 return;
635 }
636
637 frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
638 if (frag_len > 1400) {
639 frag_len = 1400;
640 more = 1;
641 }
642 resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
643 p2p->srv_update_indic,
644 wpabuf_head_u8(p2p->sd_resp) +
645 p2p->sd_resp_pos, frag_len,
646 p2p->sd_frag_id, more,
647 wpabuf_len(p2p->sd_resp));
648 if (resp == NULL)
649 return;
650 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Send GAS Comeback "
651 "Response (frag_id %d more=%d frag_len=%d)",
652 p2p->sd_frag_id, more, (int) frag_len);
653 p2p->sd_frag_id++;
654 p2p->sd_resp_pos += frag_len;
655
656 if (more) {
657 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: %d more bytes "
658 "remain to be sent",
659 (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
660 } else {
661 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: All fragments of "
662 "SD response sent");
663 wpabuf_free(p2p->sd_resp);
664 p2p->sd_resp = NULL;
665 }
666
667 p2p->pending_action_state = P2P_NO_PENDING_ACTION;
668 if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr,
669 p2p->cfg->dev_addr,
670 wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
671 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
672 "P2P: Failed to send Action frame");
673
674 wpabuf_free(resp);
675 }
676
677
p2p_rx_gas_comeback_resp(struct p2p_data * p2p,const u8 * sa,const u8 * data,size_t len,int rx_freq)678 void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
679 const u8 *data, size_t len, int rx_freq)
680 {
681 const u8 *pos = data;
682 const u8 *end = data + len;
683 const u8 *next;
684 u8 dialog_token;
685 u16 status_code;
686 u8 frag_id;
687 u8 more_frags;
688 u16 comeback_delay;
689 u16 slen;
690
691 wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
692
693 if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
694 os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
695 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
696 "P2P: Ignore unexpected GAS Comeback Response from "
697 MACSTR, MAC2STR(sa));
698 return;
699 }
700 p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
701 p2p_clear_timeout(p2p);
702
703 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
704 "P2P: Received GAS Comeback Response from " MACSTR " (len=%d)",
705 MAC2STR(sa), (int) len);
706
707 if (len < 6 + 2) {
708 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
709 "P2P: Too short GAS Comeback Response frame");
710 return;
711 }
712
713 dialog_token = *pos++;
714 /* TODO: check dialog_token match */
715 status_code = WPA_GET_LE16(pos);
716 pos += 2;
717 frag_id = *pos & 0x7f;
718 more_frags = (*pos & 0x80) >> 7;
719 pos++;
720 comeback_delay = WPA_GET_LE16(pos);
721 pos += 2;
722 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
723 "P2P: dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
724 "comeback_delay=%u",
725 dialog_token, status_code, frag_id, more_frags,
726 comeback_delay);
727 /* TODO: check frag_id match */
728 if (status_code) {
729 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
730 "P2P: Service Discovery failed: status code %u",
731 status_code);
732 return;
733 }
734
735 if (*pos != WLAN_EID_ADV_PROTO) {
736 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
737 "P2P: Unexpected IE in GAS Comeback Response: %u",
738 *pos);
739 return;
740 }
741 pos++;
742
743 slen = *pos++;
744 next = pos + slen;
745 if (next > end || slen < 2) {
746 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
747 "P2P: Invalid IE in GAS Comeback Response");
748 return;
749 }
750 pos++; /* skip QueryRespLenLimit and PAME-BI */
751
752 if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
753 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
754 "P2P: Unsupported GAS advertisement protocol id %u",
755 *pos);
756 return;
757 }
758
759 pos = next;
760 /* Query Response */
761 if (pos + 2 > end) {
762 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
763 "Response");
764 return;
765 }
766 slen = WPA_GET_LE16(pos);
767 pos += 2;
768 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
769 slen);
770 if (pos + slen > end) {
771 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
772 "Response data");
773 return;
774 }
775 if (slen == 0) {
776 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No Query Response "
777 "data");
778 return;
779 }
780 end = pos + slen;
781
782 if (p2p->sd_rx_resp) {
783 /*
784 * ANQP header is only included in the first fragment; rest of
785 * the fragments start with continue TLVs.
786 */
787 goto skip_nqp_header;
788 }
789
790 /* ANQP Query Response */
791 if (pos + 4 > end)
792 return;
793 if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
794 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
795 "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
796 return;
797 }
798 pos += 2;
799
800 slen = WPA_GET_LE16(pos);
801 pos += 2;
802 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: ANQP Query Response "
803 "length: %u", slen);
804 if (slen < 3 + 1) {
805 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
806 "P2P: Invalid ANQP Query Response length");
807 return;
808 }
809 if (pos + 4 > end)
810 return;
811
812 if (WPA_GET_BE24(pos) != OUI_WFA) {
813 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
814 "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
815 return;
816 }
817 pos += 3;
818
819 if (*pos != P2P_OUI_TYPE) {
820 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
821 "P2P: Unsupported ANQP vendor type %u", *pos);
822 return;
823 }
824 pos++;
825
826 if (pos + 2 > end)
827 return;
828 p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
829 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
830 "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 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Current SD reassembly "
838 "buffer length: %u",
839 (unsigned int) wpabuf_len(p2p->sd_rx_resp));
840
841 if (more_frags) {
842 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: More fragments "
843 "remains");
844 /* TODO: what would be a good size limit? */
845 if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
846 wpabuf_free(p2p->sd_rx_resp);
847 p2p->sd_rx_resp = NULL;
848 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too long "
849 "SD response - drop it");
850 return;
851 }
852 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
853 return;
854 }
855
856 p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
857 p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
858 p2p->sd_peer = NULL;
859
860 if (p2p->sd_query) {
861 if (!p2p->sd_query->for_all_peers) {
862 struct p2p_sd_query *q;
863 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
864 "P2P: Remove completed SD query %p",
865 p2p->sd_query);
866 q = p2p->sd_query;
867 p2p_unlink_sd_query(p2p, p2p->sd_query);
868 p2p_free_sd_query(q);
869 }
870 p2p->sd_query = NULL;
871 }
872
873 if (p2p->cfg->sd_response)
874 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
875 p2p->sd_rx_update_indic,
876 wpabuf_head(p2p->sd_rx_resp),
877 wpabuf_len(p2p->sd_rx_resp));
878 wpabuf_free(p2p->sd_rx_resp);
879 p2p->sd_rx_resp = NULL;
880
881 p2p_continue_find(p2p);
882 }
883
884
p2p_sd_request(struct p2p_data * p2p,const u8 * dst,const struct wpabuf * tlvs)885 void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
886 const struct wpabuf *tlvs)
887 {
888 struct p2p_sd_query *q;
889
890 q = os_zalloc(sizeof(*q));
891 if (q == NULL)
892 return NULL;
893
894 if (dst)
895 os_memcpy(q->peer, dst, ETH_ALEN);
896 else
897 q->for_all_peers = 1;
898
899 q->tlvs = wpabuf_dup(tlvs);
900 if (q->tlvs == NULL) {
901 p2p_free_sd_query(q);
902 return NULL;
903 }
904
905 q->next = p2p->sd_queries;
906 p2p->sd_queries = q;
907 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p", q);
908
909 if (dst == NULL) {
910 struct p2p_device *dev;
911 dl_list_for_each(dev, &p2p->devices, struct p2p_device, list)
912 dev->flags &= ~P2P_DEV_SD_INFO;
913 }
914
915 return q;
916 }
917
918
919 #ifdef CONFIG_WIFI_DISPLAY
p2p_sd_request_wfd(struct p2p_data * p2p,const u8 * dst,const struct wpabuf * tlvs)920 void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
921 const struct wpabuf *tlvs)
922 {
923 struct p2p_sd_query *q;
924 q = p2p_sd_request(p2p, dst, tlvs);
925 if (q)
926 q->wsd = 1;
927 return q;
928 }
929 #endif /* CONFIG_WIFI_DISPLAY */
930
931
p2p_sd_service_update(struct p2p_data * p2p)932 void p2p_sd_service_update(struct p2p_data *p2p)
933 {
934 p2p->srv_update_indic++;
935 }
936
937
p2p_sd_cancel_request(struct p2p_data * p2p,void * req)938 int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
939 {
940 if (p2p_unlink_sd_query(p2p, req)) {
941 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
942 "P2P: Cancel pending SD query %p", req);
943 p2p_free_sd_query(req);
944 return 0;
945 }
946 return -1;
947 }
948