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 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 79 static 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 102 static 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 111 void 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 124 static 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 148 static 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 166 static 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 197 static 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 228 int 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 271 void 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 372 void 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 421 void 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 #ifdef ANDROID_P2P 434 if (p2p->state != P2P_SD_DURING_FIND) { 435 p2p_dbg(p2p, "P2P: #### Not ignoring unexpected GAS Initial Response from " 436 MACSTR " state %d", MAC2STR(sa), p2p->state); 437 } 438 if (p2p->sd_peer == NULL || 439 #else 440 if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || 441 #endif 442 os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { 443 p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from " 444 MACSTR, MAC2STR(sa)); 445 return; 446 } 447 p2p->cfg->send_action_done(p2p->cfg->cb_ctx); 448 p2p_clear_timeout(p2p); 449 450 p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)", 451 MAC2STR(sa), (int) len); 452 453 if (len < 5 + 2) { 454 p2p_dbg(p2p, "Too short GAS Initial Response frame"); 455 return; 456 } 457 458 dialog_token = *pos++; 459 /* TODO: check dialog_token match */ 460 status_code = WPA_GET_LE16(pos); 461 pos += 2; 462 comeback_delay = WPA_GET_LE16(pos); 463 pos += 2; 464 p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u", 465 dialog_token, status_code, comeback_delay); 466 if (status_code) { 467 p2p_dbg(p2p, "Service Discovery failed: status code %u", 468 status_code); 469 return; 470 } 471 472 if (*pos != WLAN_EID_ADV_PROTO) { 473 p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos); 474 return; 475 } 476 pos++; 477 478 slen = *pos++; 479 next = pos + slen; 480 if (next > end || slen < 2) { 481 p2p_dbg(p2p, "Invalid IE in GAS Initial Response"); 482 return; 483 } 484 pos++; /* skip QueryRespLenLimit and PAME-BI */ 485 486 if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 487 p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u", 488 *pos); 489 return; 490 } 491 492 pos = next; 493 /* Query Response */ 494 if (pos + 2 > end) { 495 p2p_dbg(p2p, "Too short Query Response"); 496 return; 497 } 498 slen = WPA_GET_LE16(pos); 499 pos += 2; 500 p2p_dbg(p2p, "Query Response Length: %d", slen); 501 if (pos + slen > end) { 502 p2p_dbg(p2p, "Not enough Query Response data"); 503 return; 504 } 505 end = pos + slen; 506 507 if (comeback_delay) { 508 p2p_dbg(p2p, "Fragmented response - request fragments"); 509 if (p2p->sd_rx_resp) { 510 p2p_dbg(p2p, "Drop old SD reassembly buffer"); 511 wpabuf_free(p2p->sd_rx_resp); 512 p2p->sd_rx_resp = NULL; 513 } 514 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq); 515 return; 516 } 517 518 /* ANQP Query Response */ 519 if (pos + 4 > end) 520 return; 521 if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 522 p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 523 return; 524 } 525 pos += 2; 526 527 slen = WPA_GET_LE16(pos); 528 pos += 2; 529 if (pos + slen > end || slen < 3 + 1) { 530 p2p_dbg(p2p, "Invalid ANQP Query Response length"); 531 return; 532 } 533 534 if (WPA_GET_BE24(pos) != OUI_WFA) { 535 p2p_dbg(p2p, "Unsupported ANQP OUI %06x", WPA_GET_BE24(pos)); 536 return; 537 } 538 pos += 3; 539 540 if (*pos != P2P_OUI_TYPE) { 541 p2p_dbg(p2p, "Unsupported ANQP vendor type %u", *pos); 542 return; 543 } 544 pos++; 545 546 if (pos + 2 > end) 547 return; 548 update_indic = WPA_GET_LE16(pos); 549 p2p_dbg(p2p, "Service Update Indicator: %u", update_indic); 550 pos += 2; 551 552 p2p->sd_peer->flags |= P2P_DEV_SD_INFO; 553 p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE; 554 p2p->sd_peer = NULL; 555 556 if (p2p->sd_query) { 557 if (!p2p->sd_query->for_all_peers) { 558 struct p2p_sd_query *q; 559 p2p_dbg(p2p, "Remove completed SD query %p", 560 p2p->sd_query); 561 q = p2p->sd_query; 562 p2p_unlink_sd_query(p2p, p2p->sd_query); 563 p2p_free_sd_query(q); 564 } 565 p2p->sd_query = NULL; 566 } 567 568 if (p2p->cfg->sd_response) 569 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic, 570 pos, end - pos); 571 p2p_continue_find(p2p); 572 } 573 574 575 void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa, 576 const u8 *data, size_t len, int rx_freq) 577 { 578 struct wpabuf *resp; 579 u8 dialog_token; 580 size_t frag_len; 581 int more = 0; 582 583 wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len); 584 if (len < 1) 585 return; 586 dialog_token = *data; 587 p2p_dbg(p2p, "Dialog Token: %u", dialog_token); 588 if (dialog_token != p2p->sd_resp_dialog_token) { 589 p2p_dbg(p2p, "No pending SD response fragment for dialog token %u", 590 dialog_token); 591 return; 592 } 593 594 if (p2p->sd_resp == NULL) { 595 p2p_dbg(p2p, "No pending SD response fragment available"); 596 return; 597 } 598 if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) { 599 p2p_dbg(p2p, "No pending SD response fragment for " MACSTR, 600 MAC2STR(sa)); 601 return; 602 } 603 604 frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos; 605 if (frag_len > 1400) { 606 frag_len = 1400; 607 more = 1; 608 } 609 resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS, 610 p2p->srv_update_indic, 611 wpabuf_head_u8(p2p->sd_resp) + 612 p2p->sd_resp_pos, frag_len, 613 p2p->sd_frag_id, more, 614 wpabuf_len(p2p->sd_resp)); 615 if (resp == NULL) 616 return; 617 p2p_dbg(p2p, "Send GAS Comeback Response (frag_id %d more=%d frag_len=%d)", 618 p2p->sd_frag_id, more, (int) frag_len); 619 p2p->sd_frag_id++; 620 p2p->sd_resp_pos += frag_len; 621 622 if (more) { 623 p2p_dbg(p2p, "%d more bytes remain to be sent", 624 (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos)); 625 } else { 626 p2p_dbg(p2p, "All fragments of SD response sent"); 627 wpabuf_free(p2p->sd_resp); 628 p2p->sd_resp = NULL; 629 } 630 631 p2p->pending_action_state = P2P_NO_PENDING_ACTION; 632 if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr, 633 p2p->cfg->dev_addr, 634 wpabuf_head(resp), wpabuf_len(resp), 200) < 0) 635 p2p_dbg(p2p, "Failed to send Action frame"); 636 637 wpabuf_free(resp); 638 } 639 640 641 void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa, 642 const u8 *data, size_t len, int rx_freq) 643 { 644 const u8 *pos = data; 645 const u8 *end = data + len; 646 const u8 *next; 647 u8 dialog_token; 648 u16 status_code; 649 u8 frag_id; 650 u8 more_frags; 651 u16 comeback_delay; 652 u16 slen; 653 654 wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len); 655 656 #ifdef ANDROID_P2P 657 if (p2p->state != P2P_SD_DURING_FIND) { 658 p2p_dbg(p2p, "P2P: #### Not ignoring unexpected GAS Comeback Response from " 659 MACSTR " state %d", MAC2STR(sa), p2p->state); 660 } 661 if (p2p->sd_peer == NULL || 662 #else 663 if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || 664 #endif 665 os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { 666 p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from " 667 MACSTR, MAC2STR(sa)); 668 return; 669 } 670 p2p->cfg->send_action_done(p2p->cfg->cb_ctx); 671 p2p_clear_timeout(p2p); 672 673 p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)", 674 MAC2STR(sa), (int) len); 675 676 if (len < 6 + 2) { 677 p2p_dbg(p2p, "Too short GAS Comeback Response frame"); 678 return; 679 } 680 681 dialog_token = *pos++; 682 /* TODO: check dialog_token match */ 683 status_code = WPA_GET_LE16(pos); 684 pos += 2; 685 frag_id = *pos & 0x7f; 686 more_frags = (*pos & 0x80) >> 7; 687 pos++; 688 comeback_delay = WPA_GET_LE16(pos); 689 pos += 2; 690 p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d " 691 "comeback_delay=%u", 692 dialog_token, status_code, frag_id, more_frags, 693 comeback_delay); 694 /* TODO: check frag_id match */ 695 if (status_code) { 696 p2p_dbg(p2p, "Service Discovery failed: status code %u", 697 status_code); 698 return; 699 } 700 701 if (*pos != WLAN_EID_ADV_PROTO) { 702 p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u", 703 *pos); 704 return; 705 } 706 pos++; 707 708 slen = *pos++; 709 next = pos + slen; 710 if (next > end || slen < 2) { 711 p2p_dbg(p2p, "Invalid IE in GAS Comeback Response"); 712 return; 713 } 714 pos++; /* skip QueryRespLenLimit and PAME-BI */ 715 716 if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 717 p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u", 718 *pos); 719 return; 720 } 721 722 pos = next; 723 /* Query Response */ 724 if (pos + 2 > end) { 725 p2p_dbg(p2p, "Too short Query Response"); 726 return; 727 } 728 slen = WPA_GET_LE16(pos); 729 pos += 2; 730 p2p_dbg(p2p, "Query Response Length: %d", slen); 731 if (pos + slen > end) { 732 p2p_dbg(p2p, "Not enough Query Response data"); 733 return; 734 } 735 if (slen == 0) { 736 p2p_dbg(p2p, "No Query Response data"); 737 return; 738 } 739 end = pos + slen; 740 741 if (p2p->sd_rx_resp) { 742 /* 743 * ANQP header is only included in the first fragment; rest of 744 * the fragments start with continue TLVs. 745 */ 746 goto skip_nqp_header; 747 } 748 749 /* ANQP Query Response */ 750 if (pos + 4 > end) 751 return; 752 if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 753 p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 754 return; 755 } 756 pos += 2; 757 758 slen = WPA_GET_LE16(pos); 759 pos += 2; 760 p2p_dbg(p2p, "ANQP Query Response length: %u", slen); 761 if (slen < 3 + 1) { 762 p2p_dbg(p2p, "Invalid ANQP Query Response length"); 763 return; 764 } 765 if (pos + 4 > end) 766 return; 767 768 if (WPA_GET_BE24(pos) != OUI_WFA) { 769 p2p_dbg(p2p, "Unsupported ANQP OUI %06x", WPA_GET_BE24(pos)); 770 return; 771 } 772 pos += 3; 773 774 if (*pos != P2P_OUI_TYPE) { 775 p2p_dbg(p2p, "Unsupported ANQP vendor type %u", *pos); 776 return; 777 } 778 pos++; 779 780 if (pos + 2 > end) 781 return; 782 p2p->sd_rx_update_indic = WPA_GET_LE16(pos); 783 p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic); 784 pos += 2; 785 786 skip_nqp_header: 787 if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0) 788 return; 789 wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos); 790 p2p_dbg(p2p, "Current SD reassembly buffer length: %u", 791 (unsigned int) wpabuf_len(p2p->sd_rx_resp)); 792 793 if (more_frags) { 794 p2p_dbg(p2p, "More fragments remains"); 795 /* TODO: what would be a good size limit? */ 796 if (wpabuf_len(p2p->sd_rx_resp) > 64000) { 797 wpabuf_free(p2p->sd_rx_resp); 798 p2p->sd_rx_resp = NULL; 799 p2p_dbg(p2p, "Too long SD response - drop it"); 800 return; 801 } 802 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq); 803 return; 804 } 805 806 p2p->sd_peer->flags |= P2P_DEV_SD_INFO; 807 p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE; 808 p2p->sd_peer = NULL; 809 810 if (p2p->sd_query) { 811 if (!p2p->sd_query->for_all_peers) { 812 struct p2p_sd_query *q; 813 p2p_dbg(p2p, "Remove completed SD query %p", 814 p2p->sd_query); 815 q = p2p->sd_query; 816 p2p_unlink_sd_query(p2p, p2p->sd_query); 817 p2p_free_sd_query(q); 818 } 819 p2p->sd_query = NULL; 820 } 821 822 if (p2p->cfg->sd_response) 823 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, 824 p2p->sd_rx_update_indic, 825 wpabuf_head(p2p->sd_rx_resp), 826 wpabuf_len(p2p->sd_rx_resp)); 827 wpabuf_free(p2p->sd_rx_resp); 828 p2p->sd_rx_resp = NULL; 829 830 p2p_continue_find(p2p); 831 } 832 833 834 void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst, 835 const struct wpabuf *tlvs) 836 { 837 struct p2p_sd_query *q; 838 #ifdef ANDROID_P2P 839 /* Currently, supplicant doesn't support more than one pending broadcast SD request. 840 * So reject if application is registering another one before cancelling the existing one. 841 */ 842 for (q = p2p->sd_queries; q; q = q->next) { 843 if( (q->for_all_peers == 1) && (!dst)) { 844 wpa_printf(MSG_ERROR, "P2P: Already one pending" 845 " Broadcast request. Please cancel the current one" 846 " before adding a new one"); 847 return NULL; 848 } 849 } 850 #endif 851 852 q = os_zalloc(sizeof(*q)); 853 if (q == NULL) 854 return NULL; 855 856 if (dst) 857 os_memcpy(q->peer, dst, ETH_ALEN); 858 else 859 q->for_all_peers = 1; 860 861 q->tlvs = wpabuf_dup(tlvs); 862 if (q->tlvs == NULL) { 863 p2p_free_sd_query(q); 864 return NULL; 865 } 866 867 q->next = p2p->sd_queries; 868 p2p->sd_queries = q; 869 p2p_dbg(p2p, "Added SD Query %p", q); 870 871 if (dst == NULL) { 872 struct p2p_device *dev; 873 dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) 874 dev->flags &= ~P2P_DEV_SD_INFO; 875 } 876 877 return q; 878 } 879 880 881 #ifdef CONFIG_WIFI_DISPLAY 882 void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst, 883 const struct wpabuf *tlvs) 884 { 885 struct p2p_sd_query *q; 886 q = p2p_sd_request(p2p, dst, tlvs); 887 if (q) 888 q->wsd = 1; 889 return q; 890 } 891 #endif /* CONFIG_WIFI_DISPLAY */ 892 893 894 void p2p_sd_service_update(struct p2p_data *p2p) 895 { 896 p2p->srv_update_indic++; 897 } 898 899 900 int p2p_sd_cancel_request(struct p2p_data *p2p, void *req) 901 { 902 if (p2p_unlink_sd_query(p2p, req)) { 903 p2p_dbg(p2p, "Cancel pending SD query %p", req); 904 #ifdef ANDROID_P2P 905 p2p->sd_dev_list = NULL; 906 #endif 907 p2p_free_sd_query(req); 908 return 0; 909 } 910 return -1; 911 } 912
