src/p2p/p2p_sd.c

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