1 /* 2 * wpa_supplicant - P2P service discovery 3 * Copyright (c) 2009-2010, Atheros Communications 4 * Copyright (c) 2010-2014, Jouni Malinen <j (at) w1.fi> 5 * 6 * This software may be distributed under the terms of the BSD license. 7 * See README for more details. 8 */ 9 10 #include "utils/includes.h" 11 12 #include "utils/common.h" 13 #include "p2p/p2p.h" 14 #include "wpa_supplicant_i.h" 15 #include "notify.h" 16 #include "p2p_supplicant.h" 17 18 19 /* 20 * DNS Header section is used only to calculate compression pointers, so the 21 * contents of this data does not matter, but the length needs to be reserved 22 * in the virtual packet. 23 */ 24 #define DNS_HEADER_LEN 12 25 26 /* 27 * 27-octet in-memory packet from P2P specification containing two implied 28 * queries for _tcp.lcoal. PTR IN and _udp.local. PTR IN 29 */ 30 #define P2P_SD_IN_MEMORY_LEN 27 31 32 static int p2p_sd_dns_uncompress_label(char **upos, char *uend, u8 *start, 33 u8 **spos, const u8 *end) 34 { 35 while (*spos < end) { 36 u8 val = ((*spos)[0] & 0xc0) >> 6; 37 int len; 38 39 if (val == 1 || val == 2) { 40 /* These are reserved values in RFC 1035 */ 41 wpa_printf(MSG_DEBUG, "P2P: Invalid domain name " 42 "sequence starting with 0x%x", val); 43 return -1; 44 } 45 46 if (val == 3) { 47 u16 offset; 48 u8 *spos_tmp; 49 50 /* Offset */ 51 if (end - *spos < 2) { 52 wpa_printf(MSG_DEBUG, "P2P: No room for full " 53 "DNS offset field"); 54 return -1; 55 } 56 57 offset = (((*spos)[0] & 0x3f) << 8) | (*spos)[1]; 58 if (offset >= *spos - start) { 59 wpa_printf(MSG_DEBUG, "P2P: Invalid DNS " 60 "pointer offset %u", offset); 61 return -1; 62 } 63 64 (*spos) += 2; 65 spos_tmp = start + offset; 66 return p2p_sd_dns_uncompress_label(upos, uend, start, 67 &spos_tmp, 68 *spos - 2); 69 } 70 71 /* Label */ 72 len = (*spos)[0] & 0x3f; 73 if (len == 0) 74 return 0; 75 76 (*spos)++; 77 if (len > end - *spos) { 78 wpa_printf(MSG_DEBUG, "P2P: Invalid domain name " 79 "sequence - no room for label with length " 80 "%u", len); 81 return -1; 82 } 83 84 if (len + 2 > uend - *upos) 85 return -2; 86 87 os_memcpy(*upos, *spos, len); 88 *spos += len; 89 *upos += len; 90 (*upos)[0] = '.'; 91 (*upos)++; 92 (*upos)[0] = '\0'; 93 } 94 95 return 0; 96 } 97 98 99 /* Uncompress domain names per RFC 1035 using the P2P SD in-memory packet. 100 * Returns -1 on parsing error (invalid input sequence), -2 if output buffer is 101 * not large enough */ 102 static int p2p_sd_dns_uncompress(char *buf, size_t buf_len, const u8 *msg, 103 size_t msg_len, size_t offset) 104 { 105 /* 27-octet in-memory packet from P2P specification */ 106 const char *prefix = "\x04_tcp\x05local\x00\x00\x0C\x00\x01" 107 "\x04_udp\xC0\x11\x00\x0C\x00\x01"; 108 u8 *tmp, *end, *spos; 109 char *upos, *uend; 110 int ret = 0; 111 112 if (buf_len < 2) 113 return -1; 114 if (offset > msg_len) 115 return -1; 116 117 tmp = os_malloc(DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN + msg_len); 118 if (tmp == NULL) 119 return -1; 120 spos = tmp + DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN; 121 end = spos + msg_len; 122 spos += offset; 123 124 os_memset(tmp, 0, DNS_HEADER_LEN); 125 os_memcpy(tmp + DNS_HEADER_LEN, prefix, P2P_SD_IN_MEMORY_LEN); 126 os_memcpy(tmp + DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN, msg, msg_len); 127 128 upos = buf; 129 uend = buf + buf_len; 130 131 ret = p2p_sd_dns_uncompress_label(&upos, uend, tmp, &spos, end); 132 if (ret) { 133 os_free(tmp); 134 return ret; 135 } 136 137 if (upos == buf) { 138 upos[0] = '.'; 139 upos[1] = '\0'; 140 } else if (upos[-1] == '.') 141 upos[-1] = '\0'; 142 143 os_free(tmp); 144 return 0; 145 } 146 147 148 static struct p2p_srv_bonjour * 149 wpas_p2p_service_get_bonjour(struct wpa_supplicant *wpa_s, 150 const struct wpabuf *query) 151 { 152 struct p2p_srv_bonjour *bsrv; 153 size_t len; 154 155 len = wpabuf_len(query); 156 dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, 157 struct p2p_srv_bonjour, list) { 158 if (len == wpabuf_len(bsrv->query) && 159 os_memcmp(wpabuf_head(query), wpabuf_head(bsrv->query), 160 len) == 0) 161 return bsrv; 162 } 163 return NULL; 164 } 165 166 167 static struct p2p_srv_upnp * 168 wpas_p2p_service_get_upnp(struct wpa_supplicant *wpa_s, u8 version, 169 const char *service) 170 { 171 struct p2p_srv_upnp *usrv; 172 173 dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, 174 struct p2p_srv_upnp, list) { 175 if (version == usrv->version && 176 os_strcmp(service, usrv->service) == 0) 177 return usrv; 178 } 179 return NULL; 180 } 181 182 183 static void wpas_sd_add_empty(struct wpabuf *resp, u8 srv_proto, 184 u8 srv_trans_id, u8 status) 185 { 186 u8 *len_pos; 187 188 if (wpabuf_tailroom(resp) < 5) 189 return; 190 191 /* Length (to be filled) */ 192 len_pos = wpabuf_put(resp, 2); 193 wpabuf_put_u8(resp, srv_proto); 194 wpabuf_put_u8(resp, srv_trans_id); 195 /* Status Code */ 196 wpabuf_put_u8(resp, status); 197 /* Response Data: empty */ 198 WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); 199 } 200 201 202 static void wpas_sd_add_proto_not_avail(struct wpabuf *resp, u8 srv_proto, 203 u8 srv_trans_id) 204 { 205 wpas_sd_add_empty(resp, srv_proto, srv_trans_id, 206 P2P_SD_PROTO_NOT_AVAILABLE); 207 } 208 209 210 static void wpas_sd_add_bad_request(struct wpabuf *resp, u8 srv_proto, 211 u8 srv_trans_id) 212 { 213 wpas_sd_add_empty(resp, srv_proto, srv_trans_id, P2P_SD_BAD_REQUEST); 214 } 215 216 217 static void wpas_sd_add_not_found(struct wpabuf *resp, u8 srv_proto, 218 u8 srv_trans_id) 219 { 220 wpas_sd_add_empty(resp, srv_proto, srv_trans_id, 221 P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); 222 } 223 224 225 static void wpas_sd_all_bonjour(struct wpa_supplicant *wpa_s, 226 struct wpabuf *resp, u8 srv_trans_id) 227 { 228 struct p2p_srv_bonjour *bsrv; 229 u8 *len_pos; 230 231 wpa_printf(MSG_DEBUG, "P2P: SD Request for all Bonjour services"); 232 233 if (dl_list_empty(&wpa_s->global->p2p_srv_bonjour)) { 234 wpa_printf(MSG_DEBUG, "P2P: Bonjour protocol not available"); 235 return; 236 } 237 238 dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, 239 struct p2p_srv_bonjour, list) { 240 if (wpabuf_tailroom(resp) < 241 5 + wpabuf_len(bsrv->query) + wpabuf_len(bsrv->resp)) 242 return; 243 /* Length (to be filled) */ 244 len_pos = wpabuf_put(resp, 2); 245 wpabuf_put_u8(resp, P2P_SERV_BONJOUR); 246 wpabuf_put_u8(resp, srv_trans_id); 247 /* Status Code */ 248 wpabuf_put_u8(resp, P2P_SD_SUCCESS); 249 wpa_hexdump_ascii(MSG_DEBUG, "P2P: Matching Bonjour service", 250 wpabuf_head(bsrv->resp), 251 wpabuf_len(bsrv->resp)); 252 /* Response Data */ 253 wpabuf_put_buf(resp, bsrv->query); /* Key */ 254 wpabuf_put_buf(resp, bsrv->resp); /* Value */ 255 WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 256 2); 257 } 258 } 259 260 261 static int match_bonjour_query(struct p2p_srv_bonjour *bsrv, const u8 *query, 262 size_t query_len) 263 { 264 char str_rx[256], str_srv[256]; 265 266 if (query_len < 3 || wpabuf_len(bsrv->query) < 3) 267 return 0; /* Too short to include DNS Type and Version */ 268 if (os_memcmp(query + query_len - 3, 269 wpabuf_head_u8(bsrv->query) + wpabuf_len(bsrv->query) - 3, 270 3) != 0) 271 return 0; /* Mismatch in DNS Type or Version */ 272 if (query_len == wpabuf_len(bsrv->query) && 273 os_memcmp(query, wpabuf_head(bsrv->query), query_len - 3) == 0) 274 return 1; /* Binary match */ 275 276 if (p2p_sd_dns_uncompress(str_rx, sizeof(str_rx), query, query_len - 3, 277 0)) 278 return 0; /* Failed to uncompress query */ 279 if (p2p_sd_dns_uncompress(str_srv, sizeof(str_srv), 280 wpabuf_head(bsrv->query), 281 wpabuf_len(bsrv->query) - 3, 0)) 282 return 0; /* Failed to uncompress service */ 283 284 return os_strcmp(str_rx, str_srv) == 0; 285 } 286 287 288 static void wpas_sd_req_bonjour(struct wpa_supplicant *wpa_s, 289 struct wpabuf *resp, u8 srv_trans_id, 290 const u8 *query, size_t query_len) 291 { 292 struct p2p_srv_bonjour *bsrv; 293 u8 *len_pos; 294 int matches = 0; 295 296 wpa_hexdump_ascii(MSG_DEBUG, "P2P: SD Request for Bonjour", 297 query, query_len); 298 if (dl_list_empty(&wpa_s->global->p2p_srv_bonjour)) { 299 wpa_printf(MSG_DEBUG, "P2P: Bonjour protocol not available"); 300 wpas_sd_add_proto_not_avail(resp, P2P_SERV_BONJOUR, 301 srv_trans_id); 302 return; 303 } 304 305 if (query_len == 0) { 306 wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); 307 return; 308 } 309 310 dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, 311 struct p2p_srv_bonjour, list) { 312 if (!match_bonjour_query(bsrv, query, query_len)) 313 continue; 314 315 if (wpabuf_tailroom(resp) < 316 5 + query_len + wpabuf_len(bsrv->resp)) 317 return; 318 319 matches++; 320 321 /* Length (to be filled) */ 322 len_pos = wpabuf_put(resp, 2); 323 wpabuf_put_u8(resp, P2P_SERV_BONJOUR); 324 wpabuf_put_u8(resp, srv_trans_id); 325 326 /* Status Code */ 327 wpabuf_put_u8(resp, P2P_SD_SUCCESS); 328 wpa_hexdump_ascii(MSG_DEBUG, "P2P: Matching Bonjour service", 329 wpabuf_head(bsrv->resp), 330 wpabuf_len(bsrv->resp)); 331 332 /* Response Data */ 333 wpabuf_put_data(resp, query, query_len); /* Key */ 334 wpabuf_put_buf(resp, bsrv->resp); /* Value */ 335 336 WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); 337 } 338 339 if (matches == 0) { 340 wpa_printf(MSG_DEBUG, "P2P: Requested Bonjour service not " 341 "available"); 342 if (wpabuf_tailroom(resp) < 5) 343 return; 344 345 /* Length (to be filled) */ 346 len_pos = wpabuf_put(resp, 2); 347 wpabuf_put_u8(resp, P2P_SERV_BONJOUR); 348 wpabuf_put_u8(resp, srv_trans_id); 349 350 /* Status Code */ 351 wpabuf_put_u8(resp, P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); 352 /* Response Data: empty */ 353 WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 354 2); 355 } 356 } 357 358 359 static void wpas_sd_all_upnp(struct wpa_supplicant *wpa_s, 360 struct wpabuf *resp, u8 srv_trans_id) 361 { 362 struct p2p_srv_upnp *usrv; 363 u8 *len_pos; 364 365 wpa_printf(MSG_DEBUG, "P2P: SD Request for all UPnP services"); 366 367 if (dl_list_empty(&wpa_s->global->p2p_srv_upnp)) { 368 wpa_printf(MSG_DEBUG, "P2P: UPnP protocol not available"); 369 return; 370 } 371 372 dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, 373 struct p2p_srv_upnp, list) { 374 if (wpabuf_tailroom(resp) < 5 + 1 + os_strlen(usrv->service)) 375 return; 376 377 /* Length (to be filled) */ 378 len_pos = wpabuf_put(resp, 2); 379 wpabuf_put_u8(resp, P2P_SERV_UPNP); 380 wpabuf_put_u8(resp, srv_trans_id); 381 382 /* Status Code */ 383 wpabuf_put_u8(resp, P2P_SD_SUCCESS); 384 /* Response Data */ 385 wpabuf_put_u8(resp, usrv->version); 386 wpa_printf(MSG_DEBUG, "P2P: Matching UPnP Service: %s", 387 usrv->service); 388 wpabuf_put_str(resp, usrv->service); 389 WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 390 2); 391 } 392 } 393 394 395 static void wpas_sd_req_upnp(struct wpa_supplicant *wpa_s, 396 struct wpabuf *resp, u8 srv_trans_id, 397 const u8 *query, size_t query_len) 398 { 399 struct p2p_srv_upnp *usrv; 400 u8 *len_pos; 401 u8 version; 402 char *str; 403 int count = 0; 404 405 wpa_hexdump_ascii(MSG_DEBUG, "P2P: SD Request for UPnP", 406 query, query_len); 407 408 if (dl_list_empty(&wpa_s->global->p2p_srv_upnp)) { 409 wpa_printf(MSG_DEBUG, "P2P: UPnP protocol not available"); 410 wpas_sd_add_proto_not_avail(resp, P2P_SERV_UPNP, 411 srv_trans_id); 412 return; 413 } 414 415 if (query_len == 0) { 416 wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); 417 return; 418 } 419 420 if (wpabuf_tailroom(resp) < 5) 421 return; 422 423 /* Length (to be filled) */ 424 len_pos = wpabuf_put(resp, 2); 425 wpabuf_put_u8(resp, P2P_SERV_UPNP); 426 wpabuf_put_u8(resp, srv_trans_id); 427 428 version = query[0]; 429 str = os_malloc(query_len); 430 if (str == NULL) 431 return; 432 os_memcpy(str, query + 1, query_len - 1); 433 str[query_len - 1] = '\0'; 434 435 dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, 436 struct p2p_srv_upnp, list) { 437 if (version != usrv->version) 438 continue; 439 440 if (os_strcmp(str, "ssdp:all") != 0 && 441 os_strstr(usrv->service, str) == NULL) 442 continue; 443 444 if (wpabuf_tailroom(resp) < 2) 445 break; 446 if (count == 0) { 447 /* Status Code */ 448 wpabuf_put_u8(resp, P2P_SD_SUCCESS); 449 /* Response Data */ 450 wpabuf_put_u8(resp, version); 451 } else 452 wpabuf_put_u8(resp, ','); 453 454 count++; 455 456 wpa_printf(MSG_DEBUG, "P2P: Matching UPnP Service: %s", 457 usrv->service); 458 if (wpabuf_tailroom(resp) < os_strlen(usrv->service)) 459 break; 460 wpabuf_put_str(resp, usrv->service); 461 } 462 os_free(str); 463 464 if (count == 0) { 465 wpa_printf(MSG_DEBUG, "P2P: Requested UPnP service not " 466 "available"); 467 /* Status Code */ 468 wpabuf_put_u8(resp, P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); 469 /* Response Data: empty */ 470 } 471 472 WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); 473 } 474 475 476 #ifdef CONFIG_WIFI_DISPLAY 477 static void wpas_sd_req_wfd(struct wpa_supplicant *wpa_s, 478 struct wpabuf *resp, u8 srv_trans_id, 479 const u8 *query, size_t query_len) 480 { 481 const u8 *pos; 482 u8 role; 483 u8 *len_pos; 484 485 wpa_hexdump(MSG_DEBUG, "P2P: SD Request for WFD", query, query_len); 486 487 if (!wpa_s->global->wifi_display) { 488 wpa_printf(MSG_DEBUG, "P2P: WFD protocol not available"); 489 wpas_sd_add_proto_not_avail(resp, P2P_SERV_WIFI_DISPLAY, 490 srv_trans_id); 491 return; 492 } 493 494 if (query_len < 1) { 495 wpa_printf(MSG_DEBUG, "P2P: Missing WFD Requested Device " 496 "Role"); 497 return; 498 } 499 500 if (wpabuf_tailroom(resp) < 5) 501 return; 502 503 pos = query; 504 role = *pos++; 505 wpa_printf(MSG_DEBUG, "P2P: WSD for device role 0x%x", role); 506 507 /* TODO: role specific handling */ 508 509 /* Length (to be filled) */ 510 len_pos = wpabuf_put(resp, 2); 511 wpabuf_put_u8(resp, P2P_SERV_WIFI_DISPLAY); 512 wpabuf_put_u8(resp, srv_trans_id); 513 wpabuf_put_u8(resp, P2P_SD_SUCCESS); /* Status Code */ 514 515 while (pos < query + query_len) { 516 if (*pos < MAX_WFD_SUBELEMS && 517 wpa_s->global->wfd_subelem[*pos] && 518 wpabuf_tailroom(resp) >= 519 wpabuf_len(wpa_s->global->wfd_subelem[*pos])) { 520 wpa_printf(MSG_DEBUG, "P2P: Add WSD response " 521 "subelement %u", *pos); 522 wpabuf_put_buf(resp, wpa_s->global->wfd_subelem[*pos]); 523 } 524 pos++; 525 } 526 527 WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); 528 } 529 #endif /* CONFIG_WIFI_DISPLAY */ 530 531 532 static int find_p2ps_substr(struct p2ps_advertisement *adv_data, 533 const u8 *needle, size_t needle_len) 534 { 535 const u8 *haystack = (const u8 *) adv_data->svc_info; 536 size_t haystack_len, i; 537 538 /* Allow search term to be empty */ 539 if (!needle || !needle_len) 540 return 1; 541 542 if (!haystack) 543 return 0; 544 545 haystack_len = os_strlen(adv_data->svc_info); 546 for (i = 0; i < haystack_len; i++) { 547 if (haystack_len - i < needle_len) 548 break; 549 if (os_memcmp(haystack + i, needle, needle_len) == 0) 550 return 1; 551 } 552 553 return 0; 554 } 555 556 557 static void wpas_sd_req_asp(struct wpa_supplicant *wpa_s, 558 struct wpabuf *resp, u8 srv_trans_id, 559 const u8 *query, size_t query_len) 560 { 561 struct p2ps_advertisement *adv_data; 562 const u8 *svc; 563 const u8 *info = NULL; 564 size_t svc_len; 565 size_t info_len = 0; 566 int prefix = 0; 567 u8 *count_pos = NULL; 568 u8 *len_pos = NULL; 569 570 wpa_hexdump(MSG_DEBUG, "P2P: SD Request for ASP", query, query_len); 571 572 if (query_len < 1) { 573 wpa_printf(MSG_DEBUG, "P2P: ASP bad request"); 574 wpas_sd_add_bad_request(resp, P2P_SERV_P2PS, srv_trans_id); 575 return; 576 } 577 578 svc_len = query[0]; 579 svc = &query[1]; 580 581 if (!wpa_s->global->p2p) { 582 wpa_printf(MSG_DEBUG, "P2P: ASP protocol not available"); 583 wpas_sd_add_proto_not_avail(resp, P2P_SERV_P2PS, srv_trans_id); 584 return; 585 } 586 587 /* Info block is optional */ 588 if (svc_len + 1 < query_len) { 589 info = &svc[svc_len]; 590 info_len = *info++; 591 } 592 593 /* Range check length of svc string and info block */ 594 if (svc_len + (info_len ? info_len + 2 : 1) > query_len) { 595 wpa_printf(MSG_DEBUG, "P2P: ASP bad request"); 596 wpas_sd_add_bad_request(resp, P2P_SERV_P2PS, srv_trans_id); 597 return; 598 } 599 600 /* Detect and correct for prefix search */ 601 if (svc_len && svc[svc_len - 1] == '*') { 602 prefix = 1; 603 svc_len--; 604 } 605 606 for (adv_data = p2p_get_p2ps_adv_list(wpa_s->global->p2p); 607 adv_data; adv_data = adv_data->next) { 608 /* If not a prefix match, reject length mismatches */ 609 if (!prefix && svc_len != os_strlen(adv_data->svc_name)) 610 continue; 611 612 /* Search each service for request */ 613 if (os_memcmp(adv_data->svc_name, svc, svc_len) == 0 && 614 find_p2ps_substr(adv_data, info, info_len)) { 615 size_t len = os_strlen(adv_data->svc_name); 616 size_t svc_info_len = 0; 617 618 if (adv_data->svc_info) 619 svc_info_len = os_strlen(adv_data->svc_info); 620 621 if (len > 0xff || svc_info_len > 0xffff) 622 return; 623 624 /* Length & Count to be filled as we go */ 625 if (!len_pos && !count_pos) { 626 if (wpabuf_tailroom(resp) < 627 len + svc_info_len + 16) 628 return; 629 630 len_pos = wpabuf_put(resp, 2); 631 wpabuf_put_u8(resp, P2P_SERV_P2PS); 632 wpabuf_put_u8(resp, srv_trans_id); 633 /* Status Code */ 634 wpabuf_put_u8(resp, P2P_SD_SUCCESS); 635 count_pos = wpabuf_put(resp, 1); 636 *count_pos = 0; 637 } else if (wpabuf_tailroom(resp) < 638 len + svc_info_len + 10) 639 return; 640 641 if (svc_info_len) { 642 wpa_printf(MSG_DEBUG, 643 "P2P: Add Svc: %s info: %s", 644 adv_data->svc_name, 645 adv_data->svc_info); 646 } else { 647 wpa_printf(MSG_DEBUG, "P2P: Add Svc: %s", 648 adv_data->svc_name); 649 } 650 651 /* Advertisement ID */ 652 wpabuf_put_le32(resp, adv_data->id); 653 654 /* Config Methods */ 655 wpabuf_put_be16(resp, adv_data->config_methods); 656 657 /* Service Name */ 658 wpabuf_put_u8(resp, (u8) len); 659 wpabuf_put_data(resp, adv_data->svc_name, len); 660 661 /* Service State */ 662 wpabuf_put_u8(resp, adv_data->state); 663 664 /* Service Information */ 665 wpabuf_put_le16(resp, (u16) svc_info_len); 666 wpabuf_put_data(resp, adv_data->svc_info, svc_info_len); 667 668 /* Update length and count */ 669 (*count_pos)++; 670 WPA_PUT_LE16(len_pos, 671 (u8 *) wpabuf_put(resp, 0) - len_pos - 2); 672 } 673 } 674 675 /* Return error if no matching svc found */ 676 if (count_pos == NULL) { 677 wpa_printf(MSG_DEBUG, "P2P: ASP service not found"); 678 wpas_sd_add_not_found(resp, P2P_SERV_P2PS, srv_trans_id); 679 } 680 } 681 682 683 static void wpas_sd_all_asp(struct wpa_supplicant *wpa_s, 684 struct wpabuf *resp, u8 srv_trans_id) 685 { 686 /* Query data to add all P2PS advertisements: 687 * - Service name length: 1 688 * - Service name: '*' 689 * - Service Information Request Length: 0 690 */ 691 const u8 q[] = { 1, (const u8) '*', 0 }; 692 693 if (p2p_get_p2ps_adv_list(wpa_s->global->p2p)) 694 wpas_sd_req_asp(wpa_s, resp, srv_trans_id, q, sizeof(q)); 695 } 696 697 698 void wpas_sd_request(void *ctx, int freq, const u8 *sa, u8 dialog_token, 699 u16 update_indic, const u8 *tlvs, size_t tlvs_len) 700 { 701 struct wpa_supplicant *wpa_s = ctx; 702 const u8 *pos = tlvs; 703 const u8 *end = tlvs + tlvs_len; 704 const u8 *tlv_end; 705 u16 slen; 706 struct wpabuf *resp; 707 u8 srv_proto, srv_trans_id; 708 size_t buf_len; 709 char *buf; 710 711 wpa_hexdump(MSG_MSGDUMP, "P2P: Service Discovery Request TLVs", 712 tlvs, tlvs_len); 713 buf_len = 2 * tlvs_len + 1; 714 buf = os_malloc(buf_len); 715 if (buf) { 716 wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len); 717 wpa_msg_ctrl(wpa_s, MSG_INFO, P2P_EVENT_SERV_DISC_REQ "%d " 718 MACSTR " %u %u %s", 719 freq, MAC2STR(sa), dialog_token, update_indic, 720 buf); 721 os_free(buf); 722 } 723 724 if (wpa_s->p2p_sd_over_ctrl_iface) { 725 wpas_notify_p2p_sd_request(wpa_s, freq, sa, dialog_token, 726 update_indic, tlvs, tlvs_len); 727 return; /* to be processed by an external program */ 728 } 729 730 resp = wpabuf_alloc(10000); 731 if (resp == NULL) 732 return; 733 734 while (end - pos > 1) { 735 wpa_printf(MSG_DEBUG, "P2P: Service Request TLV"); 736 slen = WPA_GET_LE16(pos); 737 pos += 2; 738 if (slen > end - pos || slen < 2) { 739 wpa_printf(MSG_DEBUG, "P2P: Unexpected Query Data " 740 "length"); 741 wpabuf_free(resp); 742 return; 743 } 744 tlv_end = pos + slen; 745 746 srv_proto = *pos++; 747 wpa_printf(MSG_DEBUG, "P2P: Service Protocol Type %u", 748 srv_proto); 749 srv_trans_id = *pos++; 750 wpa_printf(MSG_DEBUG, "P2P: Service Transaction ID %u", 751 srv_trans_id); 752 753 wpa_hexdump(MSG_MSGDUMP, "P2P: Query Data", 754 pos, tlv_end - pos); 755 756 757 if (wpa_s->force_long_sd) { 758 wpa_printf(MSG_DEBUG, "P2P: SD test - force long " 759 "response"); 760 wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); 761 wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); 762 wpas_sd_all_asp(wpa_s, resp, srv_trans_id); 763 goto done; 764 } 765 766 switch (srv_proto) { 767 case P2P_SERV_ALL_SERVICES: 768 wpa_printf(MSG_DEBUG, "P2P: Service Discovery Request " 769 "for all services"); 770 if (dl_list_empty(&wpa_s->global->p2p_srv_upnp) && 771 dl_list_empty(&wpa_s->global->p2p_srv_bonjour) && 772 !p2p_get_p2ps_adv_list(wpa_s->global->p2p)) { 773 wpa_printf(MSG_DEBUG, "P2P: No service " 774 "discovery protocols available"); 775 wpas_sd_add_proto_not_avail( 776 resp, P2P_SERV_ALL_SERVICES, 777 srv_trans_id); 778 break; 779 } 780 wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); 781 wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); 782 wpas_sd_all_asp(wpa_s, resp, srv_trans_id); 783 break; 784 case P2P_SERV_BONJOUR: 785 wpas_sd_req_bonjour(wpa_s, resp, srv_trans_id, 786 pos, tlv_end - pos); 787 break; 788 case P2P_SERV_UPNP: 789 wpas_sd_req_upnp(wpa_s, resp, srv_trans_id, 790 pos, tlv_end - pos); 791 break; 792 #ifdef CONFIG_WIFI_DISPLAY 793 case P2P_SERV_WIFI_DISPLAY: 794 wpas_sd_req_wfd(wpa_s, resp, srv_trans_id, 795 pos, tlv_end - pos); 796 break; 797 #endif /* CONFIG_WIFI_DISPLAY */ 798 case P2P_SERV_P2PS: 799 wpas_sd_req_asp(wpa_s, resp, srv_trans_id, 800 pos, tlv_end - pos); 801 break; 802 default: 803 wpa_printf(MSG_DEBUG, "P2P: Unavailable service " 804 "protocol %u", srv_proto); 805 wpas_sd_add_proto_not_avail(resp, srv_proto, 806 srv_trans_id); 807 break; 808 } 809 810 pos = tlv_end; 811 } 812 813 done: 814 wpas_notify_p2p_sd_request(wpa_s, freq, sa, dialog_token, 815 update_indic, tlvs, tlvs_len); 816 817 wpas_p2p_sd_response(wpa_s, freq, sa, dialog_token, resp); 818 819 wpabuf_free(resp); 820 } 821 822 823 static void wpas_sd_p2ps_serv_response(struct wpa_supplicant *wpa_s, 824 const u8 *sa, u8 srv_trans_id, 825 const u8 *pos, const u8 *tlv_end) 826 { 827 u8 left = *pos++; 828 u32 adv_id; 829 u8 svc_status; 830 u16 config_methods; 831 char svc_str[256]; 832 833 while (left-- && pos < tlv_end) { 834 char *buf = NULL; 835 size_t buf_len; 836 u8 svc_len; 837 838 /* Sanity check fixed length+svc_str */ 839 if (6 >= tlv_end - pos) 840 break; 841 svc_len = pos[6]; 842 if (svc_len + 10 > tlv_end - pos) 843 break; 844 845 /* Advertisement ID */ 846 adv_id = WPA_GET_LE32(pos); 847 pos += sizeof(u32); 848 849 /* Config Methods */ 850 config_methods = WPA_GET_BE16(pos); 851 pos += sizeof(u16); 852 853 /* Service Name */ 854 pos++; /* svc_len */ 855 os_memcpy(svc_str, pos, svc_len); 856 svc_str[svc_len] = '\0'; 857 pos += svc_len; 858 859 /* Service Status */ 860 svc_status = *pos++; 861 862 /* Service Information Length */ 863 buf_len = WPA_GET_LE16(pos); 864 pos += sizeof(u16); 865 866 /* Sanity check buffer length */ 867 if (buf_len > (unsigned int) (tlv_end - pos)) 868 break; 869 870 if (buf_len) { 871 buf = os_zalloc(2 * buf_len + 1); 872 if (buf) { 873 utf8_escape((const char *) pos, buf_len, buf, 874 2 * buf_len + 1); 875 } 876 } 877 878 pos += buf_len; 879 880 if (buf) { 881 wpa_msg_global(wpa_s, MSG_INFO, P2P_EVENT_SERV_ASP_RESP 882 MACSTR " %x %x %x %x %s '%s'", 883 MAC2STR(sa), srv_trans_id, adv_id, 884 svc_status, config_methods, svc_str, 885 buf); 886 os_free(buf); 887 } else { 888 wpa_msg_global(wpa_s, MSG_INFO, P2P_EVENT_SERV_ASP_RESP 889 MACSTR " %x %x %x %x %s", 890 MAC2STR(sa), srv_trans_id, adv_id, 891 svc_status, config_methods, svc_str); 892 } 893 } 894 } 895 896 897 void wpas_sd_response(void *ctx, const u8 *sa, u16 update_indic, 898 const u8 *tlvs, size_t tlvs_len) 899 { 900 struct wpa_supplicant *wpa_s = ctx; 901 const u8 *pos = tlvs; 902 const u8 *end = tlvs + tlvs_len; 903 const u8 *tlv_end; 904 u16 slen; 905 size_t buf_len; 906 char *buf; 907 908 wpa_hexdump(MSG_MSGDUMP, "P2P: Service Discovery Response TLVs", 909 tlvs, tlvs_len); 910 if (tlvs_len > 1500) { 911 /* TODO: better way for handling this */ 912 wpa_msg_ctrl(wpa_s, MSG_INFO, 913 P2P_EVENT_SERV_DISC_RESP MACSTR 914 " %u <long response: %u bytes>", 915 MAC2STR(sa), update_indic, 916 (unsigned int) tlvs_len); 917 } else { 918 buf_len = 2 * tlvs_len + 1; 919 buf = os_malloc(buf_len); 920 if (buf) { 921 wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len); 922 wpa_msg_ctrl(wpa_s, MSG_INFO, 923 P2P_EVENT_SERV_DISC_RESP MACSTR " %u %s", 924 MAC2STR(sa), update_indic, buf); 925 os_free(buf); 926 } 927 } 928 929 while (end - pos >= 2) { 930 u8 srv_proto, srv_trans_id, status; 931 932 wpa_printf(MSG_DEBUG, "P2P: Service Response TLV"); 933 slen = WPA_GET_LE16(pos); 934 pos += 2; 935 if (slen > end - pos || slen < 3) { 936 wpa_printf(MSG_DEBUG, "P2P: Unexpected Response Data " 937 "length"); 938 return; 939 } 940 tlv_end = pos + slen; 941 942 srv_proto = *pos++; 943 wpa_printf(MSG_DEBUG, "P2P: Service Protocol Type %u", 944 srv_proto); 945 srv_trans_id = *pos++; 946 wpa_printf(MSG_DEBUG, "P2P: Service Transaction ID %u", 947 srv_trans_id); 948 status = *pos++; 949 wpa_printf(MSG_DEBUG, "P2P: Status Code ID %u", 950 status); 951 952 wpa_hexdump(MSG_MSGDUMP, "P2P: Response Data", 953 pos, tlv_end - pos); 954 955 if (srv_proto == P2P_SERV_P2PS && pos < tlv_end) { 956 wpas_sd_p2ps_serv_response(wpa_s, sa, srv_trans_id, 957 pos, tlv_end); 958 } 959 960 pos = tlv_end; 961 } 962 963 wpas_notify_p2p_sd_response(wpa_s, sa, update_indic, tlvs, tlvs_len); 964 } 965 966 967 u64 wpas_p2p_sd_request(struct wpa_supplicant *wpa_s, const u8 *dst, 968 const struct wpabuf *tlvs) 969 { 970 if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) 971 return 0; 972 return (uintptr_t) p2p_sd_request(wpa_s->global->p2p, dst, tlvs); 973 } 974 975 976 u64 wpas_p2p_sd_request_upnp(struct wpa_supplicant *wpa_s, const u8 *dst, 977 u8 version, const char *query) 978 { 979 struct wpabuf *tlvs; 980 u64 ret; 981 982 tlvs = wpabuf_alloc(2 + 1 + 1 + 1 + os_strlen(query)); 983 if (tlvs == NULL) 984 return 0; 985 wpabuf_put_le16(tlvs, 1 + 1 + 1 + os_strlen(query)); 986 wpabuf_put_u8(tlvs, P2P_SERV_UPNP); /* Service Protocol Type */ 987 wpabuf_put_u8(tlvs, 1); /* Service Transaction ID */ 988 wpabuf_put_u8(tlvs, version); 989 wpabuf_put_str(tlvs, query); 990 ret = wpas_p2p_sd_request(wpa_s, dst, tlvs); 991 wpabuf_free(tlvs); 992 return ret; 993 } 994 995 996 u64 wpas_p2p_sd_request_asp(struct wpa_supplicant *wpa_s, const u8 *dst, u8 id, 997 const char *svc_str, const char *info_substr) 998 { 999 struct wpabuf *tlvs; 1000 size_t plen, svc_len, substr_len = 0; 1001 u64 ret; 1002 1003 svc_len = os_strlen(svc_str); 1004 if (info_substr) 1005 substr_len = os_strlen(info_substr); 1006 1007 if (svc_len > 0xff || substr_len > 0xff) 1008 return 0; 1009 1010 plen = 1 + 1 + 1 + svc_len + 1 + substr_len; 1011 tlvs = wpabuf_alloc(2 + plen); 1012 if (tlvs == NULL) 1013 return 0; 1014 1015 wpabuf_put_le16(tlvs, plen); 1016 wpabuf_put_u8(tlvs, P2P_SERV_P2PS); 1017 wpabuf_put_u8(tlvs, id); /* Service Transaction ID */ 1018 wpabuf_put_u8(tlvs, (u8) svc_len); /* Service String Length */ 1019 wpabuf_put_data(tlvs, svc_str, svc_len); 1020 wpabuf_put_u8(tlvs, (u8) substr_len); /* Info Substring Length */ 1021 wpabuf_put_data(tlvs, info_substr, substr_len); 1022 ret = wpas_p2p_sd_request(wpa_s, dst, tlvs); 1023 wpabuf_free(tlvs); 1024 1025 return ret; 1026 } 1027 1028 1029 #ifdef CONFIG_WIFI_DISPLAY 1030 1031 static u64 wpas_p2p_sd_request_wfd(struct wpa_supplicant *wpa_s, const u8 *dst, 1032 const struct wpabuf *tlvs) 1033 { 1034 if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) 1035 return 0; 1036 return (uintptr_t) p2p_sd_request_wfd(wpa_s->global->p2p, dst, tlvs); 1037 } 1038 1039 1040 #define MAX_WFD_SD_SUBELEMS 20 1041 1042 static void wfd_add_sd_req_role(struct wpabuf *tlvs, u8 id, u8 role, 1043 const char *subelems) 1044 { 1045 u8 *len; 1046 const char *pos; 1047 int val; 1048 int count = 0; 1049 1050 len = wpabuf_put(tlvs, 2); 1051 wpabuf_put_u8(tlvs, P2P_SERV_WIFI_DISPLAY); /* Service Protocol Type */ 1052 wpabuf_put_u8(tlvs, id); /* Service Transaction ID */ 1053 1054 wpabuf_put_u8(tlvs, role); 1055 1056 pos = subelems; 1057 while (*pos) { 1058 val = atoi(pos); 1059 if (val >= 0 && val < 256) { 1060 wpabuf_put_u8(tlvs, val); 1061 count++; 1062 if (count == MAX_WFD_SD_SUBELEMS) 1063 break; 1064 } 1065 pos = os_strchr(pos + 1, ','); 1066 if (pos == NULL) 1067 break; 1068 pos++; 1069 } 1070 1071 WPA_PUT_LE16(len, (u8 *) wpabuf_put(tlvs, 0) - len - 2); 1072 } 1073 1074 1075 u64 wpas_p2p_sd_request_wifi_display(struct wpa_supplicant *wpa_s, 1076 const u8 *dst, const char *role) 1077 { 1078 struct wpabuf *tlvs; 1079 u64 ret; 1080 const char *subelems; 1081 u8 id = 1; 1082 1083 subelems = os_strchr(role, ' '); 1084 if (subelems == NULL) 1085 return 0; 1086 subelems++; 1087 1088 tlvs = wpabuf_alloc(4 * (2 + 1 + 1 + 1 + MAX_WFD_SD_SUBELEMS)); 1089 if (tlvs == NULL) 1090 return 0; 1091 1092 if (os_strstr(role, "[source]")) 1093 wfd_add_sd_req_role(tlvs, id++, 0x00, subelems); 1094 if (os_strstr(role, "[pri-sink]")) 1095 wfd_add_sd_req_role(tlvs, id++, 0x01, subelems); 1096 if (os_strstr(role, "[sec-sink]")) 1097 wfd_add_sd_req_role(tlvs, id++, 0x02, subelems); 1098 if (os_strstr(role, "[source+sink]")) 1099 wfd_add_sd_req_role(tlvs, id++, 0x03, subelems); 1100 1101 ret = wpas_p2p_sd_request_wfd(wpa_s, dst, tlvs); 1102 wpabuf_free(tlvs); 1103 return ret; 1104 } 1105 1106 #endif /* CONFIG_WIFI_DISPLAY */ 1107 1108 1109 int wpas_p2p_sd_cancel_request(struct wpa_supplicant *wpa_s, u64 req) 1110 { 1111 if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) 1112 return -1; 1113 return p2p_sd_cancel_request(wpa_s->global->p2p, 1114 (void *) (uintptr_t) req); 1115 } 1116 1117 1118 void wpas_p2p_sd_response(struct wpa_supplicant *wpa_s, int freq, 1119 const u8 *dst, u8 dialog_token, 1120 const struct wpabuf *resp_tlvs) 1121 { 1122 if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) 1123 return; 1124 p2p_sd_response(wpa_s->global->p2p, freq, dst, dialog_token, 1125 resp_tlvs); 1126 } 1127 1128 1129 void wpas_p2p_sd_service_update(struct wpa_supplicant *wpa_s) 1130 { 1131 if (wpa_s->global->p2p) 1132 p2p_sd_service_update(wpa_s->global->p2p); 1133 } 1134 1135 1136 static void wpas_p2p_srv_bonjour_free(struct p2p_srv_bonjour *bsrv) 1137 { 1138 dl_list_del(&bsrv->list); 1139 wpabuf_free(bsrv->query); 1140 wpabuf_free(bsrv->resp); 1141 os_free(bsrv); 1142 } 1143 1144 1145 static void wpas_p2p_srv_upnp_free(struct p2p_srv_upnp *usrv) 1146 { 1147 dl_list_del(&usrv->list); 1148 os_free(usrv->service); 1149 os_free(usrv); 1150 } 1151 1152 1153 void wpas_p2p_service_flush(struct wpa_supplicant *wpa_s) 1154 { 1155 struct p2p_srv_bonjour *bsrv, *bn; 1156 struct p2p_srv_upnp *usrv, *un; 1157 1158 dl_list_for_each_safe(bsrv, bn, &wpa_s->global->p2p_srv_bonjour, 1159 struct p2p_srv_bonjour, list) 1160 wpas_p2p_srv_bonjour_free(bsrv); 1161 1162 dl_list_for_each_safe(usrv, un, &wpa_s->global->p2p_srv_upnp, 1163 struct p2p_srv_upnp, list) 1164 wpas_p2p_srv_upnp_free(usrv); 1165 1166 wpas_p2p_service_flush_asp(wpa_s); 1167 wpas_p2p_sd_service_update(wpa_s); 1168 } 1169 1170 1171 int wpas_p2p_service_p2ps_id_exists(struct wpa_supplicant *wpa_s, u32 adv_id) 1172 { 1173 if (adv_id == 0) 1174 return 1; 1175 1176 if (p2p_service_p2ps_id(wpa_s->global->p2p, adv_id)) 1177 return 1; 1178 1179 return 0; 1180 } 1181 1182 1183 int wpas_p2p_service_del_asp(struct wpa_supplicant *wpa_s, u32 adv_id) 1184 { 1185 int ret; 1186 1187 ret = p2p_service_del_asp(wpa_s->global->p2p, adv_id); 1188 if (ret == 0) 1189 wpas_p2p_sd_service_update(wpa_s); 1190 return ret; 1191 } 1192 1193 1194 int wpas_p2p_service_add_asp(struct wpa_supplicant *wpa_s, 1195 int auto_accept, u32 adv_id, 1196 const char *adv_str, u8 svc_state, 1197 u16 config_methods, const char *svc_info, 1198 const u8 *cpt_priority) 1199 { 1200 int ret; 1201 1202 ret = p2p_service_add_asp(wpa_s->global->p2p, auto_accept, adv_id, 1203 adv_str, svc_state, config_methods, 1204 svc_info, cpt_priority); 1205 if (ret == 0) 1206 wpas_p2p_sd_service_update(wpa_s); 1207 return ret; 1208 } 1209 1210 1211 void wpas_p2p_service_flush_asp(struct wpa_supplicant *wpa_s) 1212 { 1213 p2p_service_flush_asp(wpa_s->global->p2p); 1214 } 1215 1216 1217 int wpas_p2p_service_add_bonjour(struct wpa_supplicant *wpa_s, 1218 struct wpabuf *query, struct wpabuf *resp) 1219 { 1220 struct p2p_srv_bonjour *bsrv; 1221 1222 bsrv = os_zalloc(sizeof(*bsrv)); 1223 if (bsrv == NULL) 1224 return -1; 1225 bsrv->query = query; 1226 bsrv->resp = resp; 1227 dl_list_add(&wpa_s->global->p2p_srv_bonjour, &bsrv->list); 1228 1229 wpas_p2p_sd_service_update(wpa_s); 1230 return 0; 1231 } 1232 1233 1234 int wpas_p2p_service_del_bonjour(struct wpa_supplicant *wpa_s, 1235 const struct wpabuf *query) 1236 { 1237 struct p2p_srv_bonjour *bsrv; 1238 1239 bsrv = wpas_p2p_service_get_bonjour(wpa_s, query); 1240 if (bsrv == NULL) 1241 return -1; 1242 wpas_p2p_srv_bonjour_free(bsrv); 1243 wpas_p2p_sd_service_update(wpa_s); 1244 return 0; 1245 } 1246 1247 1248 int wpas_p2p_service_add_upnp(struct wpa_supplicant *wpa_s, u8 version, 1249 const char *service) 1250 { 1251 struct p2p_srv_upnp *usrv; 1252 1253 if (wpas_p2p_service_get_upnp(wpa_s, version, service)) 1254 return 0; /* Already listed */ 1255 usrv = os_zalloc(sizeof(*usrv)); 1256 if (usrv == NULL) 1257 return -1; 1258 usrv->version = version; 1259 usrv->service = os_strdup(service); 1260 if (usrv->service == NULL) { 1261 os_free(usrv); 1262 return -1; 1263 } 1264 dl_list_add(&wpa_s->global->p2p_srv_upnp, &usrv->list); 1265 1266 wpas_p2p_sd_service_update(wpa_s); 1267 return 0; 1268 } 1269 1270 1271 int wpas_p2p_service_del_upnp(struct wpa_supplicant *wpa_s, u8 version, 1272 const char *service) 1273 { 1274 struct p2p_srv_upnp *usrv; 1275 1276 usrv = wpas_p2p_service_get_upnp(wpa_s, version, service); 1277 if (usrv == NULL) 1278 return -1; 1279 wpas_p2p_srv_upnp_free(usrv); 1280 wpas_p2p_sd_service_update(wpa_s); 1281 return 0; 1282 } 1283
