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 static 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 50 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 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 goto found; 79 count++; 80 } 81 if (!q->for_all_peers && 82 os_memcmp(q->peer, dev->info.p2p_device_addr, ETH_ALEN) == 83 0) 84 goto found; 85 } 86 87 return NULL; 88 89 found: 90 if (dev->sd_reqs > 100) { 91 p2p_dbg(p2p, "Too many SD request attempts to " MACSTR 92 " - skip remaining queries", 93 MAC2STR(dev->info.p2p_device_addr)); 94 return NULL; 95 } 96 return q; 97 } 98 99 100 static void p2p_decrease_sd_bc_queries(struct p2p_data *p2p, int query_number) 101 { 102 struct p2p_device *dev; 103 104 p2p->num_p2p_sd_queries--; 105 dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) { 106 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 dev->sd_pending_bcast_queries--; 112 } 113 } 114 } 115 116 117 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 int query_number = 0; 122 123 q = p2p->sd_queries; 124 prev = NULL; 125 while (q) { 126 if (q == query) { 127 /* If the query is a broadcast query, decrease one from 128 * all the devices */ 129 if (query->for_all_peers) 130 p2p_decrease_sd_bc_queries(p2p, query_number); 131 if (prev) 132 prev->next = q->next; 133 else 134 p2p->sd_queries = q->next; 135 if (p2p->sd_query == query) 136 p2p->sd_query = NULL; 137 return 1; 138 } 139 if (q->for_all_peers) 140 query_number++; 141 prev = q; 142 q = q->next; 143 } 144 return 0; 145 } 146 147 148 static void p2p_free_sd_query(struct p2p_sd_query *q) 149 { 150 if (q == NULL) 151 return; 152 wpabuf_free(q->tlvs); 153 os_free(q); 154 } 155 156 157 void p2p_free_sd_queries(struct p2p_data *p2p) 158 { 159 struct p2p_sd_query *q, *prev; 160 q = p2p->sd_queries; 161 p2p->sd_queries = NULL; 162 while (q) { 163 prev = q; 164 q = q->next; 165 p2p_free_sd_query(prev); 166 } 167 p2p->num_p2p_sd_queries = 0; 168 } 169 170 171 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 buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs)); 178 if (buf == NULL) 179 return NULL; 180 181 /* ANQP Query Request Frame */ 182 len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); 183 wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); 184 wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */ 185 wpabuf_put_buf(buf, tlvs); 186 gas_anqp_set_element_len(buf, len_pos); 187 188 gas_anqp_set_len(buf); 189 190 return buf; 191 } 192 193 194 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 req = gas_build_comeback_req(dialog_token); 200 if (req == NULL) 201 return; 202 203 p2p->pending_action_state = P2P_NO_PENDING_ACTION; 204 if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst, 205 wpabuf_head(req), wpabuf_len(req), 200) < 0) 206 p2p_dbg(p2p, "Failed to send Action frame"); 207 208 wpabuf_free(req); 209 } 210 211 212 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 buf = gas_anqp_build_initial_resp(dialog_token, status_code, 221 comeback_delay, 222 100 + (tlvs ? wpabuf_len(tlvs) : 0)); 223 if (buf == NULL) 224 return NULL; 225 226 if (tlvs) { 227 /* ANQP Query Response Frame */ 228 len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); 229 wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); 230 /* Service Update Indicator */ 231 wpabuf_put_le16(buf, update_indic); 232 wpabuf_put_buf(buf, tlvs); 233 gas_anqp_set_element_len(buf, len_pos); 234 } 235 236 gas_anqp_set_len(buf); 237 238 return buf; 239 } 240 241 242 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 buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id, 252 more, 0, 100 + len); 253 if (buf == NULL) 254 return NULL; 255 256 if (frag_id == 0) { 257 /* ANQP Query Response Frame */ 258 wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */ 259 wpabuf_put_le16(buf, 3 + 1 + 2 + total_len); 260 wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); 261 /* Service Update Indicator */ 262 wpabuf_put_le16(buf, update_indic); 263 } 264 265 wpabuf_put_data(buf, data, len); 266 gas_anqp_set_len(buf); 267 268 return buf; 269 } 270 271 272 int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev) 273 { 274 struct wpabuf *req; 275 int ret = 0; 276 struct p2p_sd_query *query; 277 int freq; 278 unsigned int wait_time; 279 280 freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq; 281 if (freq <= 0) { 282 p2p_dbg(p2p, "No Listen/Operating frequency known for the peer " 283 MACSTR " to send SD Request", 284 MAC2STR(dev->info.p2p_device_addr)); 285 return -1; 286 } 287 288 query = p2p_pending_sd_req(p2p, dev); 289 if (query == NULL) 290 return -1; 291 292 p2p_dbg(p2p, "Start Service Discovery with " MACSTR, 293 MAC2STR(dev->info.p2p_device_addr)); 294 295 req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs); 296 if (req == NULL) 297 return -1; 298 299 dev->sd_reqs++; 300 p2p->sd_peer = dev; 301 p2p->sd_query = query; 302 p2p->pending_action_state = P2P_PENDING_SD; 303 304 wait_time = 5000; 305 if (p2p->cfg->max_listen && wait_time > p2p->cfg->max_listen) 306 wait_time = p2p->cfg->max_listen; 307 if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr, 308 p2p->cfg->dev_addr, dev->info.p2p_device_addr, 309 wpabuf_head(req), wpabuf_len(req), wait_time) < 0) { 310 p2p_dbg(p2p, "Failed to send Action frame"); 311 ret = -1; 312 } 313 314 wpabuf_free(req); 315 316 return ret; 317 } 318 319 320 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 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 416 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 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 465 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 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 604 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 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 670 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 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 802 skip_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 848 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 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 891 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 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 903 void p2p_sd_service_update(struct p2p_data *p2p) 904 { 905 p2p->srv_update_indic++; 906 } 907 908 909 int 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