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 = &query[1]; 563 const u8 *info = NULL; 564 size_t svc_len = query[0]; 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 (!wpa_s->global->p2p) { 573 wpa_printf(MSG_DEBUG, "P2P: ASP protocol not available"); 574 wpas_sd_add_proto_not_avail(resp, P2P_SERV_P2PS, srv_trans_id); 575 return; 576 } 577 578 /* Info block is optional */ 579 if (svc_len + 1 < query_len) { 580 info = &svc[svc_len]; 581 info_len = *info++; 582 } 583 584 /* Range check length of svc string and info block */ 585 if (svc_len + (info_len ? info_len + 2 : 1) > query_len) { 586 wpa_printf(MSG_DEBUG, "P2P: ASP bad request"); 587 wpas_sd_add_bad_request(resp, P2P_SERV_P2PS, srv_trans_id); 588 return; 589 } 590 591 /* Detect and correct for prefix search */ 592 if (svc_len && svc[svc_len - 1] == '*') { 593 prefix = 1; 594 svc_len--; 595 } 596 597 for (adv_data = p2p_get_p2ps_adv_list(wpa_s->global->p2p); 598 adv_data; adv_data = adv_data->next) { 599 /* If not a prefix match, reject length mismatches */ 600 if (!prefix && svc_len != os_strlen(adv_data->svc_name)) 601 continue; 602 603 /* Search each service for request */ 604 if (os_memcmp(adv_data->svc_name, svc, svc_len) == 0 && 605 find_p2ps_substr(adv_data, info, info_len)) { 606 size_t len = os_strlen(adv_data->svc_name); 607 size_t svc_info_len = 0; 608 609 if (adv_data->svc_info) 610 svc_info_len = os_strlen(adv_data->svc_info); 611 612 if (len > 0xff || svc_info_len > 0xffff) 613 return; 614 615 /* Length & Count to be filled as we go */ 616 if (!len_pos && !count_pos) { 617 if (wpabuf_tailroom(resp) < 618 len + svc_info_len + 16) 619 return; 620 621 len_pos = wpabuf_put(resp, 2); 622 wpabuf_put_u8(resp, P2P_SERV_P2PS); 623 wpabuf_put_u8(resp, srv_trans_id); 624 /* Status Code */ 625 wpabuf_put_u8(resp, P2P_SD_SUCCESS); 626 count_pos = wpabuf_put(resp, 1); 627 *count_pos = 0; 628 } else if (wpabuf_tailroom(resp) < 629 len + svc_info_len + 10) 630 return; 631 632 if (svc_info_len) { 633 wpa_printf(MSG_DEBUG, 634 "P2P: Add Svc: %s info: %s", 635 adv_data->svc_name, 636 adv_data->svc_info); 637 } else { 638 wpa_printf(MSG_DEBUG, "P2P: Add Svc: %s", 639 adv_data->svc_name); 640 } 641 642 /* Advertisement ID */ 643 wpabuf_put_le32(resp, adv_data->id); 644 645 /* Config Methods */ 646 wpabuf_put_be16(resp, adv_data->config_methods); 647 648 /* Service Name */ 649 wpabuf_put_u8(resp, (u8) len); 650 wpabuf_put_data(resp, adv_data->svc_name, len); 651 652 /* Service State */ 653 wpabuf_put_u8(resp, adv_data->state); 654 655 /* Service Information */ 656 wpabuf_put_le16(resp, (u16) svc_info_len); 657 wpabuf_put_data(resp, adv_data->svc_info, svc_info_len); 658 659 /* Update length and count */ 660 (*count_pos)++; 661 WPA_PUT_LE16(len_pos, 662 (u8 *) wpabuf_put(resp, 0) - len_pos - 2); 663 } 664 } 665 666 /* Return error if no matching svc found */ 667 if (count_pos == NULL) { 668 wpa_printf(MSG_DEBUG, "P2P: ASP service not found"); 669 wpas_sd_add_not_found(resp, P2P_SERV_P2PS, srv_trans_id); 670 } 671 } 672 673 674 static void wpas_sd_all_asp(struct wpa_supplicant *wpa_s, 675 struct wpabuf *resp, u8 srv_trans_id) 676 { 677 /* Query data to add all P2PS advertisements: 678 * - Service name length: 1 679 * - Service name: '*' 680 * - Service Information Request Length: 0 681 */ 682 const u8 q[] = { 1, (const u8) '*', 0 }; 683 684 if (p2p_get_p2ps_adv_list(wpa_s->global->p2p)) 685 wpas_sd_req_asp(wpa_s, resp, srv_trans_id, q, sizeof(q)); 686 } 687 688 689 void wpas_sd_request(void *ctx, int freq, const u8 *sa, u8 dialog_token, 690 u16 update_indic, const u8 *tlvs, size_t tlvs_len) 691 { 692 struct wpa_supplicant *wpa_s = ctx; 693 const u8 *pos = tlvs; 694 const u8 *end = tlvs + tlvs_len; 695 const u8 *tlv_end; 696 u16 slen; 697 struct wpabuf *resp; 698 u8 srv_proto, srv_trans_id; 699 size_t buf_len; 700 char *buf; 701 702 wpa_hexdump(MSG_MSGDUMP, "P2P: Service Discovery Request TLVs", 703 tlvs, tlvs_len); 704 buf_len = 2 * tlvs_len + 1; 705 buf = os_malloc(buf_len); 706 if (buf) { 707 wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len); 708 wpa_msg_ctrl(wpa_s, MSG_INFO, P2P_EVENT_SERV_DISC_REQ "%d " 709 MACSTR " %u %u %s", 710 freq, MAC2STR(sa), dialog_token, update_indic, 711 buf); 712 os_free(buf); 713 } 714 715 if (wpa_s->p2p_sd_over_ctrl_iface) { 716 wpas_notify_p2p_sd_request(wpa_s, freq, sa, dialog_token, 717 update_indic, tlvs, tlvs_len); 718 return; /* to be processed by an external program */ 719 } 720 721 resp = wpabuf_alloc(10000); 722 if (resp == NULL) 723 return; 724 725 while (end - pos > 1) { 726 wpa_printf(MSG_DEBUG, "P2P: Service Request TLV"); 727 slen = WPA_GET_LE16(pos); 728 pos += 2; 729 if (slen > end - pos || slen < 2) { 730 wpa_printf(MSG_DEBUG, "P2P: Unexpected Query Data " 731 "length"); 732 wpabuf_free(resp); 733 return; 734 } 735 tlv_end = pos + slen; 736 737 srv_proto = *pos++; 738 wpa_printf(MSG_DEBUG, "P2P: Service Protocol Type %u", 739 srv_proto); 740 srv_trans_id = *pos++; 741 wpa_printf(MSG_DEBUG, "P2P: Service Transaction ID %u", 742 srv_trans_id); 743 744 wpa_hexdump(MSG_MSGDUMP, "P2P: Query Data", 745 pos, tlv_end - pos); 746 747 748 if (wpa_s->force_long_sd) { 749 wpa_printf(MSG_DEBUG, "P2P: SD test - force long " 750 "response"); 751 wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); 752 wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); 753 wpas_sd_all_asp(wpa_s, resp, srv_trans_id); 754 goto done; 755 } 756 757 switch (srv_proto) { 758 case P2P_SERV_ALL_SERVICES: 759 wpa_printf(MSG_DEBUG, "P2P: Service Discovery Request " 760 "for all services"); 761 if (dl_list_empty(&wpa_s->global->p2p_srv_upnp) && 762 dl_list_empty(&wpa_s->global->p2p_srv_bonjour) && 763 !p2p_get_p2ps_adv_list(wpa_s->global->p2p)) { 764 wpa_printf(MSG_DEBUG, "P2P: No service " 765 "discovery protocols available"); 766 wpas_sd_add_proto_not_avail( 767 resp, P2P_SERV_ALL_SERVICES, 768 srv_trans_id); 769 break; 770 } 771 wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); 772 wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); 773 wpas_sd_all_asp(wpa_s, resp, srv_trans_id); 774 break; 775 case P2P_SERV_BONJOUR: 776 wpas_sd_req_bonjour(wpa_s, resp, srv_trans_id, 777 pos, tlv_end - pos); 778 break; 779 case P2P_SERV_UPNP: 780 wpas_sd_req_upnp(wpa_s, resp, srv_trans_id, 781 pos, tlv_end - pos); 782 break; 783 #ifdef CONFIG_WIFI_DISPLAY 784 case P2P_SERV_WIFI_DISPLAY: 785 wpas_sd_req_wfd(wpa_s, resp, srv_trans_id, 786 pos, tlv_end - pos); 787 break; 788 #endif /* CONFIG_WIFI_DISPLAY */ 789 case P2P_SERV_P2PS: 790 wpas_sd_req_asp(wpa_s, resp, srv_trans_id, 791 pos, tlv_end - pos); 792 break; 793 default: 794 wpa_printf(MSG_DEBUG, "P2P: Unavailable service " 795 "protocol %u", srv_proto); 796 wpas_sd_add_proto_not_avail(resp, srv_proto, 797 srv_trans_id); 798 break; 799 } 800 801 pos = tlv_end; 802 } 803 804 done: 805 wpas_notify_p2p_sd_request(wpa_s, freq, sa, dialog_token, 806 update_indic, tlvs, tlvs_len); 807 808 wpas_p2p_sd_response(wpa_s, freq, sa, dialog_token, resp); 809 810 wpabuf_free(resp); 811 } 812 813 814 static void wpas_sd_p2ps_serv_response(struct wpa_supplicant *wpa_s, 815 const u8 *sa, u8 srv_trans_id, 816 const u8 *pos, const u8 *tlv_end) 817 { 818 u8 left = *pos++; 819 u32 adv_id; 820 u8 svc_status; 821 u16 config_methods; 822 char svc_str[256]; 823 824 while (left-- && pos < tlv_end) { 825 char *buf = NULL; 826 size_t buf_len; 827 u8 svc_len; 828 829 /* Sanity check fixed length+svc_str */ 830 if (6 >= tlv_end - pos) 831 break; 832 svc_len = pos[6]; 833 if (svc_len + 10 > tlv_end - pos) 834 break; 835 836 /* Advertisement ID */ 837 adv_id = WPA_GET_LE32(pos); 838 pos += sizeof(u32); 839 840 /* Config Methods */ 841 config_methods = WPA_GET_BE16(pos); 842 pos += sizeof(u16); 843 844 /* Service Name */ 845 pos++; /* svc_len */ 846 os_memcpy(svc_str, pos, svc_len); 847 svc_str[svc_len] = '\0'; 848 pos += svc_len; 849 850 /* Service Status */ 851 svc_status = *pos++; 852 853 /* Service Information Length */ 854 buf_len = WPA_GET_LE16(pos); 855 pos += sizeof(u16); 856 857 /* Sanity check buffer length */ 858 if (buf_len > (unsigned int) (tlv_end - pos)) 859 break; 860 861 if (buf_len) { 862 buf = os_zalloc(2 * buf_len + 1); 863 if (buf) { 864 utf8_escape((const char *) pos, buf_len, buf, 865 2 * buf_len + 1); 866 } 867 } 868 869 pos += buf_len; 870 871 if (buf) { 872 wpa_msg_global(wpa_s, MSG_INFO, P2P_EVENT_SERV_ASP_RESP 873 MACSTR " %x %x %x %x %s '%s'", 874 MAC2STR(sa), srv_trans_id, adv_id, 875 svc_status, config_methods, svc_str, 876 buf); 877 os_free(buf); 878 } else { 879 wpa_msg_global(wpa_s, MSG_INFO, P2P_EVENT_SERV_ASP_RESP 880 MACSTR " %x %x %x %x %s", 881 MAC2STR(sa), srv_trans_id, adv_id, 882 svc_status, config_methods, svc_str); 883 } 884 } 885 } 886 887 888 void wpas_sd_response(void *ctx, const u8 *sa, u16 update_indic, 889 const u8 *tlvs, size_t tlvs_len) 890 { 891 struct wpa_supplicant *wpa_s = ctx; 892 const u8 *pos = tlvs; 893 const u8 *end = tlvs + tlvs_len; 894 const u8 *tlv_end; 895 u16 slen; 896 size_t buf_len; 897 char *buf; 898 899 wpa_hexdump(MSG_MSGDUMP, "P2P: Service Discovery Response TLVs", 900 tlvs, tlvs_len); 901 if (tlvs_len > 1500) { 902 /* TODO: better way for handling this */ 903 wpa_msg_ctrl(wpa_s, MSG_INFO, 904 P2P_EVENT_SERV_DISC_RESP MACSTR 905 " %u <long response: %u bytes>", 906 MAC2STR(sa), update_indic, 907 (unsigned int) tlvs_len); 908 } else { 909 buf_len = 2 * tlvs_len + 1; 910 buf = os_malloc(buf_len); 911 if (buf) { 912 wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len); 913 wpa_msg_ctrl(wpa_s, MSG_INFO, 914 P2P_EVENT_SERV_DISC_RESP MACSTR " %u %s", 915 MAC2STR(sa), update_indic, buf); 916 os_free(buf); 917 } 918 } 919 920 while (end - pos >= 2) { 921 u8 srv_proto, srv_trans_id, status; 922 923 wpa_printf(MSG_DEBUG, "P2P: Service Response TLV"); 924 slen = WPA_GET_LE16(pos); 925 pos += 2; 926 if (slen > end - pos || slen < 3) { 927 wpa_printf(MSG_DEBUG, "P2P: Unexpected Response Data " 928 "length"); 929 return; 930 } 931 tlv_end = pos + slen; 932 933 srv_proto = *pos++; 934 wpa_printf(MSG_DEBUG, "P2P: Service Protocol Type %u", 935 srv_proto); 936 srv_trans_id = *pos++; 937 wpa_printf(MSG_DEBUG, "P2P: Service Transaction ID %u", 938 srv_trans_id); 939 status = *pos++; 940 wpa_printf(MSG_DEBUG, "P2P: Status Code ID %u", 941 status); 942 943 wpa_hexdump(MSG_MSGDUMP, "P2P: Response Data", 944 pos, tlv_end - pos); 945 946 if (srv_proto == P2P_SERV_P2PS && pos < tlv_end) { 947 wpas_sd_p2ps_serv_response(wpa_s, sa, srv_trans_id, 948 pos, tlv_end); 949 } 950 951 pos = tlv_end; 952 } 953 954 wpas_notify_p2p_sd_response(wpa_s, sa, update_indic, tlvs, tlvs_len); 955 } 956 957 958 u64 wpas_p2p_sd_request(struct wpa_supplicant *wpa_s, const u8 *dst, 959 const struct wpabuf *tlvs) 960 { 961 if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) 962 return 0; 963 return (uintptr_t) p2p_sd_request(wpa_s->global->p2p, dst, tlvs); 964 } 965 966 967 u64 wpas_p2p_sd_request_upnp(struct wpa_supplicant *wpa_s, const u8 *dst, 968 u8 version, const char *query) 969 { 970 struct wpabuf *tlvs; 971 u64 ret; 972 973 tlvs = wpabuf_alloc(2 + 1 + 1 + 1 + os_strlen(query)); 974 if (tlvs == NULL) 975 return 0; 976 wpabuf_put_le16(tlvs, 1 + 1 + 1 + os_strlen(query)); 977 wpabuf_put_u8(tlvs, P2P_SERV_UPNP); /* Service Protocol Type */ 978 wpabuf_put_u8(tlvs, 1); /* Service Transaction ID */ 979 wpabuf_put_u8(tlvs, version); 980 wpabuf_put_str(tlvs, query); 981 ret = wpas_p2p_sd_request(wpa_s, dst, tlvs); 982 wpabuf_free(tlvs); 983 return ret; 984 } 985 986 987 u64 wpas_p2p_sd_request_asp(struct wpa_supplicant *wpa_s, const u8 *dst, u8 id, 988 const char *svc_str, const char *info_substr) 989 { 990 struct wpabuf *tlvs; 991 size_t plen, svc_len, substr_len = 0; 992 u64 ret; 993 994 svc_len = os_strlen(svc_str); 995 if (info_substr) 996 substr_len = os_strlen(info_substr); 997 998 if (svc_len > 0xff || substr_len > 0xff) 999 return 0; 1000 1001 plen = 1 + 1 + 1 + svc_len + 1 + substr_len; 1002 tlvs = wpabuf_alloc(2 + plen); 1003 if (tlvs == NULL) 1004 return 0; 1005 1006 wpabuf_put_le16(tlvs, plen); 1007 wpabuf_put_u8(tlvs, P2P_SERV_P2PS); 1008 wpabuf_put_u8(tlvs, id); /* Service Transaction ID */ 1009 wpabuf_put_u8(tlvs, (u8) svc_len); /* Service String Length */ 1010 wpabuf_put_data(tlvs, svc_str, svc_len); 1011 wpabuf_put_u8(tlvs, (u8) substr_len); /* Info Substring Length */ 1012 wpabuf_put_data(tlvs, info_substr, substr_len); 1013 ret = wpas_p2p_sd_request(wpa_s, dst, tlvs); 1014 wpabuf_free(tlvs); 1015 1016 return ret; 1017 } 1018 1019 1020 #ifdef CONFIG_WIFI_DISPLAY 1021 1022 static u64 wpas_p2p_sd_request_wfd(struct wpa_supplicant *wpa_s, const u8 *dst, 1023 const struct wpabuf *tlvs) 1024 { 1025 if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) 1026 return 0; 1027 return (uintptr_t) p2p_sd_request_wfd(wpa_s->global->p2p, dst, tlvs); 1028 } 1029 1030 1031 #define MAX_WFD_SD_SUBELEMS 20 1032 1033 static void wfd_add_sd_req_role(struct wpabuf *tlvs, u8 id, u8 role, 1034 const char *subelems) 1035 { 1036 u8 *len; 1037 const char *pos; 1038 int val; 1039 int count = 0; 1040 1041 len = wpabuf_put(tlvs, 2); 1042 wpabuf_put_u8(tlvs, P2P_SERV_WIFI_DISPLAY); /* Service Protocol Type */ 1043 wpabuf_put_u8(tlvs, id); /* Service Transaction ID */ 1044 1045 wpabuf_put_u8(tlvs, role); 1046 1047 pos = subelems; 1048 while (*pos) { 1049 val = atoi(pos); 1050 if (val >= 0 && val < 256) { 1051 wpabuf_put_u8(tlvs, val); 1052 count++; 1053 if (count == MAX_WFD_SD_SUBELEMS) 1054 break; 1055 } 1056 pos = os_strchr(pos + 1, ','); 1057 if (pos == NULL) 1058 break; 1059 pos++; 1060 } 1061 1062 WPA_PUT_LE16(len, (u8 *) wpabuf_put(tlvs, 0) - len - 2); 1063 } 1064 1065 1066 u64 wpas_p2p_sd_request_wifi_display(struct wpa_supplicant *wpa_s, 1067 const u8 *dst, const char *role) 1068 { 1069 struct wpabuf *tlvs; 1070 u64 ret; 1071 const char *subelems; 1072 u8 id = 1; 1073 1074 subelems = os_strchr(role, ' '); 1075 if (subelems == NULL) 1076 return 0; 1077 subelems++; 1078 1079 tlvs = wpabuf_alloc(4 * (2 + 1 + 1 + 1 + MAX_WFD_SD_SUBELEMS)); 1080 if (tlvs == NULL) 1081 return 0; 1082 1083 if (os_strstr(role, "[source]")) 1084 wfd_add_sd_req_role(tlvs, id++, 0x00, subelems); 1085 if (os_strstr(role, "[pri-sink]")) 1086 wfd_add_sd_req_role(tlvs, id++, 0x01, subelems); 1087 if (os_strstr(role, "[sec-sink]")) 1088 wfd_add_sd_req_role(tlvs, id++, 0x02, subelems); 1089 if (os_strstr(role, "[source+sink]")) 1090 wfd_add_sd_req_role(tlvs, id++, 0x03, subelems); 1091 1092 ret = wpas_p2p_sd_request_wfd(wpa_s, dst, tlvs); 1093 wpabuf_free(tlvs); 1094 return ret; 1095 } 1096 1097 #endif /* CONFIG_WIFI_DISPLAY */ 1098 1099 1100 int wpas_p2p_sd_cancel_request(struct wpa_supplicant *wpa_s, u64 req) 1101 { 1102 if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) 1103 return -1; 1104 return p2p_sd_cancel_request(wpa_s->global->p2p, 1105 (void *) (uintptr_t) req); 1106 } 1107 1108 1109 void wpas_p2p_sd_response(struct wpa_supplicant *wpa_s, int freq, 1110 const u8 *dst, u8 dialog_token, 1111 const struct wpabuf *resp_tlvs) 1112 { 1113 if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) 1114 return; 1115 p2p_sd_response(wpa_s->global->p2p, freq, dst, dialog_token, 1116 resp_tlvs); 1117 } 1118 1119 1120 void wpas_p2p_sd_service_update(struct wpa_supplicant *wpa_s) 1121 { 1122 if (wpa_s->global->p2p) 1123 p2p_sd_service_update(wpa_s->global->p2p); 1124 } 1125 1126 1127 static void wpas_p2p_srv_bonjour_free(struct p2p_srv_bonjour *bsrv) 1128 { 1129 dl_list_del(&bsrv->list); 1130 wpabuf_free(bsrv->query); 1131 wpabuf_free(bsrv->resp); 1132 os_free(bsrv); 1133 } 1134 1135 1136 static void wpas_p2p_srv_upnp_free(struct p2p_srv_upnp *usrv) 1137 { 1138 dl_list_del(&usrv->list); 1139 os_free(usrv->service); 1140 os_free(usrv); 1141 } 1142 1143 1144 void wpas_p2p_service_flush(struct wpa_supplicant *wpa_s) 1145 { 1146 struct p2p_srv_bonjour *bsrv, *bn; 1147 struct p2p_srv_upnp *usrv, *un; 1148 1149 dl_list_for_each_safe(bsrv, bn, &wpa_s->global->p2p_srv_bonjour, 1150 struct p2p_srv_bonjour, list) 1151 wpas_p2p_srv_bonjour_free(bsrv); 1152 1153 dl_list_for_each_safe(usrv, un, &wpa_s->global->p2p_srv_upnp, 1154 struct p2p_srv_upnp, list) 1155 wpas_p2p_srv_upnp_free(usrv); 1156 1157 wpas_p2p_service_flush_asp(wpa_s); 1158 wpas_p2p_sd_service_update(wpa_s); 1159 } 1160 1161 1162 int wpas_p2p_service_p2ps_id_exists(struct wpa_supplicant *wpa_s, u32 adv_id) 1163 { 1164 if (adv_id == 0) 1165 return 1; 1166 1167 if (p2p_service_p2ps_id(wpa_s->global->p2p, adv_id)) 1168 return 1; 1169 1170 return 0; 1171 } 1172 1173 1174 int wpas_p2p_service_del_asp(struct wpa_supplicant *wpa_s, u32 adv_id) 1175 { 1176 int ret; 1177 1178 ret = p2p_service_del_asp(wpa_s->global->p2p, adv_id); 1179 if (ret == 0) 1180 wpas_p2p_sd_service_update(wpa_s); 1181 return ret; 1182 } 1183 1184 1185 int wpas_p2p_service_add_asp(struct wpa_supplicant *wpa_s, 1186 int auto_accept, u32 adv_id, 1187 const char *adv_str, u8 svc_state, 1188 u16 config_methods, const char *svc_info, 1189 const u8 *cpt_priority) 1190 { 1191 int ret; 1192 1193 ret = p2p_service_add_asp(wpa_s->global->p2p, auto_accept, adv_id, 1194 adv_str, svc_state, config_methods, 1195 svc_info, cpt_priority); 1196 if (ret == 0) 1197 wpas_p2p_sd_service_update(wpa_s); 1198 return ret; 1199 } 1200 1201 1202 void wpas_p2p_service_flush_asp(struct wpa_supplicant *wpa_s) 1203 { 1204 p2p_service_flush_asp(wpa_s->global->p2p); 1205 } 1206 1207 1208 int wpas_p2p_service_add_bonjour(struct wpa_supplicant *wpa_s, 1209 struct wpabuf *query, struct wpabuf *resp) 1210 { 1211 struct p2p_srv_bonjour *bsrv; 1212 1213 bsrv = os_zalloc(sizeof(*bsrv)); 1214 if (bsrv == NULL) 1215 return -1; 1216 bsrv->query = query; 1217 bsrv->resp = resp; 1218 dl_list_add(&wpa_s->global->p2p_srv_bonjour, &bsrv->list); 1219 1220 wpas_p2p_sd_service_update(wpa_s); 1221 return 0; 1222 } 1223 1224 1225 int wpas_p2p_service_del_bonjour(struct wpa_supplicant *wpa_s, 1226 const struct wpabuf *query) 1227 { 1228 struct p2p_srv_bonjour *bsrv; 1229 1230 bsrv = wpas_p2p_service_get_bonjour(wpa_s, query); 1231 if (bsrv == NULL) 1232 return -1; 1233 wpas_p2p_srv_bonjour_free(bsrv); 1234 wpas_p2p_sd_service_update(wpa_s); 1235 return 0; 1236 } 1237 1238 1239 int wpas_p2p_service_add_upnp(struct wpa_supplicant *wpa_s, u8 version, 1240 const char *service) 1241 { 1242 struct p2p_srv_upnp *usrv; 1243 1244 if (wpas_p2p_service_get_upnp(wpa_s, version, service)) 1245 return 0; /* Already listed */ 1246 usrv = os_zalloc(sizeof(*usrv)); 1247 if (usrv == NULL) 1248 return -1; 1249 usrv->version = version; 1250 usrv->service = os_strdup(service); 1251 if (usrv->service == NULL) { 1252 os_free(usrv); 1253 return -1; 1254 } 1255 dl_list_add(&wpa_s->global->p2p_srv_upnp, &usrv->list); 1256 1257 wpas_p2p_sd_service_update(wpa_s); 1258 return 0; 1259 } 1260 1261 1262 int wpas_p2p_service_del_upnp(struct wpa_supplicant *wpa_s, u8 version, 1263 const char *service) 1264 { 1265 struct p2p_srv_upnp *usrv; 1266 1267 usrv = wpas_p2p_service_get_upnp(wpa_s, version, service); 1268 if (usrv == NULL) 1269 return -1; 1270 wpas_p2p_srv_upnp_free(usrv); 1271 wpas_p2p_sd_service_update(wpa_s); 1272 return 0; 1273 } 1274
