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