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