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