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