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