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