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