libeap/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 (pos + 3 <= end) {
  32                 subelem = *pos++;
  33                 len = WPA_GET_BE16(pos);
  34                 pos += 2;
  35                 if (pos + len > end)
  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
 292         p2p_dbg(p2p, "Start Service Discovery with " MACSTR,
 293                 MAC2STR(dev->info.p2p_device_addr));
 294
 295         req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs);
 296         if (req == NULL)
 297                 return -1;
 298
 299         dev->sd_reqs++;
 300         p2p->sd_peer = dev;
 301         p2p->sd_query = query;
 302         p2p->pending_action_state = P2P_PENDING_SD;
 303
 304         wait_time = 5000;
 305         if (p2p->cfg->max_listen && wait_time > p2p->cfg->max_listen)
 306                 wait_time = p2p->cfg->max_listen;
 307         if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr,
 308                             p2p->cfg->dev_addr, dev->info.p2p_device_addr,
 309                             wpabuf_head(req), wpabuf_len(req), wait_time) < 0) {
 310                 p2p_dbg(p2p, "Failed to send Action frame");
 311                 ret = -1;
 312         }
 313
 314         wpabuf_free(req);
 315
 316         return ret;
 317 }
 318
 319
 320 void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
 321                             const u8 *data, size_t len, int rx_freq)
 322 {
 323         const u8 *pos = data;
 324         const u8 *end = data + len;
 325         const u8 *next;
 326         u8 dialog_token;
 327         u16 slen;
 328         int freq;
 329         u16 update_indic;
 330
 331
 332         if (p2p->cfg->sd_request == NULL)
 333                 return;
 334
 335         if (rx_freq > 0)
 336                 freq = rx_freq;
 337         else
 338                 freq = p2p_channel_to_freq(p2p->cfg->reg_class,
 339                                            p2p->cfg->channel);
 340         if (freq < 0)
 341                 return;
 342
 343         if (len < 1 + 2)
 344                 return;
 345
 346         dialog_token = *pos++;
 347         p2p_dbg(p2p, "GAS Initial Request from " MACSTR
 348                 " (dialog token %u, freq %d)",
 349                 MAC2STR(sa), dialog_token, rx_freq);
 350
 351         if (*pos != WLAN_EID_ADV_PROTO) {
 352                 p2p_dbg(p2p, "Unexpected IE in GAS Initial Request: %u", *pos);
 353                 return;
 354         }
 355         pos++;
 356
 357         slen = *pos++;
 358         next = pos + slen;
 359         if (next > end || slen < 2) {
 360                 p2p_dbg(p2p, "Invalid IE in GAS Initial Request");
 361                 return;
 362         }
 363         pos++; /* skip QueryRespLenLimit and PAME-BI */
 364
 365         if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
 366                 p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
 367                         *pos);
 368                 return;
 369         }
 370
 371         pos = next;
 372         /* Query Request */
 373         if (pos + 2 > end)
 374                 return;
 375         slen = WPA_GET_LE16(pos);
 376         pos += 2;
 377         if (pos + slen > end)
 378                 return;
 379         end = pos + slen;
 380
 381         /* ANQP Query Request */
 382         if (pos + 4 > end)
 383                 return;
 384         if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
 385                 p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
 386                 return;
 387         }
 388         pos += 2;
 389
 390         slen = WPA_GET_LE16(pos);
 391         pos += 2;
 392         if (pos + slen > end || slen < 3 + 1) {
 393                 p2p_dbg(p2p, "Invalid ANQP Query Request length");
 394                 return;
 395         }
 396
 397         if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
 398                 p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
 399                         WPA_GET_BE32(pos));
 400                 return;
 401         }
 402         pos += 4;
 403
 404         if (pos + 2 > end)
 405                 return;
 406         update_indic = WPA_GET_LE16(pos);
 407         p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
 408         pos += 2;
 409
 410         p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token,
 411                              update_indic, pos, end - pos);
 412         /* the response will be indicated with a call to p2p_sd_response() */
 413 }
 414
 415
 416 void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
 417                      u8 dialog_token, const struct wpabuf *resp_tlvs)
 418 {
 419         struct wpabuf *resp;
 420
 421         /* TODO: fix the length limit to match with the maximum frame length */
 422         if (wpabuf_len(resp_tlvs) > 1400) {
 423                 p2p_dbg(p2p, "SD response long enough to require fragmentation");
 424                 if (p2p->sd_resp) {
 425                         /*
 426                          * TODO: Could consider storing the fragmented response
 427                          * separately for each peer to avoid having to drop old
 428                          * one if there is more than one pending SD query.
 429                          * Though, that would eat more memory, so there are
 430                          * also benefits to just using a single buffer.
 431                          */
 432                         p2p_dbg(p2p, "Drop previous SD response");
 433                         wpabuf_free(p2p->sd_resp);
 434                 }
 435                 p2p->sd_resp = wpabuf_dup(resp_tlvs);
 436                 if (p2p->sd_resp == NULL) {
 437                         p2p_err(p2p, "Failed to allocate SD response fragmentation area");
 438                         return;
 439                 }
 440                 os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
 441                 p2p->sd_resp_dialog_token = dialog_token;
 442                 p2p->sd_resp_pos = 0;
 443                 p2p->sd_frag_id = 0;
 444                 resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
 445                                              1, p2p->srv_update_indic, NULL);
 446         } else {
 447                 p2p_dbg(p2p, "SD response fits in initial response");
 448                 resp = p2p_build_sd_response(dialog_token,
 449                                              WLAN_STATUS_SUCCESS, 0,
 450                                              p2p->srv_update_indic, resp_tlvs);
 451         }
 452         if (resp == NULL)
 453                 return;
 454
 455         p2p->pending_action_state = P2P_NO_PENDING_ACTION;
 456         if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr,
 457                             p2p->cfg->dev_addr,
 458                             wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
 459                 p2p_dbg(p2p, "Failed to send Action frame");
 460
 461         wpabuf_free(resp);
 462 }
 463
 464
 465 void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
 466                              const u8 *data, size_t len, int rx_freq)
 467 {
 468         const u8 *pos = data;
 469         const u8 *end = data + len;
 470         const u8 *next;
 471         u8 dialog_token;
 472         u16 status_code;
 473         u16 comeback_delay;
 474         u16 slen;
 475         u16 update_indic;
 476
 477         if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
 478             os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
 479                 p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from "
 480                         MACSTR, MAC2STR(sa));
 481                 return;
 482         }
 483         p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
 484         p2p_clear_timeout(p2p);
 485
 486         p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)",
 487                 MAC2STR(sa), (int) len);
 488
 489         if (len < 5 + 2) {
 490                 p2p_dbg(p2p, "Too short GAS Initial Response frame");
 491                 return;
 492         }
 493
 494         dialog_token = *pos++;
 495         /* TODO: check dialog_token match */
 496         status_code = WPA_GET_LE16(pos);
 497         pos += 2;
 498         comeback_delay = WPA_GET_LE16(pos);
 499         pos += 2;
 500         p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u",
 501                 dialog_token, status_code, comeback_delay);
 502         if (status_code) {
 503                 p2p_dbg(p2p, "Service Discovery failed: status code %u",
 504                         status_code);
 505                 return;
 506         }
 507
 508         if (*pos != WLAN_EID_ADV_PROTO) {
 509                 p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos);
 510                 return;
 511         }
 512         pos++;
 513
 514         slen = *pos++;
 515         next = pos + slen;
 516         if (next > end || slen < 2) {
 517                 p2p_dbg(p2p, "Invalid IE in GAS Initial Response");
 518                 return;
 519         }
 520         pos++; /* skip QueryRespLenLimit and PAME-BI */
 521
 522         if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
 523                 p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
 524                         *pos);
 525                 return;
 526         }
 527
 528         pos = next;
 529         /* Query Response */
 530         if (pos + 2 > end) {
 531                 p2p_dbg(p2p, "Too short Query Response");
 532                 return;
 533         }
 534         slen = WPA_GET_LE16(pos);
 535         pos += 2;
 536         p2p_dbg(p2p, "Query Response Length: %d", slen);
 537         if (pos + slen > end) {
 538                 p2p_dbg(p2p, "Not enough Query Response data");
 539                 return;
 540         }
 541         end = pos + slen;
 542
 543         if (comeback_delay) {
 544                 p2p_dbg(p2p, "Fragmented response - request fragments");
 545                 if (p2p->sd_rx_resp) {
 546                         p2p_dbg(p2p, "Drop old SD reassembly buffer");
 547                         wpabuf_free(p2p->sd_rx_resp);
 548                         p2p->sd_rx_resp = NULL;
 549                 }
 550                 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
 551                 return;
 552         }
 553
 554         /* ANQP Query Response */
 555         if (pos + 4 > end)
 556                 return;
 557         if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
 558                 p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
 559                 return;
 560         }
 561         pos += 2;
 562
 563         slen = WPA_GET_LE16(pos);
 564         pos += 2;
 565         if (pos + slen > end || slen < 3 + 1) {
 566                 p2p_dbg(p2p, "Invalid ANQP Query Response length");
 567                 return;
 568         }
 569
 570         if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
 571                 p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
 572                         WPA_GET_BE32(pos));
 573                 return;
 574         }
 575         pos += 4;
 576
 577         if (pos + 2 > end)
 578                 return;
 579         update_indic = WPA_GET_LE16(pos);
 580         p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
 581         pos += 2;
 582
 583         p2p->sd_peer = NULL;
 584
 585         if (p2p->sd_query) {
 586                 if (!p2p->sd_query->for_all_peers) {
 587                         struct p2p_sd_query *q;
 588                         p2p_dbg(p2p, "Remove completed SD query %p",
 589                                 p2p->sd_query);
 590                         q = p2p->sd_query;
 591                         p2p_unlink_sd_query(p2p, p2p->sd_query);
 592                         p2p_free_sd_query(q);
 593                 }
 594                 p2p->sd_query = NULL;
 595         }
 596
 597         if (p2p->cfg->sd_response)
 598                 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
 599                                       pos, end - pos);
 600         p2p_continue_find(p2p);
 601 }
 602
 603
 604 void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
 605                              const u8 *data, size_t len, int rx_freq)
 606 {
 607         struct wpabuf *resp;
 608         u8 dialog_token;
 609         size_t frag_len;
 610         int more = 0;
 611
 612         wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
 613         if (len < 1)
 614                 return;
 615         dialog_token = *data;
 616         p2p_dbg(p2p, "Dialog Token: %u", dialog_token);
 617         if (dialog_token != p2p->sd_resp_dialog_token) {
 618                 p2p_dbg(p2p, "No pending SD response fragment for dialog token %u",
 619                         dialog_token);
 620                 return;
 621         }
 622
 623         if (p2p->sd_resp == NULL) {
 624                 p2p_dbg(p2p, "No pending SD response fragment available");
 625                 return;
 626         }
 627         if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
 628                 p2p_dbg(p2p, "No pending SD response fragment for " MACSTR,
 629                         MAC2STR(sa));
 630                 return;
 631         }
 632
 633         frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
 634         if (frag_len > 1400) {
 635                 frag_len = 1400;
 636                 more = 1;
 637         }
 638         resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
 639                                            p2p->srv_update_indic,
 640                                            wpabuf_head_u8(p2p->sd_resp) +
 641                                            p2p->sd_resp_pos, frag_len,
 642                                            p2p->sd_frag_id, more,
 643                                            wpabuf_len(p2p->sd_resp));
 644         if (resp == NULL)
 645                 return;
 646         p2p_dbg(p2p, "Send GAS Comeback Response (frag_id %d more=%d frag_len=%d)",
 647                 p2p->sd_frag_id, more, (int) frag_len);
 648         p2p->sd_frag_id++;
 649         p2p->sd_resp_pos += frag_len;
 650
 651         if (more) {
 652                 p2p_dbg(p2p, "%d more bytes remain to be sent",
 653                         (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
 654         } else {
 655                 p2p_dbg(p2p, "All fragments of SD response sent");
 656                 wpabuf_free(p2p->sd_resp);
 657                 p2p->sd_resp = NULL;
 658         }
 659
 660         p2p->pending_action_state = P2P_NO_PENDING_ACTION;
 661         if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr,
 662                             p2p->cfg->dev_addr,
 663                             wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
 664                 p2p_dbg(p2p, "Failed to send Action frame");
 665
 666         wpabuf_free(resp);
 667 }
 668
 669
 670 void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
 671                               const u8 *data, size_t len, int rx_freq)
 672 {
 673         const u8 *pos = data;
 674         const u8 *end = data + len;
 675         const u8 *next;
 676         u8 dialog_token;
 677         u16 status_code;
 678         u8 frag_id;
 679         u8 more_frags;
 680         u16 comeback_delay;
 681         u16 slen;
 682
 683         wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
 684
 685         if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
 686             os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
 687                 p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from "
 688                         MACSTR, MAC2STR(sa));
 689                 return;
 690         }
 691         p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
 692         p2p_clear_timeout(p2p);
 693
 694         p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)",
 695                 MAC2STR(sa), (int) len);
 696
 697         if (len < 6 + 2) {
 698                 p2p_dbg(p2p, "Too short GAS Comeback Response frame");
 699                 return;
 700         }
 701
 702         dialog_token = *pos++;
 703         /* TODO: check dialog_token match */
 704         status_code = WPA_GET_LE16(pos);
 705         pos += 2;
 706         frag_id = *pos & 0x7f;
 707         more_frags = (*pos & 0x80) >> 7;
 708         pos++;
 709         comeback_delay = WPA_GET_LE16(pos);
 710         pos += 2;
 711         p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
 712                 "comeback_delay=%u",
 713                 dialog_token, status_code, frag_id, more_frags,
 714                 comeback_delay);
 715         /* TODO: check frag_id match */
 716         if (status_code) {
 717                 p2p_dbg(p2p, "Service Discovery failed: status code %u",
 718                         status_code);
 719                 return;
 720         }
 721
 722         if (*pos != WLAN_EID_ADV_PROTO) {
 723                 p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u",
 724                         *pos);
 725                 return;
 726         }
 727         pos++;
 728
 729         slen = *pos++;
 730         next = pos + slen;
 731         if (next > end || slen < 2) {
 732                 p2p_dbg(p2p, "Invalid IE in GAS Comeback Response");
 733                 return;
 734         }
 735         pos++; /* skip QueryRespLenLimit and PAME-BI */
 736
 737         if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
 738                 p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
 739                         *pos);
 740                 return;
 741         }
 742
 743         pos = next;
 744         /* Query Response */
 745         if (pos + 2 > end) {
 746                 p2p_dbg(p2p, "Too short Query Response");
 747                 return;
 748         }
 749         slen = WPA_GET_LE16(pos);
 750         pos += 2;
 751         p2p_dbg(p2p, "Query Response Length: %d", slen);
 752         if (pos + slen > end) {
 753                 p2p_dbg(p2p, "Not enough Query Response data");
 754                 return;
 755         }
 756         if (slen == 0) {
 757                 p2p_dbg(p2p, "No Query Response data");
 758                 return;
 759         }
 760         end = pos + slen;
 761
 762         if (p2p->sd_rx_resp) {
 763                  /*
 764                   * ANQP header is only included in the first fragment; rest of
 765                   * the fragments start with continue TLVs.
 766                   */
 767                 goto skip_nqp_header;
 768         }
 769
 770         /* ANQP Query Response */
 771         if (pos + 4 > end)
 772                 return;
 773         if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
 774                 p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
 775                 return;
 776         }
 777         pos += 2;
 778
 779         slen = WPA_GET_LE16(pos);
 780         pos += 2;
 781         p2p_dbg(p2p, "ANQP Query Response length: %u", slen);
 782         if (slen < 3 + 1) {
 783                 p2p_dbg(p2p, "Invalid ANQP Query Response length");
 784                 return;
 785         }
 786         if (pos + 4 > end)
 787                 return;
 788
 789         if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
 790                 p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
 791                         WPA_GET_BE32(pos));
 792                 return;
 793         }
 794         pos += 4;
 795
 796         if (pos + 2 > end)
 797                 return;
 798         p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
 799         p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic);
 800         pos += 2;
 801
 802 skip_nqp_header:
 803         if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
 804                 return;
 805         wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
 806         p2p_dbg(p2p, "Current SD reassembly buffer length: %u",
 807                 (unsigned int) wpabuf_len(p2p->sd_rx_resp));
 808
 809         if (more_frags) {
 810                 p2p_dbg(p2p, "More fragments remains");
 811                 /* TODO: what would be a good size limit? */
 812                 if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
 813                         wpabuf_free(p2p->sd_rx_resp);
 814                         p2p->sd_rx_resp = NULL;
 815                         p2p_dbg(p2p, "Too long SD response - drop it");
 816                         return;
 817                 }
 818                 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
 819                 return;
 820         }
 821
 822         p2p->sd_peer = NULL;
 823
 824         if (p2p->sd_query) {
 825                 if (!p2p->sd_query->for_all_peers) {
 826                         struct p2p_sd_query *q;
 827                         p2p_dbg(p2p, "Remove completed SD query %p",
 828                                 p2p->sd_query);
 829                         q = p2p->sd_query;
 830                         p2p_unlink_sd_query(p2p, p2p->sd_query);
 831                         p2p_free_sd_query(q);
 832                 }
 833                 p2p->sd_query = NULL;
 834         }
 835
 836         if (p2p->cfg->sd_response)
 837                 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
 838                                       p2p->sd_rx_update_indic,
 839                                       wpabuf_head(p2p->sd_rx_resp),
 840                                       wpabuf_len(p2p->sd_rx_resp));
 841         wpabuf_free(p2p->sd_rx_resp);
 842         p2p->sd_rx_resp = NULL;
 843
 844         p2p_continue_find(p2p);
 845 }
 846
 847
 848 void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
 849                       const struct wpabuf *tlvs)
 850 {
 851         struct p2p_sd_query *q;
 852
 853         q = os_zalloc(sizeof(*q));
 854         if (q == NULL)
 855                 return NULL;
 856
 857         if (dst)
 858                 os_memcpy(q->peer, dst, ETH_ALEN);
 859         else
 860                 q->for_all_peers = 1;
 861
 862         q->tlvs = wpabuf_dup(tlvs);
 863         if (q->tlvs == NULL) {
 864                 p2p_free_sd_query(q);
 865                 return NULL;
 866         }
 867
 868         q->next = p2p->sd_queries;
 869         p2p->sd_queries = q;
 870         p2p_dbg(p2p, "Added SD Query %p", q);
 871
 872         if (dst == NULL) {
 873                 struct p2p_device *dev;
 874
 875                 p2p->num_p2p_sd_queries++;
 876
 877                 /* Update all the devices for the newly added broadcast query */
 878                 dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) {
 879                         if (dev->sd_pending_bcast_queries <= 0)
 880                                 dev->sd_pending_bcast_queries = 1;
 881                         else
 882                                 dev->sd_pending_bcast_queries++;
 883                 }
 884         }
 885
 886         return q;
 887 }
 888
 889
 890 #ifdef CONFIG_WIFI_DISPLAY
 891 void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
 892                           const struct wpabuf *tlvs)
 893 {
 894         struct p2p_sd_query *q;
 895         q = p2p_sd_request(p2p, dst, tlvs);
 896         if (q)
 897                 q->wsd = 1;
 898         return q;
 899 }
 900 #endif /* CONFIG_WIFI_DISPLAY */
 901
 902
 903 void p2p_sd_service_update(struct p2p_data *p2p)
 904 {
 905         p2p->srv_update_indic++;
 906 }
 907
 908
 909 int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
 910 {
 911         if (p2p_unlink_sd_query(p2p, req)) {
 912                 p2p_dbg(p2p, "Cancel pending SD query %p", req);
 913                 p2p_free_sd_query(req);
 914                 return 0;
 915         }
 916         return -1;
 917 }