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