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