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