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