src/p2p/p2p_sd.c

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