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