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