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