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_sd_response(u8 dialog_token, u16 status_code,
 127                                              u16 update_indic,
 128                                              const struct wpabuf *tlvs)
 129 {
 130         struct wpabuf *buf;
 131         u8 *len_pos, *len_pos2;
 132
 133         buf = wpabuf_alloc(1000 + wpabuf_len(tlvs));
 134         if (buf == NULL)
 135                 return NULL;
 136
 137         wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC);
 138         wpabuf_put_u8(buf, WLAN_PA_GAS_INITIAL_RESP);
 139         wpabuf_put_u8(buf, dialog_token);
 140         wpabuf_put_le16(buf, status_code);
 141         wpabuf_put_le16(buf, 0); /* GAS Comeback Delay */
 142
 143         /* Advertisement Protocol IE */
 144         wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO);
 145         wpabuf_put_u8(buf, 2); /* Length */
 146         wpabuf_put_u8(buf, 0x7f); /* QueryRespLenLimit | PAME-BI */
 147         wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */
 148
 149         /* Query Response */
 150         len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */
 151
 152         /* NQP Query Response Frame */
 153         wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */
 154         len_pos2 = wpabuf_put(buf, 2); /* Length (to be filled) */
 155         wpabuf_put_be24(buf, OUI_WFA);
 156         wpabuf_put_u8(buf, P2P_OUI_TYPE);
 157         wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
 158         wpabuf_put_buf(buf, tlvs);
 159
 160         WPA_PUT_LE16(len_pos2, (u8 *) wpabuf_put(buf, 0) - len_pos2 - 2);
 161         WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2);
 162
 163         return buf;
 164 }
 165
 166
 167 int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev)
 168 {
 169         struct wpabuf *req;
 170         int ret = 0;
 171         struct p2p_sd_query *query;
 172         int freq;
 173
 174         freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq;
 175         if (freq <= 0) {
 176                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 177                         "P2P: No Listen/Operating frequency known for the "
 178                         "peer " MACSTR " to send SD Request",
 179                         MAC2STR(dev->p2p_device_addr));
 180                 return -1;
 181         }
 182
 183         query = p2p_pending_sd_req(p2p, dev);
 184         if (query == NULL)
 185                 return -1;
 186
 187         wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 188                 "P2P: Start Service Discovery with " MACSTR,
 189                 MAC2STR(dev->p2p_device_addr));
 190
 191         req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs);
 192         if (req == NULL)
 193                 return -1;
 194
 195         p2p->sd_peer = dev;
 196         p2p->sd_query = query;
 197         p2p->pending_action_state = P2P_PENDING_SD;
 198
 199         if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq,
 200                                   dev->p2p_device_addr, p2p->cfg->dev_addr,
 201                                   dev->p2p_device_addr,
 202                                   wpabuf_head(req), wpabuf_len(req), 5000) < 0)
 203         {
 204                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 205                         "P2P: Failed to send Action frame");
 206                 ret = -1;
 207         }
 208
 209         wpabuf_free(req);
 210
 211         return ret;
 212 }
 213
 214
 215 void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
 216                             const u8 *data, size_t len, int rx_freq)
 217 {
 218         const u8 *pos = data;
 219         const u8 *end = data + len;
 220         const u8 *next;
 221         u8 dialog_token;
 222         u16 slen;
 223         int freq;
 224         u16 update_indic;
 225
 226
 227         if (p2p->cfg->sd_request == NULL)
 228                 return;
 229
 230         if (rx_freq > 0)
 231                 freq = rx_freq;
 232         else
 233                 freq = p2p_channel_to_freq(p2p->cfg->country,
 234                                            p2p->cfg->reg_class,
 235                                            p2p->cfg->channel);
 236         if (freq < 0)
 237                 return;
 238
 239         if (len < 1 + 2)
 240                 return;
 241
 242         dialog_token = *pos++;
 243         wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 244                 "P2P: GAS Initial Request from " MACSTR " (dialog token %u, "
 245                 "freq %d)",
 246                 MAC2STR(sa), dialog_token, rx_freq);
 247
 248         if (*pos != WLAN_EID_ADV_PROTO) {
 249                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 250                         "P2P: Unexpected IE in GAS Initial Request: %u", *pos);
 251                 return;
 252         }
 253         pos++;
 254
 255         slen = *pos++;
 256         next = pos + slen;
 257         if (next > end || slen < 2) {
 258                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 259                         "P2P: Invalid IE in GAS Initial Request");
 260                 return;
 261         }
 262         pos++; /* skip QueryRespLenLimit and PAME-BI */
 263
 264         if (*pos != NATIVE_QUERY_PROTOCOL) {
 265                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 266                         "P2P: Unsupported GAS advertisement protocol id %u",
 267                         *pos);
 268                 return;
 269         }
 270
 271         pos = next;
 272         /* Query Request */
 273         if (pos + 2 > end)
 274                 return;
 275         slen = WPA_GET_LE16(pos);
 276         pos += 2;
 277         if (pos + slen > end)
 278                 return;
 279         end = pos + slen;
 280
 281         /* NQP Query Request */
 282         if (pos + 4 > end)
 283                 return;
 284         if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) {
 285                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 286                         "P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos));
 287                 return;
 288         }
 289         pos += 2;
 290
 291         slen = WPA_GET_LE16(pos);
 292         pos += 2;
 293         if (pos + slen > end || slen < 3 + 1) {
 294                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 295                         "P2P: Invalid NQP Query Request length");
 296                 return;
 297         }
 298
 299         if (WPA_GET_BE24(pos) != OUI_WFA) {
 300                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 301                         "P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos));
 302                 return;
 303         }
 304         pos += 3;
 305
 306         if (*pos != P2P_OUI_TYPE) {
 307                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 308                         "P2P: Unsupported NQP vendor type %u", *pos);
 309                 return;
 310         }
 311         pos++;
 312
 313         if (pos + 2 > end)
 314                 return;
 315         update_indic = WPA_GET_LE16(pos);
 316         wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 317                 "P2P: Service Update Indicator: %u", update_indic);
 318         pos += 2;
 319
 320         p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token,
 321                              update_indic, pos, end - pos);
 322         /* the response will be indicated with a call to p2p_sd_response() */
 323 }
 324
 325
 326 void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
 327                      u8 dialog_token, const struct wpabuf *resp_tlvs)
 328 {
 329         struct wpabuf *resp;
 330         resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
 331                                      p2p->srv_update_indic, resp_tlvs);
 332         if (resp == NULL)
 333                 return;
 334
 335         p2p->pending_action_state = P2P_NO_PENDING_ACTION;
 336         if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq,
 337                                   dst, p2p->cfg->dev_addr, p2p->cfg->dev_addr,
 338                                   wpabuf_head(resp), wpabuf_len(resp), 200) <
 339             0)
 340                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 341                         "P2P: Failed to send Action frame");
 342
 343         wpabuf_free(resp);
 344 }
 345
 346
 347 void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
 348                              const u8 *data, size_t len)
 349 {
 350         const u8 *pos = data;
 351         const u8 *end = data + len;
 352         const u8 *next;
 353         u8 dialog_token;
 354         u16 status_code;
 355         u16 comeback_delay;
 356         u16 slen;
 357         u16 update_indic;
 358
 359         if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
 360             os_memcmp(sa, p2p->sd_peer->p2p_device_addr, ETH_ALEN) != 0) {
 361                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 362                         "P2P: Ignore unexpected GAS Initial Response from "
 363                         MACSTR, MAC2STR(sa));
 364                 return;
 365         }
 366         p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
 367         p2p_clear_timeout(p2p);
 368
 369         wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 370                 "P2P: Received GAS Initial Response from " MACSTR,
 371                 MAC2STR(sa));
 372
 373         if (len < 5 + 2)
 374                 return;
 375
 376         dialog_token = *pos++;
 377         /* TODO: check dialog_token match */
 378         status_code = WPA_GET_LE16(pos);
 379         pos += 2;
 380         comeback_delay = WPA_GET_LE16(pos);
 381         pos += 2;
 382         wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 383                 "P2P: dialog_token=%u status_code=%u comeback_delay=%u",
 384                 dialog_token, status_code, comeback_delay);
 385
 386         if (*pos != WLAN_EID_ADV_PROTO) {
 387                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 388                         "P2P: Unexpected IE in GAS Initial Response: %u",
 389                         *pos);
 390                 return;
 391         }
 392         pos++;
 393
 394         slen = *pos++;
 395         next = pos + slen;
 396         if (next > end || slen < 2) {
 397                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 398                         "P2P: Invalid IE in GAS Initial Response");
 399                 return;
 400         }
 401         pos++; /* skip QueryRespLenLimit and PAME-BI */
 402
 403         if (*pos != NATIVE_QUERY_PROTOCOL) {
 404                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 405                         "P2P: Unsupported GAS advertisement protocol id %u",
 406                         *pos);
 407                 return;
 408         }
 409
 410         pos = next;
 411         /* Query Response */
 412         if (pos + 2 > end)
 413                 return;
 414         slen = WPA_GET_LE16(pos);
 415         pos += 2;
 416         if (pos + slen > end)
 417                 return;
 418         end = pos + slen;
 419
 420         /* NQP Query Response */
 421         if (pos + 4 > end)
 422                 return;
 423         if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) {
 424                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 425                         "P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos));
 426                 return;
 427         }
 428         pos += 2;
 429
 430         slen = WPA_GET_LE16(pos);
 431         pos += 2;
 432         if (pos + slen > end || slen < 3 + 1) {
 433                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 434                         "P2P: Invalid NQP Query Response length");
 435                 return;
 436         }
 437
 438         if (WPA_GET_BE24(pos) != OUI_WFA) {
 439                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 440                         "P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos));
 441                 return;
 442         }
 443         pos += 3;
 444
 445         if (*pos != P2P_OUI_TYPE) {
 446                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 447                         "P2P: Unsupported NQP vendor type %u", *pos);
 448                 return;
 449         }
 450         pos++;
 451
 452         if (pos + 2 > end)
 453                 return;
 454         update_indic = WPA_GET_LE16(pos);
 455         wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 456                 "P2P: Service Update Indicator: %u", update_indic);
 457         pos += 2;
 458
 459         p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
 460         p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
 461         p2p->sd_peer = NULL;
 462
 463         if (p2p->sd_query) {
 464                 if (!p2p->sd_query->for_all_peers) {
 465                         struct p2p_sd_query *q;
 466                         wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 467                                 "P2P: Remove completed SD query %p",
 468                                 p2p->sd_query);
 469                         q = p2p->sd_query;
 470                         p2p_unlink_sd_query(p2p, p2p->sd_query);
 471                         p2p_free_sd_query(q);
 472                 }
 473                 p2p->sd_query = NULL;
 474         }
 475
 476         if (p2p->cfg->sd_response)
 477                 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
 478                                       pos, end - pos);
 479         p2p_continue_find(p2p);
 480 }
 481
 482
 483 void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
 484                       const struct wpabuf *tlvs)
 485 {
 486         struct p2p_sd_query *q;
 487
 488         q = os_zalloc(sizeof(*q));
 489         if (q == NULL)
 490                 return NULL;
 491
 492         if (dst)
 493                 os_memcpy(q->peer, dst, ETH_ALEN);
 494         else
 495                 q->for_all_peers = 1;
 496
 497         q->tlvs = wpabuf_dup(tlvs);
 498         if (q->tlvs == NULL) {
 499                 p2p_free_sd_query(q);
 500                 return NULL;
 501         }
 502
 503         q->next = p2p->sd_queries;
 504         p2p->sd_queries = q;
 505         wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p", q);
 506
 507         return q;
 508 }
 509
 510
 511 void p2p_sd_service_update(struct p2p_data *p2p)
 512 {
 513         p2p->srv_update_indic++;
 514 }
 515
 516
 517 int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
 518 {
 519         if (p2p_unlink_sd_query(p2p, req)) {
 520                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 521                         "P2P: Cancel pending SD query %p", req);
 522                 p2p_free_sd_query(req);
 523                 return 0;
 524         }
 525         return -1;
 526 }