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