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