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