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