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