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