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;
 104
 105         if (group == NULL)
 106                 return;
 107
 108         p2p = group->p2p;
 109
 110         for (g = 0; g < p2p->num_groups; g++) {
 111                 if (p2p->groups[g] == group) {
 112                         while (g + 1 < p2p->num_groups) {
 113                                 p2p->groups[g] = p2p->groups[g + 1];
 114                                 g++;
 115                         }
 116                         p2p->num_groups--;
 117                         break;
 118                 }
 119         }
 120
 121         p2p_group_free_members(group);
 122         os_free(group->cfg);
 123         wpabuf_free(group->noa);
 124         os_free(group);
 125 }
 126
 127
 128 static void p2p_client_info(struct wpabuf *ie, struct p2p_group_member *m)
 129 {
 130         if (wpabuf_tailroom(ie) < wpabuf_len(m->client_info) + 1)
 131                 return;
 132         wpabuf_put_buf(ie, m->client_info);
 133 }
 134
 135
 136 static void p2p_group_add_common_ies(struct p2p_group *group,
 137                                      struct wpabuf *ie)
 138 {
 139         u8 dev_capab = 0, group_capab = 0;
 140
 141         /* P2P Capability */
 142         dev_capab |= P2P_DEV_CAPAB_SERVICE_DISCOVERY;
 143         dev_capab |= P2P_DEV_CAPAB_INVITATION_PROCEDURE;
 144         group_capab |= P2P_GROUP_CAPAB_GROUP_OWNER;
 145         if (group->cfg->persistent_group)
 146                 group_capab |= P2P_GROUP_CAPAB_PERSISTENT_GROUP;
 147         group_capab |= P2P_GROUP_CAPAB_INTRA_BSS_DIST;
 148         if (group->group_formation)
 149                 group_capab |= P2P_GROUP_CAPAB_GROUP_FORMATION;
 150         if (group->p2p->cross_connect)
 151                 group_capab |= P2P_GROUP_CAPAB_CROSS_CONN;
 152         p2p_buf_add_capability(ie, dev_capab, group_capab);
 153 }
 154
 155
 156 static void p2p_group_add_noa(struct wpabuf *ie, struct wpabuf *noa)
 157 {
 158         if (noa == NULL)
 159                 return;
 160         /* Notice of Absence */
 161         wpabuf_put_u8(ie, P2P_ATTR_NOTICE_OF_ABSENCE);
 162         wpabuf_put_le16(ie, wpabuf_len(noa));
 163         wpabuf_put_buf(ie, noa);
 164 }
 165
 166
 167 static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group)
 168 {
 169         struct wpabuf *ie;
 170         u8 *len;
 171
 172         ie = wpabuf_alloc(257);
 173         if (ie == NULL)
 174                 return NULL;
 175
 176         len = p2p_buf_add_ie_hdr(ie);
 177         p2p_group_add_common_ies(group, ie);
 178         p2p_buf_add_device_id(ie, group->p2p->cfg->dev_addr);
 179         p2p_group_add_noa(ie, group->noa);
 180         p2p_buf_update_ie_hdr(ie, len);
 181
 182         return ie;
 183 }
 184
 185
 186 static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group)
 187 {
 188         u8 *group_info;
 189         struct wpabuf *ie;
 190         struct p2p_group_member *m;
 191         u8 *len;
 192
 193         ie = wpabuf_alloc(257);
 194         if (ie == NULL)
 195                 return NULL;
 196
 197         len = p2p_buf_add_ie_hdr(ie);
 198
 199         p2p_group_add_common_ies(group, ie);
 200         p2p_group_add_noa(ie, group->noa);
 201
 202         /* P2P Device Info */
 203         p2p_buf_add_device_info(ie, group->p2p, NULL);
 204
 205         /* P2P Group Info */
 206         group_info = wpabuf_put(ie, 0);
 207         wpabuf_put_u8(ie, P2P_ATTR_GROUP_INFO);
 208         wpabuf_put_le16(ie, 0); /* Length to be filled */
 209         for (m = group->members; m; m = m->next)
 210                 p2p_client_info(ie, m);
 211         WPA_PUT_LE16(group_info + 1,
 212                      (u8 *) wpabuf_put(ie, 0) - group_info - 3);
 213
 214         p2p_buf_update_ie_hdr(ie, len);
 215         return ie;
 216 }
 217
 218
 219 static void p2p_group_update_ies(struct p2p_group *group)
 220 {
 221         struct wpabuf *beacon_ie;
 222         struct wpabuf *probe_resp_ie;
 223
 224         probe_resp_ie = p2p_group_build_probe_resp_ie(group);
 225         if (probe_resp_ie == NULL)
 226                 return;
 227         wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Probe Response P2P IE",
 228                         probe_resp_ie);
 229
 230         if (group->beacon_update) {
 231                 beacon_ie = p2p_group_build_beacon_ie(group);
 232                 if (beacon_ie)
 233                         group->beacon_update = 0;
 234                 wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Beacon P2P IE",
 235                                 beacon_ie);
 236         } else
 237                 beacon_ie = NULL;
 238
 239         group->cfg->ie_update(group->cfg->cb_ctx, beacon_ie, probe_resp_ie);
 240 }
 241
 242
 243 /**
 244  * p2p_build_client_info - Build P2P Client Info Descriptor
 245  * @addr: MAC address of the peer device
 246  * @p2p_ie: P2P IE from (Re)Association Request
 247  * @dev_capab: Buffer for returning Device Capability
 248  * @dev_addr: Buffer for returning P2P Device Address
 249  * Returns: P2P Client Info Descriptor or %NULL on failure
 250  *
 251  * This function builds P2P Client Info Descriptor based on the information
 252  * available from (Re)Association Request frame. Group owner can use this to
 253  * build the P2P Group Info attribute for Probe Response frames.
 254  */
 255 static struct wpabuf * p2p_build_client_info(const u8 *addr,
 256                                              struct wpabuf *p2p_ie,
 257                                              u8 *dev_capab, u8 *dev_addr)
 258 {
 259         const u8 *spos;
 260         struct p2p_message msg;
 261         u8 *len_pos;
 262         struct wpabuf *buf;
 263
 264         if (p2p_ie == NULL)
 265                 return NULL;
 266
 267         os_memset(&msg, 0, sizeof(msg));
 268         if (p2p_parse_p2p_ie(p2p_ie, &msg) ||
 269             msg.capability == NULL || msg.p2p_device_info == NULL)
 270                 return NULL;
 271
 272         buf = wpabuf_alloc(ETH_ALEN + 1 + 1 + msg.p2p_device_info_len);
 273         if (buf == NULL)
 274                 return NULL;
 275
 276         *dev_capab = msg.capability[0];
 277         os_memcpy(dev_addr, msg.p2p_device_addr, ETH_ALEN);
 278
 279         spos = msg.p2p_device_info; /* P2P Device address */
 280
 281         /* P2P Client Info Descriptor */
 282         /* Length to be set */
 283         len_pos = wpabuf_put(buf, 1);
 284         /* P2P Device address */
 285         wpabuf_put_data(buf, spos, ETH_ALEN);
 286         /* P2P Interface address */
 287         wpabuf_put_data(buf, addr, ETH_ALEN);
 288         /* Device Capability Bitmap */
 289         wpabuf_put_u8(buf, msg.capability[0]);
 290         /*
 291          * Config Methods, Primary Device Type, Number of Secondary Device
 292          * Types, Secondary Device Type List, Device Name copied from
 293          * Device Info
 294          */
 295         wpabuf_put_data(buf, spos + ETH_ALEN,
 296                         msg.p2p_device_info_len - ETH_ALEN);
 297
 298         *len_pos = wpabuf_len(buf) - 1;
 299
 300
 301         return buf;
 302 }
 303
 304
 305 int p2p_group_notif_assoc(struct p2p_group *group, const u8 *addr,
 306                           const u8 *ie, size_t len)
 307 {
 308         struct p2p_group_member *m;
 309
 310         if (group == NULL)
 311                 return -1;
 312
 313         m = os_zalloc(sizeof(*m));
 314         if (m == NULL)
 315                 return -1;
 316         os_memcpy(m->addr, addr, ETH_ALEN);
 317         m->p2p_ie = ieee802_11_vendor_ie_concat(ie, len, P2P_IE_VENDOR_TYPE);
 318         if (m->p2p_ie == NULL) {
 319                 p2p_group_free_member(m);
 320                 return -1;
 321         }
 322
 323         m->client_info = p2p_build_client_info(addr, m->p2p_ie, &m->dev_capab,
 324                                                m->dev_addr);
 325         if (m->client_info == NULL) {
 326                 /*
 327                  * This can happen, e.g., when a P2P client connects to a P2P
 328                  * group using the infrastructure WLAN interface instead of
 329                  * P2P group interface. In that case, the P2P client may behave
 330                  * as if the GO would be a P2P Manager WLAN AP.
 331                  */
 332                 wpa_msg(group->p2p->cfg->msg_ctx, MSG_DEBUG,
 333                         "P2P: Could not build Client Info from P2P IE - "
 334                         "assume " MACSTR " is not a P2P client",
 335                         MAC2STR(addr));
 336                 p2p_group_free_member(m);
 337                 return 0;
 338         }
 339
 340         m->next = group->members;
 341         group->members = m;
 342
 343         p2p_group_update_ies(group);
 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                 p2p_group_update_ies(group);
 395         }
 396 }
 397
 398
 399 /**
 400  * p2p_match_dev_type_member - Match client device type with requested type
 401  * @m: Group member
 402  * @wps: WPS TLVs from Probe Request frame (concatenated WPS IEs)
 403  * Returns: 1 on match, 0 on mismatch
 404  *
 405  * This function can be used to match the Requested Device Type attribute in
 406  * WPS IE with the device types of a group member for deciding whether a GO
 407  * should reply to a Probe Request frame.
 408  */
 409 static int p2p_match_dev_type_member(struct p2p_group_member *m,
 410                                      struct wpabuf *wps)
 411 {
 412         const u8 *pos, *end;
 413         struct wps_parse_attr attr;
 414         u8 num_sec;
 415
 416         if (m->client_info == NULL || wps == NULL)
 417                 return 0;
 418
 419         pos = wpabuf_head(m->client_info);
 420         end = pos + wpabuf_len(m->client_info);
 421
 422         pos += 1 + 2 * ETH_ALEN + 1 + 2;
 423         if (end - pos < WPS_DEV_TYPE_LEN + 1)
 424                 return 0;
 425
 426         if (wps_parse_msg(wps, &attr))
 427                 return 1; /* assume no Requested Device Type attributes */
 428
 429         if (attr.num_req_dev_type == 0)
 430                 return 1; /* no Requested Device Type attributes -> match */
 431
 432         if (dev_type_list_match(pos, attr.req_dev_type, attr.num_req_dev_type))
 433                 return 1; /* Match with client Primary Device Type */
 434
 435         pos += WPS_DEV_TYPE_LEN;
 436         num_sec = *pos++;
 437         if (end - pos < num_sec * WPS_DEV_TYPE_LEN)
 438                 return 0;
 439         while (num_sec > 0) {
 440                 num_sec--;
 441                 if (dev_type_list_match(pos, attr.req_dev_type,
 442                                         attr.num_req_dev_type))
 443                         return 1; /* Match with client Secondary Device Type */
 444                 pos += WPS_DEV_TYPE_LEN;
 445         }
 446
 447         /* No matching device type found */
 448         return 0;
 449 }
 450
 451
 452 int p2p_group_match_dev_type(struct p2p_group *group, struct wpabuf *wps)
 453 {
 454         struct p2p_group_member *m;
 455
 456         if (p2p_match_dev_type(group->p2p, wps))
 457                 return 1; /* Match with own device type */
 458
 459         for (m = group->members; m; m = m->next) {
 460                 if (p2p_match_dev_type_member(m, wps))
 461                         return 1; /* Match with group client device type */
 462         }
 463
 464         /* No match with Requested Device Type */
 465         return 0;
 466 }
 467
 468
 469 void p2p_group_notif_formation_done(struct p2p_group *group)
 470 {
 471         if (group == NULL)
 472                 return;
 473         group->group_formation = 0;
 474         group->beacon_update = 1;
 475         p2p_group_update_ies(group);
 476 }
 477
 478
 479 int p2p_group_notif_noa(struct p2p_group *group, const u8 *noa,
 480                         size_t noa_len)
 481 {
 482         if (noa == NULL) {
 483                 wpabuf_free(group->noa);
 484                 group->noa = NULL;
 485         } else {
 486                 if (group->noa) {
 487                         if (wpabuf_size(group->noa) >= noa_len) {
 488                                 group->noa->size = 0;
 489                                 wpabuf_put_data(group->noa, noa, noa_len);
 490                         } else {
 491                                 wpabuf_free(group->noa);
 492                                 group->noa = NULL;
 493                         }
 494                 }
 495
 496                 if (!group->noa) {
 497                         group->noa = wpabuf_alloc_copy(noa, noa_len);
 498                         if (group->noa == NULL)
 499                                 return -1;
 500                 }
 501         }
 502
 503         group->beacon_update = 1;
 504         p2p_group_update_ies(group);
 505         return 0;
 506 }
 507
 508
 509 static struct p2p_group_member * p2p_group_get_client(struct p2p_group *group,
 510                                                       const u8 *dev_id)
 511 {
 512         struct p2p_group_member *m;
 513
 514         for (m = group->members; m; m = m->next) {
 515                 if (os_memcmp(dev_id, m->dev_addr, ETH_ALEN) == 0)
 516                         return m;
 517         }
 518
 519         return NULL;
 520 }
 521
 522
 523 static struct p2p_group_member * p2p_group_get_client_iface(
 524         struct p2p_group *group, const u8 *interface_addr)
 525 {
 526         struct p2p_group_member *m;
 527
 528         for (m = group->members; m; m = m->next) {
 529                 if (os_memcmp(interface_addr, m->addr, ETH_ALEN) == 0)
 530                         return m;
 531         }
 532
 533         return NULL;
 534 }
 535
 536
 537 static struct wpabuf * p2p_build_go_disc_req(void)
 538 {
 539         struct wpabuf *buf;
 540
 541         buf = wpabuf_alloc(100);
 542         if (buf == NULL)
 543                 return NULL;
 544
 545         p2p_buf_add_action_hdr(buf, P2P_GO_DISC_REQ, 0);
 546
 547         return buf;
 548 }
 549
 550
 551 int p2p_group_go_discover(struct p2p_group *group, const u8 *dev_id,
 552                           const u8 *searching_dev, int rx_freq)
 553 {
 554         struct p2p_group_member *m;
 555         struct wpabuf *req;
 556         struct p2p_data *p2p = group->p2p;
 557         int freq;
 558
 559         m = p2p_group_get_client(group, dev_id);
 560         if (m == NULL) {
 561                 wpa_printf(MSG_DEBUG, "P2P: Requested client was not in this "
 562                            "group " MACSTR,
 563                            MAC2STR(group->cfg->interface_addr));
 564                 return -1;
 565         }
 566
 567         if (!(m->dev_capab & P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY)) {
 568                 wpa_printf(MSG_DEBUG, "P2P: Requested client does not support "
 569                            "client discoverability");
 570                 return -1;
 571         }
 572
 573         wpa_printf(MSG_DEBUG, "P2P: Schedule GO Discoverability Request to be "
 574                    "sent to " MACSTR, MAC2STR(dev_id));
 575
 576         req = p2p_build_go_disc_req();
 577         if (req == NULL)
 578                 return -1;
 579
 580         /* TODO: Should really use group operating frequency here */
 581         freq = rx_freq;
 582
 583         p2p->pending_action_state = P2P_PENDING_GO_DISC_REQ;
 584         if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, m->addr,
 585                                   group->cfg->interface_addr,
 586                                   group->cfg->interface_addr,
 587                                   wpabuf_head(req), wpabuf_len(req), 200) < 0)
 588         {
 589                 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 590                         "P2P: Failed to send Action frame");
 591         }
 592
 593         wpabuf_free(req);
 594
 595         return 0;
 596 }
 597
 598
 599 const u8 * p2p_group_get_interface_addr(struct p2p_group *group)
 600 {
 601         return group->cfg->interface_addr;
 602 }
 603
 604
 605 u8 p2p_group_presence_req(struct p2p_group *group,
 606                           const u8 *client_interface_addr,
 607                           const u8 *noa, size_t noa_len)
 608 {
 609         struct p2p_group_member *m;
 610         u8 curr_noa[50];
 611         int curr_noa_len;
 612
 613         m = p2p_group_get_client_iface(group, client_interface_addr);
 614         if (m == NULL) {
 615                 wpa_printf(MSG_DEBUG, "P2P: Client was not in this group");
 616                 return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
 617         }
 618
 619         wpa_hexdump(MSG_DEBUG, "P2P: Presence Request NoA", noa, noa_len);
 620
 621         if (group->p2p->cfg->get_noa)
 622                 curr_noa_len = group->p2p->cfg->get_noa(
 623                         group->p2p->cfg->cb_ctx, group->cfg->interface_addr,
 624                         curr_noa, sizeof(curr_noa));
 625         else
 626                 curr_noa_len = -1;
 627         if (curr_noa_len < 0)
 628                 wpa_printf(MSG_DEBUG, "P2P: Failed to fetch current NoA");
 629         else if (curr_noa_len == 0)
 630                 wpa_printf(MSG_DEBUG, "P2P: No NoA being advertized");
 631         else
 632                 wpa_hexdump(MSG_DEBUG, "P2P: Current NoA", curr_noa,
 633                             curr_noa_len);
 634
 635         /* TODO: properly process request and store copy */
 636         if (curr_noa_len > 0)
 637                 return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
 638
 639         return P2P_SC_SUCCESS;
 640 }