src/p2p/p2p_group.c

   1 /*
   2  * Wi-Fi Direct - P2P group operations
   3  * Copyright (c) 2009-2010, Atheros Communications
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License version 2 as
   7  * published by the Free Software Foundation.
   8  *
   9  * Alternatively, this software may be distributed under the terms of BSD
  10  * license.
  11  *
  12  * See README and COPYING for more details.
  13  */
  14
  15 #include "includes.h"
  16
  17 #include "common.h"
  18 #include "common/ieee802_11_defs.h"
  19 #include "common/ieee802_11_common.h"
  20 #include "wps/wps_defs.h"
  21 #include "wps/wps_i.h"
  22 #include "p2p_i.h"
  23 #include "p2p.h"
  24
  25
  26 struct p2p_group_member {
  27         struct p2p_group_member *next;
  28         u8 addr[ETH_ALEN]; /* P2P Interface Address */
  29         u8 dev_addr[ETH_ALEN]; /* P2P Device Address */
  30         struct wpabuf *p2p_ie;
  31         struct wpabuf *client_info;
  32         u8 dev_capab;
  33 };
  34
  35 /**
  36  * struct p2p_group - Internal P2P module per-group data
  37  */
  38 struct p2p_group {
  39         struct p2p_data *p2p;
  40         struct p2p_group_config *cfg;
  41         struct p2p_group_member *members;
  42         int group_formation;
  43         int beacon_update;
  44         struct wpabuf *noa;
  45 };
  46
  47
  48 static void p2p_group_update_ies(struct p2p_group *group);
  49
  50
  51 struct p2p_group * p2p_group_init(struct p2p_data *p2p,
  52                                   struct p2p_group_config *config)
  53 {
  54         struct p2p_group *group, **groups;
  55
  56         group = os_zalloc(sizeof(*group));
  57         if (group == NULL)
  58                 return NULL;
  59
  60         groups = os_realloc(p2p->groups, (p2p->num_groups + 1) *
  61                             sizeof(struct p2p_group *));
  62         if (groups == NULL) {
  63                 os_free(group);
  64                 return NULL;
  65         }
  66         groups[p2p->num_groups++] = group;
  67         p2p->groups = groups;
  68
  69         group->p2p = p2p;
  70         group->cfg = config;
  71         group->group_formation = 1;
  72         group->beacon_update = 1;
  73         p2p_group_update_ies(group);
  74
  75         return group;
  76 }
  77
  78
  79 static void p2p_group_free_member(struct p2p_group_member *m)
  80 {
  81         wpabuf_free(m->p2p_ie);
  82         wpabuf_free(m->client_info);
  83         os_free(m);
  84 }
  85
  86
  87 static void p2p_group_free_members(struct p2p_group *group)
  88 {
  89         struct p2p_group_member *m, *prev;
  90         m = group->members;
  91         group->members = NULL;
  92         while (m) {
  93                 prev = m;
  94                 m = m->next;
  95                 p2p_group_free_member(prev);
  96         }
  97 }
  98
  99
 100 void p2p_group_deinit(struct p2p_group *group)
 101 {
 102         size_t g;
 103         struct p2p_data *p2p = group->p2p;
 104
 105         if (group == NULL)
 106                 return;
 107
 108         for (g = 0; g < p2p->num_groups; g++) {
 109                 if (p2p->groups[g] == group) {
 110                         while (g + 1 < p2p->num_groups) {
 111                                 p2p->groups[g] = p2p->groups[g + 1];
 112                                 g++;
 113                         }
 114                         p2p->num_groups--;
 115                         break;
 116                 }
 117         }
 118
 119         p2p_group_free_members(group);
 120         os_free(group->cfg);
 121         wpabuf_free(group->noa);
 122         os_free(group);
 123 }
 124
 125
 126 static void p2p_client_info(struct wpabuf *ie, struct p2p_group_member *m)
 127 {
 128         if (wpabuf_tailroom(ie) < wpabuf_len(m->client_info) + 1)
 129                 return;
 130         wpabuf_put_buf(ie, m->client_info);
 131 }
 132
 133
 134 static void p2p_group_add_common_ies(struct p2p_group *group,
 135                                      struct wpabuf *ie)
 136 {
 137         u8 dev_capab = 0, group_capab = 0;
 138
 139         /* P2P Capability */
 140         dev_capab |= P2P_DEV_CAPAB_SERVICE_DISCOVERY;
 141         dev_capab |= P2P_DEV_CAPAB_INVITATION_PROCEDURE;
 142         group_capab |= P2P_GROUP_CAPAB_GROUP_OWNER;
 143         if (group->cfg->persistent_group)
 144                 group_capab |= P2P_GROUP_CAPAB_PERSISTENT_GROUP;
 145         group_capab |= P2P_GROUP_CAPAB_INTRA_BSS_DIST;
 146         if (group->group_formation)
 147                 group_capab |= P2P_GROUP_CAPAB_GROUP_FORMATION;
 148         p2p_buf_add_capability(ie, dev_capab, group_capab);
 149 }
 150
 151
 152 static void p2p_group_add_noa(struct wpabuf *ie, struct wpabuf *noa)
 153 {
 154         if (noa == NULL)
 155                 return;
 156         /* Notice of Absence */
 157         wpabuf_put_u8(ie, P2P_ATTR_NOTICE_OF_ABSENCE);
 158         wpabuf_put_le16(ie, wpabuf_len(noa));
 159         wpabuf_put_buf(ie, noa);
 160 }
 161
 162
 163 static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group)
 164 {
 165         struct wpabuf *ie;
 166         u8 *len;
 167
 168         ie = wpabuf_alloc(257);
 169         if (ie == NULL)
 170                 return NULL;
 171
 172         len = p2p_buf_add_ie_hdr(ie);
 173         p2p_group_add_common_ies(group, ie);
 174         p2p_buf_add_device_id(ie, group->p2p->cfg->dev_addr);
 175         p2p_group_add_noa(ie, group->noa);
 176         p2p_buf_update_ie_hdr(ie, len);
 177
 178         return ie;
 179 }
 180
 181
 182 static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group)
 183 {
 184         u8 *group_info;
 185         struct wpabuf *ie;
 186         struct p2p_group_member *m;
 187         u8 *len;
 188
 189         ie = wpabuf_alloc(257);
 190         if (ie == NULL)
 191                 return NULL;
 192
 193         len = p2p_buf_add_ie_hdr(ie);
 194
 195         p2p_group_add_common_ies(group, ie);
 196         p2p_group_add_noa(ie, group->noa);
 197
 198         /* P2P Device Info */
 199         p2p_buf_add_device_info(ie, group->p2p, NULL);
 200
 201         if (group->members) {
 202                 /* P2P Group Info */
 203                 group_info = wpabuf_put(ie, 0);
 204                 wpabuf_put_u8(ie, P2P_ATTR_GROUP_INFO);
 205                 wpabuf_put_le16(ie, 0); /* Length to be filled */
 206                 for (m = group->members; m; m = m->next)
 207                         p2p_client_info(ie, m);
 208                 WPA_PUT_LE16(group_info + 1,
 209                              (u8 *) wpabuf_put(ie, 0) - group_info - 3);
 210         }
 211
 212         p2p_buf_update_ie_hdr(ie, len);
 213         return ie;
 214 }
 215
 216
 217 static void p2p_group_update_ies(struct p2p_group *group)
 218 {
 219         struct wpabuf *beacon_ie;
 220         struct wpabuf *probe_resp_ie;
 221
 222         probe_resp_ie = p2p_group_build_probe_resp_ie(group);
 223         if (probe_resp_ie == NULL)
 224                 return;
 225         wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Probe Response P2P IE",
 226                         probe_resp_ie);
 227
 228         if (group->beacon_update) {
 229                 beacon_ie = p2p_group_build_beacon_ie(group);
 230                 if (beacon_ie)
 231                         group->beacon_update = 0;
 232                 wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Beacon P2P IE",
 233                                 beacon_ie);
 234         } else
 235                 beacon_ie = NULL;
 236
 237         group->cfg->ie_update(group->cfg->cb_ctx, beacon_ie, probe_resp_ie);
 238 }
 239
 240
 241 /**
 242  * p2p_build_client_info - Build P2P Client Info Descriptor
 243  * @addr: MAC address of the peer device
 244  * @p2p_ie: P2P IE from (Re)Association Request
 245  * @dev_capab: Buffer for returning Device Capability
 246  * @dev_addr: Buffer for returning P2P Device Address
 247  * Returns: P2P Client Info Descriptor or %NULL on failure
 248  *
 249  * This function builds P2P Client Info Descriptor based on the information
 250  * available from (Re)Association Request frame. Group owner can use this to
 251  * build the P2P Group Info attribute for Probe Response frames.
 252  */
 253 static struct wpabuf * p2p_build_client_info(const u8 *addr,
 254                                              struct wpabuf *p2p_ie,
 255                                              u8 *dev_capab, u8 *dev_addr)
 256 {
 257         const u8 *spos;
 258         struct p2p_message msg;
 259         u8 *len_pos;
 260         struct wpabuf *buf;
 261
 262         if (p2p_ie == NULL)
 263                 return NULL;
 264
 265         os_memset(&msg, 0, sizeof(msg));
 266         if (p2p_parse_p2p_ie(p2p_ie, &msg) ||
 267             msg.capability == NULL || msg.p2p_device_info == NULL)
 268                 return NULL;
 269
 270         buf = wpabuf_alloc(ETH_ALEN + 1 + 1 + msg.p2p_device_info_len);
 271         if (buf == NULL)
 272                 return NULL;
 273
 274         *dev_capab = msg.capability[0];
 275         os_memcpy(dev_addr, msg.p2p_device_addr, ETH_ALEN);
 276
 277         spos = msg.p2p_device_info; /* P2P Device address */
 278
 279         /* P2P Client Info Descriptor */
 280         /* Length to be set */
 281         len_pos = wpabuf_put(buf, 1);
 282         /* P2P Device address */
 283         wpabuf_put_data(buf, spos, ETH_ALEN);
 284         /* P2P Interface address */
 285         wpabuf_put_data(buf, addr, ETH_ALEN);
 286         /* Device Capability Bitmap */
 287         wpabuf_put_u8(buf, msg.capability[0]);
 288         /*
 289          * Config Methods, Primary Device Type, Number of Secondary Device
 290          * Types, Secondary Device Type List, Device Name copied from
 291          * Device Info
 292          */
 293         wpabuf_put_data(buf, spos + ETH_ALEN,
 294                         msg.p2p_device_info_len - ETH_ALEN);
 295
 296         *len_pos = wpabuf_len(buf) - 1;
 297
 298
 299         return buf;
 300 }
 301
 302
 303 int p2p_group_notif_assoc(struct p2p_group *group, const u8 *addr,
 304                           const u8 *ie, size_t len)
 305 {
 306         struct p2p_group_member *m;
 307
 308         if (group == NULL)
 309                 return -1;
 310
 311         m = os_zalloc(sizeof(*m));
 312         if (m == NULL)
 313                 return -1;
 314         os_memcpy(m->addr, addr, ETH_ALEN);
 315         m->p2p_ie = ieee802_11_vendor_ie_concat(ie, len, P2P_IE_VENDOR_TYPE);
 316         if (m->p2p_ie == NULL) {
 317                 p2p_group_free_member(m);
 318                 return -1;
 319         }
 320
 321         m->client_info = p2p_build_client_info(addr, m->p2p_ie, &m->dev_capab,
 322                                                m->dev_addr);
 323         if (m->client_info == NULL) {
 324                 p2p_group_free_member(m);
 325                 return -1;
 326         }
 327
 328         m->next = group->members;
 329         group->members = m;
 330
 331         p2p_group_update_ies(group);
 332
 333         return 0;
 334 }
 335
 336
 337 struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status)
 338 {
 339         struct wpabuf *resp;
 340         u8 *rlen;
 341
 342         /*
 343          * (Re)Association Response - P2P IE
 344          * Status attribute (shall be present when association request is
 345          *      denied)
 346          * Extended Listen Timing (may be present)
 347          */
 348         resp = wpabuf_alloc(20);
 349         if (resp == NULL)
 350                 return NULL;
 351         rlen = p2p_buf_add_ie_hdr(resp);
 352         if (status != P2P_SC_SUCCESS)
 353                 p2p_buf_add_status(resp, status);
 354         p2p_buf_update_ie_hdr(resp, rlen);
 355
 356         return resp;
 357 }
 358
 359
 360 void p2p_group_notif_disassoc(struct p2p_group *group, const u8 *addr)
 361 {
 362         struct p2p_group_member *m, *prev;
 363
 364         if (group == NULL)
 365                 return;
 366
 367         m = group->members;
 368         prev = NULL;
 369         while (m) {
 370                 if (os_memcmp(m->addr, addr, ETH_ALEN) == 0)
 371                         break;
 372                 prev = m;
 373                 m = m->next;
 374         }
 375
 376         if (m) {
 377                 if (prev)
 378                         prev->next = m->next;
 379                 else
 380                         group->members = m->next;
 381                 p2p_group_free_member(m);
 382                 p2p_group_update_ies(group);
 383         }
 384 }
 385
 386
 387 /**
 388  * p2p_match_dev_type_member - Match client device type with requested type
 389  * @m: Group member
 390  * @wps: WPS TLVs from Probe Request frame (concatenated WPS IEs)
 391  * Returns: 1 on match, 0 on mismatch
 392  *
 393  * This function can be used to match the Requested Device Type attribute in
 394  * WPS IE with the device types of a group member for deciding whether a GO
 395  * should reply to a Probe Request frame.
 396  */
 397 static int p2p_match_dev_type_member(struct p2p_group_member *m,
 398                                      struct wpabuf *wps)
 399 {
 400         const u8 *pos, *end;
 401         struct wps_parse_attr attr;
 402         u8 num_sec;
 403
 404         if (m->client_info == NULL || wps == NULL)
 405                 return 0;
 406
 407         pos = wpabuf_head(m->client_info);
 408         end = pos + wpabuf_len(m->client_info);
 409
 410         pos += 1 + 2 * ETH_ALEN + 1 + 2;
 411         if (end - pos < WPS_DEV_TYPE_LEN + 1)
 412                 return 0;
 413
 414         if (wps_parse_msg(wps, &attr))
 415                 return 1; /* assume no Requested Device Type attributes */
 416
 417         if (attr.num_req_dev_type == 0)
 418                 return 1; /* no Requested Device Type attributes -> match */
 419
 420         if (dev_type_list_match(pos, attr.req_dev_type, attr.num_req_dev_type))
 421                 return 1; /* Match with client Primary Device Type */
 422
 423         pos += WPS_DEV_TYPE_LEN;
 424         num_sec = *pos++;
 425         if (end - pos < num_sec * WPS_DEV_TYPE_LEN)
 426                 return 0;
 427         while (num_sec > 0) {
 428                 num_sec--;
 429                 if (dev_type_list_match(pos, attr.req_dev_type,
 430                                         attr.num_req_dev_type))
 431                         return 1; /* Match with client Secondary Device Type */
 432                 pos += WPS_DEV_TYPE_LEN;
 433         }
 434
 435         /* No matching device type found */
 436         return 0;
 437 }
 438
 439
 440 int p2p_group_match_dev_type(struct p2p_group *group, struct wpabuf *wps)
 441 {
 442         struct p2p_group_member *m;
 443
 444         if (p2p_match_dev_type(group->p2p, wps))
 445                 return 1; /* Match with own device type */
 446
 447         for (m = group->members; m; m = m->next) {
 448                 if (p2p_match_dev_type_member(m, wps))
 449                         return 1; /* Match with group client device type */
 450         }
 451
 452         /* No match with Requested Device Type */
 453         return 0;
 454 }
 455
 456
 457 void p2p_group_notif_formation_done(struct p2p_group *group)
 458 {
 459         if (group == NULL)
 460                 return;
 461         group->group_formation = 0;
 462         group->beacon_update = 1;
 463         p2p_group_update_ies(group);
 464 }
 465
 466
 467 int p2p_group_notif_noa(struct p2p_group *group, const u8 *noa,
 468                         size_t noa_len)
 469 {
 470         if (noa == NULL) {
 471                 wpabuf_free(group->noa);
 472                 group->noa = NULL;
 473         } else {
 474                 if (group->noa) {
 475                         if (wpabuf_size(group->noa) >= noa_len) {
 476                                 group->noa->size = 0;
 477                                 wpabuf_put_data(group->noa, noa, noa_len);
 478                         } else {
 479                                 wpabuf_free(group->noa);
 480                                 group->noa = NULL;
 481                         }
 482                 }
 483
 484                 if (!group->noa) {
 485                         group->noa = wpabuf_alloc_copy(noa, noa_len);
 486                         if (group->noa == NULL)
 487                                 return -1;
 488                 }
 489         }
 490
 491         group->beacon_update = 1;
 492         p2p_group_update_ies(group);
 493         return 0;
 494 }
 495
 496
 497 static struct p2p_group_member * p2p_group_get_client(struct p2p_group *group,
 498                                                       const u8 *dev_id)
 499 {
 500         struct p2p_group_member *m;
 501
 502         for (m = group->members; m; m = m->next) {
 503                 if (os_memcmp(dev_id, m->dev_addr, ETH_ALEN) == 0)
 504                         return m;
 505         }
 506
 507         return NULL;
 508 }
 509
 510
 511 static struct p2p_group_member * p2p_group_get_client_iface(
 512         struct p2p_group *group, const u8 *interface_addr)
 513 {
 514         struct p2p_group_member *m;
 515
 516         for (m = group->members; m; m = m->next) {
 517                 if (os_memcmp(interface_addr, m->addr, ETH_ALEN) == 0)
 518                         return m;
 519         }
 520
 521         return NULL;
 522 }
 523
 524
 525 static struct wpabuf * p2p_build_go_disc_req(void)
 526 {
 527         struct wpabuf *buf;
 528
 529         buf = wpabuf_alloc(100);
 530         if (buf == NULL)
 531                 return NULL;
 532
 533         p2p_buf_add_action_hdr(buf, P2P_GO_DISC_REQ, 0);
 534
 535         return buf;
 536 }
 537
 538
 539 int p2p_group_go_discover(struct p2p_group *group, const u8 *dev_id,
 540                           const u8 *searching_dev, int rx_freq)
 541 {
 542         struct p2p_group_member *m;
 543         struct wpabuf *req;
 544         struct p2p_data *p2p = group->p2p;
 545         int freq;
 546
 547         m = p2p_group_get_client(group, dev_id);
 548         if (m == NULL) {
 549                 wpa_printf(MSG_DEBUG, "P2P: Requested client was not in this "
 550                            "group " MACSTR,
 551                            MAC2STR(group->cfg->interface_addr));
 552                 return -1;
 553         }
 554
 555         if (!(m->dev_capab & P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY)) {
 556                 wpa_printf(MSG_DEBUG, "P2P: Requested client does not support "
 557                            "client discoverability");
 558                 return -1;
 559         }
 560
 561         wpa_printf(MSG_DEBUG, "P2P: Schedule GO Discoverability Request to be "
 562                    "sent to " MACSTR, MAC2STR(dev_id));
 563
 564         req = p2p_build_go_disc_req();
 565         if (req == NULL)
 566                 return -1;
 567
 568         /* TODO: Should really use group operating frequency here */
 569         freq = rx_freq;
 570
 571         p2p->pending_action_state = P2P_PENDING_GO_DISC_REQ;
 572         if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, m->addr,
 573                                   group->cfg->interface_addr,
 574                                   group->cfg->interface_addr,
 575                                   wpabuf_head(req), wpabuf_len(req), 200) < 0)
 576         {
 577                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 578                         "P2P: Failed to send Action frame");
 579         }
 580
 581         wpabuf_free(req);
 582
 583         return 0;
 584 }
 585
 586
 587 const u8 * p2p_group_get_interface_addr(struct p2p_group *group)
 588 {
 589         return group->cfg->interface_addr;
 590 }
 591
 592
 593 u8 p2p_group_presence_req(struct p2p_group *group,
 594                           const u8 *client_interface_addr,
 595                           const u8 *noa, size_t noa_len)
 596 {
 597         struct p2p_group_member *m;
 598         u8 curr_noa[50];
 599         int curr_noa_len;
 600
 601         m = p2p_group_get_client_iface(group, client_interface_addr);
 602         if (m == NULL) {
 603                 wpa_printf(MSG_DEBUG, "P2P: Client was not in this group");
 604                 return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
 605         }
 606
 607         wpa_hexdump(MSG_DEBUG, "P2P: Presence Request NoA", noa, noa_len);
 608
 609         if (group->p2p->cfg->get_noa)
 610                 curr_noa_len = group->p2p->cfg->get_noa(
 611                         group->p2p->cfg->cb_ctx, group->cfg->interface_addr,
 612                         curr_noa, sizeof(curr_noa));
 613         else
 614                 curr_noa_len = -1;
 615         if (curr_noa_len < 0)
 616                 wpa_printf(MSG_DEBUG, "P2P: Failed to fetch current NoA");
 617         else if (curr_noa_len == 0)
 618                 wpa_printf(MSG_DEBUG, "P2P: No NoA being advertized");
 619         else
 620                 wpa_hexdump(MSG_DEBUG, "P2P: Current NoA", curr_noa,
 621                             curr_noa_len);
 622
 623         /* TODO: properly process request and store copy */
 624         if (curr_noa_len > 0)
 625                 return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
 626
 627         return P2P_SC_SUCCESS;
 628 }