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