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