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