wpa_supplicant/p2p_supplicant_sd.c

   1 /*
   2  * wpa_supplicant - P2P service discovery
   3  * Copyright (c) 2009-2010, Atheros Communications
   4  * Copyright (c) 2010-2014, Jouni Malinen <j@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 }