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