libeap/src/p2p/p2p_group.c

   1 /*
   2  * Wi-Fi Direct - P2P group operations
   3  * Copyright (c) 2009-2010, Atheros Communications
   4  *
   5  * This software may be distributed under the terms of the BSD license.
   6  * See README for more details.
   7  */
   8
   9 #include "includes.h"
  10
  11 #include "common.h"
  12 #include "common/ieee802_11_defs.h"
  13 #include "common/ieee802_11_common.h"
  14 #include "common/wpa_ctrl.h"
  15 #include "wps/wps_defs.h"
  16 #include "wps/wps_i.h"
  17 #include "p2p_i.h"
  18 #include "p2p.h"
  19
  20
  21 struct p2p_group_member {
  22         struct p2p_group_member *next;
  23         u8 addr[ETH_ALEN]; /* P2P Interface Address */
  24         u8 dev_addr[ETH_ALEN]; /* P2P Device Address */
  25         struct wpabuf *p2p_ie;
  26         struct wpabuf *wfd_ie;
  27         struct wpabuf *client_info;
  28         u8 dev_capab;
  29 };
  30
  31 /**
  32  * struct p2p_group - Internal P2P module per-group data
  33  */
  34 struct p2p_group {
  35         struct p2p_data *p2p;
  36         struct p2p_group_config *cfg;
  37         struct p2p_group_member *members;
  38         unsigned int num_members;
  39         int group_formation;
  40         int beacon_update;
  41         struct wpabuf *noa;
  42         struct wpabuf *wfd_ie;
  43 };
  44
  45
  46 struct p2p_group * p2p_group_init(struct p2p_data *p2p,
  47                                   struct p2p_group_config *config)
  48 {
  49         struct p2p_group *group, **groups;
  50
  51         group = os_zalloc(sizeof(*group));
  52         if (group == NULL)
  53                 return NULL;
  54
  55         groups = os_realloc_array(p2p->groups, p2p->num_groups + 1,
  56                                   sizeof(struct p2p_group *));
  57         if (groups == NULL) {
  58                 os_free(group);
  59                 return NULL;
  60         }
  61         groups[p2p->num_groups++] = group;
  62         p2p->groups = groups;
  63
  64         group->p2p = p2p;
  65         group->cfg = config;
  66         group->group_formation = 1;
  67         group->beacon_update = 1;
  68         p2p_group_update_ies(group);
  69         group->cfg->idle_update(group->cfg->cb_ctx, 1);
  70
  71         return group;
  72 }
  73
  74
  75 static void p2p_group_free_member(struct p2p_group_member *m)
  76 {
  77         wpabuf_free(m->wfd_ie);
  78         wpabuf_free(m->p2p_ie);
  79         wpabuf_free(m->client_info);
  80         os_free(m);
  81 }
  82
  83
  84 static void p2p_group_free_members(struct p2p_group *group)
  85 {
  86         struct p2p_group_member *m, *prev;
  87         m = group->members;
  88         group->members = NULL;
  89         group->num_members = 0;
  90         while (m) {
  91                 prev = m;
  92                 m = m->next;
  93                 p2p_group_free_member(prev);
  94         }
  95 }
  96
  97
  98 void p2p_group_deinit(struct p2p_group *group)
  99 {
 100         size_t g;
 101         struct p2p_data *p2p;
 102
 103         if (group == NULL)
 104                 return;
 105
 106         p2p = group->p2p;
 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         wpabuf_free(group->wfd_ie);
 123         os_free(group);
 124 }
 125
 126
 127 static void p2p_client_info(struct wpabuf *ie, struct p2p_group_member *m)
 128 {
 129         if (m->client_info == NULL)
 130                 return;
 131         if (wpabuf_tailroom(ie) < wpabuf_len(m->client_info) + 1)
 132                 return;
 133         wpabuf_put_buf(ie, m->client_info);
 134 }
 135
 136
 137 static void p2p_group_add_common_ies(struct p2p_group *group,
 138                                      struct wpabuf *ie)
 139 {
 140         u8 dev_capab = group->p2p->dev_capab, group_capab = 0;
 141
 142         /* P2P Capability */
 143         dev_capab &= ~P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY;
 144         group_capab |= P2P_GROUP_CAPAB_GROUP_OWNER;
 145         if (group->cfg->persistent_group) {
 146                 group_capab |= P2P_GROUP_CAPAB_PERSISTENT_GROUP;
 147                 if (group->cfg->persistent_group == 2)
 148                         group_capab |= P2P_GROUP_CAPAB_PERSISTENT_RECONN;
 149         }
 150         if (group->p2p->cfg->p2p_intra_bss)
 151                 group_capab |= P2P_GROUP_CAPAB_INTRA_BSS_DIST;
 152         if (group->group_formation)
 153                 group_capab |= P2P_GROUP_CAPAB_GROUP_FORMATION;
 154         if (group->p2p->cross_connect)
 155                 group_capab |= P2P_GROUP_CAPAB_CROSS_CONN;
 156         if (group->num_members >= group->cfg->max_clients)
 157                 group_capab |= P2P_GROUP_CAPAB_GROUP_LIMIT;
 158         group_capab |= P2P_GROUP_CAPAB_IP_ADDR_ALLOCATION;
 159         p2p_buf_add_capability(ie, dev_capab, group_capab);
 160 }
 161
 162
 163 static void p2p_group_add_noa(struct wpabuf *ie, struct wpabuf *noa)
 164 {
 165         if (noa == NULL)
 166                 return;
 167         /* Notice of Absence */
 168         wpabuf_put_u8(ie, P2P_ATTR_NOTICE_OF_ABSENCE);
 169         wpabuf_put_le16(ie, wpabuf_len(noa));
 170         wpabuf_put_buf(ie, noa);
 171 }
 172
 173
 174 static struct wpabuf * p2p_group_encaps_probe_resp(struct wpabuf *subelems)
 175 {
 176         struct wpabuf *ie;
 177         const u8 *pos, *end;
 178         size_t len;
 179
 180         if (subelems == NULL)
 181                 return NULL;
 182
 183         len = wpabuf_len(subelems) + 100;
 184
 185         ie = wpabuf_alloc(len);
 186         if (ie == NULL)
 187                 return NULL;
 188
 189         pos = wpabuf_head(subelems);
 190         end = pos + wpabuf_len(subelems);
 191
 192         while (end > pos) {
 193                 size_t frag_len = end - pos;
 194                 if (frag_len > 251)
 195                         frag_len = 251;
 196                 wpabuf_put_u8(ie, WLAN_EID_VENDOR_SPECIFIC);
 197                 wpabuf_put_u8(ie, 4 + frag_len);
 198                 wpabuf_put_be32(ie, P2P_IE_VENDOR_TYPE);
 199                 wpabuf_put_data(ie, pos, frag_len);
 200                 pos += frag_len;
 201         }
 202
 203         return ie;
 204 }
 205
 206
 207 static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group)
 208 {
 209         struct wpabuf *ie;
 210         u8 *len;
 211         size_t extra = 0;
 212
 213 #ifdef CONFIG_WIFI_DISPLAY
 214         if (group->p2p->wfd_ie_beacon)
 215                 extra = wpabuf_len(group->p2p->wfd_ie_beacon);
 216 #endif /* CONFIG_WIFI_DISPLAY */
 217
 218         if (group->p2p->vendor_elem &&
 219             group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO])
 220                 extra += wpabuf_len(group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO]);
 221
 222         ie = wpabuf_alloc(257 + extra);
 223         if (ie == NULL)
 224                 return NULL;
 225
 226 #ifdef CONFIG_WIFI_DISPLAY
 227         if (group->p2p->wfd_ie_beacon)
 228                 wpabuf_put_buf(ie, group->p2p->wfd_ie_beacon);
 229 #endif /* CONFIG_WIFI_DISPLAY */
 230
 231         if (group->p2p->vendor_elem &&
 232             group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO])
 233                 wpabuf_put_buf(ie,
 234                                group->p2p->vendor_elem[VENDOR_ELEM_BEACON_P2P_GO]);
 235
 236         len = p2p_buf_add_ie_hdr(ie);
 237         p2p_group_add_common_ies(group, ie);
 238         p2p_buf_add_device_id(ie, group->p2p->cfg->dev_addr);
 239         p2p_group_add_noa(ie, group->noa);
 240         p2p_buf_update_ie_hdr(ie, len);
 241
 242         return ie;
 243 }
 244
 245
 246 #ifdef CONFIG_WIFI_DISPLAY
 247
 248 struct wpabuf * p2p_group_get_wfd_ie(struct p2p_group *g)
 249 {
 250         return g->wfd_ie;
 251 }
 252
 253
 254 struct wpabuf * wifi_display_encaps(struct wpabuf *subelems)
 255 {
 256         struct wpabuf *ie;
 257         const u8 *pos, *end;
 258
 259         if (subelems == NULL)
 260                 return NULL;
 261
 262         ie = wpabuf_alloc(wpabuf_len(subelems) + 100);
 263         if (ie == NULL)
 264                 return NULL;
 265
 266         pos = wpabuf_head(subelems);
 267         end = pos + wpabuf_len(subelems);
 268
 269         while (end > pos) {
 270                 size_t frag_len = end - pos;
 271                 if (frag_len > 251)
 272                         frag_len = 251;
 273                 wpabuf_put_u8(ie, WLAN_EID_VENDOR_SPECIFIC);
 274                 wpabuf_put_u8(ie, 4 + frag_len);
 275                 wpabuf_put_be32(ie, WFD_IE_VENDOR_TYPE);
 276                 wpabuf_put_data(ie, pos, frag_len);
 277                 pos += frag_len;
 278         }
 279
 280         return ie;
 281 }
 282
 283
 284 static int wifi_display_add_dev_info_descr(struct wpabuf *buf,
 285                                            struct p2p_group_member *m)
 286 {
 287         const u8 *pos, *end;
 288         const u8 *dev_info = NULL;
 289         const u8 *assoc_bssid = NULL;
 290         const u8 *coupled_sink = NULL;
 291         u8 zero_addr[ETH_ALEN];
 292
 293         if (m->wfd_ie == NULL)
 294                 return 0;
 295
 296         os_memset(zero_addr, 0, ETH_ALEN);
 297         pos = wpabuf_head_u8(m->wfd_ie);
 298         end = pos + wpabuf_len(m->wfd_ie);
 299         while (pos + 1 < end) {
 300                 u8 id;
 301                 u16 len;
 302
 303                 id = *pos++;
 304                 len = WPA_GET_BE16(pos);
 305                 pos += 2;
 306                 if (pos + len > end)
 307                         break;
 308
 309                 switch (id) {
 310                 case WFD_SUBELEM_DEVICE_INFO:
 311                         if (len < 6)
 312                                 break;
 313                         dev_info = pos;
 314                         break;
 315                 case WFD_SUBELEM_ASSOCIATED_BSSID:
 316                         if (len < ETH_ALEN)
 317                                 break;
 318                         assoc_bssid = pos;
 319                         break;
 320                 case WFD_SUBELEM_COUPLED_SINK:
 321                         if (len < 1 + ETH_ALEN)
 322                                 break;
 323                         coupled_sink = pos;
 324                         break;
 325                 }
 326
 327                 pos += len;
 328         }
 329
 330         if (dev_info == NULL)
 331                 return 0;
 332
 333         wpabuf_put_u8(buf, 23);
 334         wpabuf_put_data(buf, m->dev_addr, ETH_ALEN);
 335         if (assoc_bssid)
 336                 wpabuf_put_data(buf, assoc_bssid, ETH_ALEN);
 337         else
 338                 wpabuf_put_data(buf, zero_addr, ETH_ALEN);
 339         wpabuf_put_data(buf, dev_info, 2); /* WFD Device Info */
 340         wpabuf_put_data(buf, dev_info + 4, 2); /* WFD Device Max Throughput */
 341         if (coupled_sink) {
 342                 wpabuf_put_data(buf, coupled_sink, 1 + ETH_ALEN);
 343         } else {
 344                 wpabuf_put_u8(buf, 0);
 345                 wpabuf_put_data(buf, zero_addr, ETH_ALEN);
 346         }
 347
 348         return 1;
 349 }
 350
 351
 352 static struct wpabuf *
 353 wifi_display_build_go_ie(struct p2p_group *group)
 354 {
 355         struct wpabuf *wfd_subelems, *wfd_ie;
 356         struct p2p_group_member *m;
 357         u8 *len;
 358         unsigned int count = 0;
 359
 360         if (!group->p2p->wfd_ie_probe_resp)
 361                 return NULL;
 362
 363         wfd_subelems = wpabuf_alloc(wpabuf_len(group->p2p->wfd_ie_probe_resp) +
 364                                     group->num_members * 24 + 100);
 365         if (wfd_subelems == NULL)
 366                 return NULL;
 367         if (group->p2p->wfd_dev_info)
 368                 wpabuf_put_buf(wfd_subelems, group->p2p->wfd_dev_info);
 369         if (group->p2p->wfd_assoc_bssid)
 370                 wpabuf_put_buf(wfd_subelems,
 371                                group->p2p->wfd_assoc_bssid);
 372         if (group->p2p->wfd_coupled_sink_info)
 373                 wpabuf_put_buf(wfd_subelems,
 374                                group->p2p->wfd_coupled_sink_info);
 375
 376         /* Build WFD Session Info */
 377         wpabuf_put_u8(wfd_subelems, WFD_SUBELEM_SESSION_INFO);
 378         len = wpabuf_put(wfd_subelems, 2);
 379         m = group->members;
 380         while (m) {
 381                 if (wifi_display_add_dev_info_descr(wfd_subelems, m))
 382                         count++;
 383                 m = m->next;
 384         }
 385
 386         if (count == 0) {
 387                 /* No Wi-Fi Display clients - do not include subelement */
 388                 wfd_subelems->used -= 3;
 389         } else {
 390                 WPA_PUT_BE16(len, (u8 *) wpabuf_put(wfd_subelems, 0) - len -
 391                              2);
 392                 p2p_dbg(group->p2p, "WFD: WFD Session Info: %u descriptors",
 393                         count);
 394         }
 395
 396         wfd_ie = wifi_display_encaps(wfd_subelems);
 397         wpabuf_free(wfd_subelems);
 398
 399         return wfd_ie;
 400 }
 401
 402 static void wifi_display_group_update(struct p2p_group *group)
 403 {
 404         wpabuf_free(group->wfd_ie);
 405         group->wfd_ie = wifi_display_build_go_ie(group);
 406 }
 407
 408 #endif /* CONFIG_WIFI_DISPLAY */
 409
 410
 411 void p2p_buf_add_group_info(struct p2p_group *group, struct wpabuf *buf,
 412                             int max_clients)
 413 {
 414         u8 *group_info;
 415         int count = 0;
 416         struct p2p_group_member *m;
 417
 418         p2p_dbg(group->p2p, "* P2P Group Info");
 419         group_info = wpabuf_put(buf, 0);
 420         wpabuf_put_u8(buf, P2P_ATTR_GROUP_INFO);
 421         wpabuf_put_le16(buf, 0); /* Length to be filled */
 422         for (m = group->members; m; m = m->next) {
 423                 p2p_client_info(buf, m);
 424                 count++;
 425                 if (max_clients >= 0 && count >= max_clients)
 426                         break;
 427         }
 428         WPA_PUT_LE16(group_info + 1,
 429                      (u8 *) wpabuf_put(buf, 0) - group_info - 3);
 430 }
 431
 432
 433 void p2p_group_buf_add_id(struct p2p_group *group, struct wpabuf *buf)
 434 {
 435         p2p_buf_add_group_id(buf, group->p2p->cfg->dev_addr, group->cfg->ssid,
 436                              group->cfg->ssid_len);
 437 }
 438
 439
 440 static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group)
 441 {
 442         struct wpabuf *p2p_subelems, *ie;
 443
 444         p2p_subelems = wpabuf_alloc(500);
 445         if (p2p_subelems == NULL)
 446                 return NULL;
 447
 448         p2p_group_add_common_ies(group, p2p_subelems);
 449         p2p_group_add_noa(p2p_subelems, group->noa);
 450
 451         /* P2P Device Info */
 452         p2p_buf_add_device_info(p2p_subelems, group->p2p, NULL);
 453
 454         /* P2P Group Info: Only when at least one P2P Client is connected */
 455         if (group->members)
 456                 p2p_buf_add_group_info(group, p2p_subelems, -1);
 457
 458         ie = p2p_group_encaps_probe_resp(p2p_subelems);
 459         wpabuf_free(p2p_subelems);
 460
 461         if (group->p2p->vendor_elem &&
 462             group->p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P_GO]) {
 463                 struct wpabuf *extra;
 464                 extra = wpabuf_dup(group->p2p->vendor_elem[VENDOR_ELEM_PROBE_RESP_P2P_GO]);
 465                 ie = wpabuf_concat(extra, ie);
 466         }
 467
 468 #ifdef CONFIG_WIFI_DISPLAY
 469         if (group->wfd_ie) {
 470                 struct wpabuf *wfd = wpabuf_dup(group->wfd_ie);
 471                 ie = wpabuf_concat(wfd, ie);
 472         }
 473 #endif /* CONFIG_WIFI_DISPLAY */
 474
 475         return ie;
 476 }
 477
 478
 479 void p2p_group_update_ies(struct p2p_group *group)
 480 {
 481         struct wpabuf *beacon_ie;
 482         struct wpabuf *probe_resp_ie;
 483
 484 #ifdef CONFIG_WIFI_DISPLAY
 485         wifi_display_group_update(group);
 486 #endif /* CONFIG_WIFI_DISPLAY */
 487
 488         probe_resp_ie = p2p_group_build_probe_resp_ie(group);
 489         if (probe_resp_ie == NULL)
 490                 return;
 491         wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Probe Response P2P IE",
 492                         probe_resp_ie);
 493
 494         if (group->beacon_update) {
 495                 beacon_ie = p2p_group_build_beacon_ie(group);
 496                 if (beacon_ie)
 497                         group->beacon_update = 0;
 498                 wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Beacon P2P IE",
 499                                 beacon_ie);
 500         } else
 501                 beacon_ie = NULL;
 502
 503         group->cfg->ie_update(group->cfg->cb_ctx, beacon_ie, probe_resp_ie);
 504 }
 505
 506
 507 /**
 508  * p2p_build_client_info - Build P2P Client Info Descriptor
 509  * @addr: MAC address of the peer device
 510  * @p2p_ie: P2P IE from (Re)Association Request
 511  * @dev_capab: Buffer for returning Device Capability
 512  * @dev_addr: Buffer for returning P2P Device Address
 513  * Returns: P2P Client Info Descriptor or %NULL on failure
 514  *
 515  * This function builds P2P Client Info Descriptor based on the information
 516  * available from (Re)Association Request frame. Group owner can use this to
 517  * build the P2P Group Info attribute for Probe Response frames.
 518  */
 519 static struct wpabuf * p2p_build_client_info(const u8 *addr,
 520                                              struct wpabuf *p2p_ie,
 521                                              u8 *dev_capab, u8 *dev_addr)
 522 {
 523         const u8 *spos;
 524         struct p2p_message msg;
 525         u8 *len_pos;
 526         struct wpabuf *buf;
 527
 528         if (p2p_ie == NULL)
 529                 return NULL;
 530
 531         os_memset(&msg, 0, sizeof(msg));
 532         if (p2p_parse_p2p_ie(p2p_ie, &msg) ||
 533             msg.capability == NULL || msg.p2p_device_info == NULL)
 534                 return NULL;
 535
 536         buf = wpabuf_alloc(ETH_ALEN + 1 + 1 + msg.p2p_device_info_len);
 537         if (buf == NULL)
 538                 return NULL;
 539
 540         *dev_capab = msg.capability[0];
 541         os_memcpy(dev_addr, msg.p2p_device_addr, ETH_ALEN);
 542
 543         spos = msg.p2p_device_info; /* P2P Device address */
 544
 545         /* P2P Client Info Descriptor */
 546         /* Length to be set */
 547         len_pos = wpabuf_put(buf, 1);
 548         /* P2P Device address */
 549         wpabuf_put_data(buf, spos, ETH_ALEN);
 550         /* P2P Interface address */
 551         wpabuf_put_data(buf, addr, ETH_ALEN);
 552         /* Device Capability Bitmap */
 553         wpabuf_put_u8(buf, msg.capability[0]);
 554         /*
 555          * Config Methods, Primary Device Type, Number of Secondary Device
 556          * Types, Secondary Device Type List, Device Name copied from
 557          * Device Info
 558          */
 559         wpabuf_put_data(buf, spos + ETH_ALEN,
 560                         msg.p2p_device_info_len - ETH_ALEN);
 561
 562         *len_pos = wpabuf_len(buf) - 1;
 563
 564
 565         return buf;
 566 }
 567
 568
 569 static int p2p_group_remove_member(struct p2p_group *group, const u8 *addr)
 570 {
 571         struct p2p_group_member *m, *prev;
 572
 573         if (group == NULL)
 574                 return 0;
 575
 576         m = group->members;
 577         prev = NULL;
 578         while (m) {
 579                 if (os_memcmp(m->addr, addr, ETH_ALEN) == 0)
 580                         break;
 581                 prev = m;
 582                 m = m->next;
 583         }
 584
 585         if (m == NULL)
 586                 return 0;
 587
 588         if (prev)
 589                 prev->next = m->next;
 590         else
 591                 group->members = m->next;
 592         p2p_group_free_member(m);
 593         group->num_members--;
 594
 595         return 1;
 596 }
 597
 598
 599 int p2p_group_notif_assoc(struct p2p_group *group, const u8 *addr,
 600                           const u8 *ie, size_t len)
 601 {
 602         struct p2p_group_member *m;
 603
 604         if (group == NULL)
 605                 return -1;
 606
 607         p2p_add_device(group->p2p, addr, 0, NULL, 0, ie, len, 0);
 608
 609         m = os_zalloc(sizeof(*m));
 610         if (m == NULL)
 611                 return -1;
 612         os_memcpy(m->addr, addr, ETH_ALEN);
 613         m->p2p_ie = ieee802_11_vendor_ie_concat(ie, len, P2P_IE_VENDOR_TYPE);
 614         if (m->p2p_ie) {
 615                 m->client_info = p2p_build_client_info(addr, m->p2p_ie,
 616                                                        &m->dev_capab,
 617                                                        m->dev_addr);
 618         }
 619 #ifdef CONFIG_WIFI_DISPLAY
 620         m->wfd_ie = ieee802_11_vendor_ie_concat(ie, len, WFD_IE_VENDOR_TYPE);
 621 #endif /* CONFIG_WIFI_DISPLAY */
 622
 623         p2p_group_remove_member(group, addr);
 624
 625         m->next = group->members;
 626         group->members = m;
 627         group->num_members++;
 628         p2p_dbg(group->p2p,  "Add client " MACSTR
 629                 " to group (p2p=%d wfd=%d client_info=%d); num_members=%u/%u",
 630                 MAC2STR(addr), m->p2p_ie ? 1 : 0, m->wfd_ie ? 1 : 0,
 631                 m->client_info ? 1 : 0,
 632                 group->num_members, group->cfg->max_clients);
 633         if (group->num_members == group->cfg->max_clients)
 634                 group->beacon_update = 1;
 635         p2p_group_update_ies(group);
 636         if (group->num_members == 1)
 637                 group->cfg->idle_update(group->cfg->cb_ctx, 0);
 638
 639         return 0;
 640 }
 641
 642
 643 struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status)
 644 {
 645         struct wpabuf *resp;
 646         u8 *rlen;
 647         size_t extra = 0;
 648
 649 #ifdef CONFIG_WIFI_DISPLAY
 650         if (group->wfd_ie)
 651                 extra = wpabuf_len(group->wfd_ie);
 652 #endif /* CONFIG_WIFI_DISPLAY */
 653
 654         if (group->p2p->vendor_elem &&
 655             group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP])
 656                 extra += wpabuf_len(group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP]);
 657
 658         /*
 659          * (Re)Association Response - P2P IE
 660          * Status attribute (shall be present when association request is
 661          *      denied)
 662          * Extended Listen Timing (may be present)
 663          */
 664         resp = wpabuf_alloc(20 + extra);
 665         if (resp == NULL)
 666                 return NULL;
 667
 668 #ifdef CONFIG_WIFI_DISPLAY
 669         if (group->wfd_ie)
 670                 wpabuf_put_buf(resp, group->wfd_ie);
 671 #endif /* CONFIG_WIFI_DISPLAY */
 672
 673         if (group->p2p->vendor_elem &&
 674             group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP])
 675                 wpabuf_put_buf(resp,
 676                                group->p2p->vendor_elem[VENDOR_ELEM_P2P_ASSOC_RESP]);
 677
 678         rlen = p2p_buf_add_ie_hdr(resp);
 679         if (status != P2P_SC_SUCCESS)
 680                 p2p_buf_add_status(resp, status);
 681         p2p_buf_update_ie_hdr(resp, rlen);
 682
 683         return resp;
 684 }
 685
 686
 687 void p2p_group_notif_disassoc(struct p2p_group *group, const u8 *addr)
 688 {
 689         if (p2p_group_remove_member(group, addr)) {
 690                 p2p_dbg(group->p2p, "Remove client " MACSTR
 691                         " from group; num_members=%u/%u",
 692                         MAC2STR(addr), group->num_members,
 693                         group->cfg->max_clients);
 694                 if (group->num_members == group->cfg->max_clients - 1)
 695                         group->beacon_update = 1;
 696                 p2p_group_update_ies(group);
 697                 if (group->num_members == 0)
 698                         group->cfg->idle_update(group->cfg->cb_ctx, 1);
 699         }
 700 }
 701
 702
 703 /**
 704  * p2p_match_dev_type_member - Match client device type with requested type
 705  * @m: Group member
 706  * @wps: WPS TLVs from Probe Request frame (concatenated WPS IEs)
 707  * Returns: 1 on match, 0 on mismatch
 708  *
 709  * This function can be used to match the Requested Device Type attribute in
 710  * WPS IE with the device types of a group member for deciding whether a GO
 711  * should reply to a Probe Request frame.
 712  */
 713 static int p2p_match_dev_type_member(struct p2p_group_member *m,
 714                                      struct wpabuf *wps)
 715 {
 716         const u8 *pos, *end;
 717         struct wps_parse_attr attr;
 718         u8 num_sec;
 719
 720         if (m->client_info == NULL || wps == NULL)
 721                 return 0;
 722
 723         pos = wpabuf_head(m->client_info);
 724         end = pos + wpabuf_len(m->client_info);
 725
 726         pos += 1 + 2 * ETH_ALEN + 1 + 2;
 727         if (end - pos < WPS_DEV_TYPE_LEN + 1)
 728                 return 0;
 729
 730         if (wps_parse_msg(wps, &attr))
 731                 return 1; /* assume no Requested Device Type attributes */
 732
 733         if (attr.num_req_dev_type == 0)
 734                 return 1; /* no Requested Device Type attributes -> match */
 735
 736         if (dev_type_list_match(pos, attr.req_dev_type, attr.num_req_dev_type))
 737                 return 1; /* Match with client Primary Device Type */
 738
 739         pos += WPS_DEV_TYPE_LEN;
 740         num_sec = *pos++;
 741         if (end - pos < num_sec * WPS_DEV_TYPE_LEN)
 742                 return 0;
 743         while (num_sec > 0) {
 744                 num_sec--;
 745                 if (dev_type_list_match(pos, attr.req_dev_type,
 746                                         attr.num_req_dev_type))
 747                         return 1; /* Match with client Secondary Device Type */
 748                 pos += WPS_DEV_TYPE_LEN;
 749         }
 750
 751         /* No matching device type found */
 752         return 0;
 753 }
 754
 755
 756 int p2p_group_match_dev_type(struct p2p_group *group, struct wpabuf *wps)
 757 {
 758         struct p2p_group_member *m;
 759
 760         if (p2p_match_dev_type(group->p2p, wps))
 761                 return 1; /* Match with own device type */
 762
 763         for (m = group->members; m; m = m->next) {
 764                 if (p2p_match_dev_type_member(m, wps))
 765                         return 1; /* Match with group client device type */
 766         }
 767
 768         /* No match with Requested Device Type */
 769         return 0;
 770 }
 771
 772
 773 int p2p_group_match_dev_id(struct p2p_group *group, struct wpabuf *p2p)
 774 {
 775         struct p2p_group_member *m;
 776         struct p2p_message msg;
 777
 778         os_memset(&msg, 0, sizeof(msg));
 779         if (p2p_parse_p2p_ie(p2p, &msg))
 780                 return 1; /* Failed to parse - assume no filter on Device ID */
 781
 782         if (!msg.device_id)
 783                 return 1; /* No filter on Device ID */
 784
 785         if (os_memcmp(msg.device_id, group->p2p->cfg->dev_addr, ETH_ALEN) == 0)
 786                 return 1; /* Match with our P2P Device Address */
 787
 788         for (m = group->members; m; m = m->next) {
 789                 if (os_memcmp(msg.device_id, m->dev_addr, ETH_ALEN) == 0)
 790                         return 1; /* Match with group client P2P Device Address */
 791         }
 792
 793         /* No match with Device ID */
 794         return 0;
 795 }
 796
 797
 798 void p2p_group_notif_formation_done(struct p2p_group *group)
 799 {
 800         if (group == NULL)
 801                 return;
 802         group->group_formation = 0;
 803         group->beacon_update = 1;
 804         p2p_group_update_ies(group);
 805 }
 806
 807
 808 int p2p_group_notif_noa(struct p2p_group *group, const u8 *noa,
 809                         size_t noa_len)
 810 {
 811         if (noa == NULL) {
 812                 wpabuf_free(group->noa);
 813                 group->noa = NULL;
 814         } else {
 815                 if (group->noa) {
 816                         if (wpabuf_size(group->noa) >= noa_len) {
 817                                 group->noa->used = 0;
 818                                 wpabuf_put_data(group->noa, noa, noa_len);
 819                         } else {
 820                                 wpabuf_free(group->noa);
 821                                 group->noa = NULL;
 822                         }
 823                 }
 824
 825                 if (!group->noa) {
 826                         group->noa = wpabuf_alloc_copy(noa, noa_len);
 827                         if (group->noa == NULL)
 828                                 return -1;
 829                 }
 830         }
 831
 832         group->beacon_update = 1;
 833         p2p_group_update_ies(group);
 834         return 0;
 835 }
 836
 837
 838 static struct p2p_group_member * p2p_group_get_client(struct p2p_group *group,
 839                                                       const u8 *dev_id)
 840 {
 841         struct p2p_group_member *m;
 842
 843         for (m = group->members; m; m = m->next) {
 844                 if (os_memcmp(dev_id, m->dev_addr, ETH_ALEN) == 0)
 845                         return m;
 846         }
 847
 848         return NULL;
 849 }
 850
 851
 852 static struct p2p_group_member * p2p_group_get_client_iface(
 853         struct p2p_group *group, const u8 *interface_addr)
 854 {
 855         struct p2p_group_member *m;
 856
 857         for (m = group->members; m; m = m->next) {
 858                 if (os_memcmp(interface_addr, m->addr, ETH_ALEN) == 0)
 859                         return m;
 860         }
 861
 862         return NULL;
 863 }
 864
 865
 866 const u8 * p2p_group_get_dev_addr(struct p2p_group *group, const u8 *addr)
 867 {
 868         struct p2p_group_member *m;
 869
 870         if (group == NULL)
 871                 return NULL;
 872         m = p2p_group_get_client_iface(group, addr);
 873         if (m && !is_zero_ether_addr(m->dev_addr))
 874                 return m->dev_addr;
 875         return NULL;
 876 }
 877
 878
 879 static struct wpabuf * p2p_build_go_disc_req(void)
 880 {
 881         struct wpabuf *buf;
 882
 883         buf = wpabuf_alloc(100);
 884         if (buf == NULL)
 885                 return NULL;
 886
 887         p2p_buf_add_action_hdr(buf, P2P_GO_DISC_REQ, 0);
 888
 889         return buf;
 890 }
 891
 892
 893 int p2p_group_go_discover(struct p2p_group *group, const u8 *dev_id,
 894                           const u8 *searching_dev, int rx_freq)
 895 {
 896         struct p2p_group_member *m;
 897         struct wpabuf *req;
 898         struct p2p_data *p2p = group->p2p;
 899         int freq;
 900
 901         m = p2p_group_get_client(group, dev_id);
 902         if (m == NULL || m->client_info == NULL) {
 903                 p2p_dbg(group->p2p, "Requested client was not in this group "
 904                         MACSTR, MAC2STR(group->cfg->interface_addr));
 905                 return -1;
 906         }
 907
 908         if (!(m->dev_capab & P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY)) {
 909                 p2p_dbg(group->p2p, "Requested client does not support client discoverability");
 910                 return -1;
 911         }
 912
 913         p2p_dbg(group->p2p, "Schedule GO Discoverability Request to be sent to "
 914                 MACSTR, MAC2STR(dev_id));
 915
 916         req = p2p_build_go_disc_req();
 917         if (req == NULL)
 918                 return -1;
 919
 920         /* TODO: Should really use group operating frequency here */
 921         freq = rx_freq;
 922
 923         p2p->pending_action_state = P2P_PENDING_GO_DISC_REQ;
 924         if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, m->addr,
 925                                   group->cfg->interface_addr,
 926                                   group->cfg->interface_addr,
 927                                   wpabuf_head(req), wpabuf_len(req), 200) < 0)
 928         {
 929                 p2p_dbg(p2p, "Failed to send Action frame");
 930         }
 931
 932         wpabuf_free(req);
 933
 934         return 0;
 935 }
 936
 937
 938 const u8 * p2p_group_get_interface_addr(struct p2p_group *group)
 939 {
 940         return group->cfg->interface_addr;
 941 }
 942
 943
 944 u8 p2p_group_presence_req(struct p2p_group *group,
 945                           const u8 *client_interface_addr,
 946                           const u8 *noa, size_t noa_len)
 947 {
 948         struct p2p_group_member *m;
 949         u8 curr_noa[50];
 950         int curr_noa_len;
 951
 952         m = p2p_group_get_client_iface(group, client_interface_addr);
 953         if (m == NULL || m->client_info == NULL) {
 954                 p2p_dbg(group->p2p, "Client was not in this group");
 955                 return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
 956         }
 957
 958         wpa_hexdump(MSG_DEBUG, "P2P: Presence Request NoA", noa, noa_len);
 959
 960         if (group->p2p->cfg->get_noa)
 961                 curr_noa_len = group->p2p->cfg->get_noa(
 962                         group->p2p->cfg->cb_ctx, group->cfg->interface_addr,
 963                         curr_noa, sizeof(curr_noa));
 964         else
 965                 curr_noa_len = -1;
 966         if (curr_noa_len < 0)
 967                 p2p_dbg(group->p2p, "Failed to fetch current NoA");
 968         else if (curr_noa_len == 0)
 969                 p2p_dbg(group->p2p, "No NoA being advertized");
 970         else
 971                 wpa_hexdump(MSG_DEBUG, "P2P: Current NoA", curr_noa,
 972                             curr_noa_len);
 973
 974         /* TODO: properly process request and store copy */
 975         if (curr_noa_len > 0 || curr_noa_len == -1)
 976                 return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
 977
 978         return P2P_SC_SUCCESS;
 979 }
 980
 981
 982 unsigned int p2p_get_group_num_members(struct p2p_group *group)
 983 {
 984         if (!group)
 985                 return 0;
 986
 987         return group->num_members;
 988 }
 989
 990
 991 int p2p_client_limit_reached(struct p2p_group *group)
 992 {
 993         if (!group || !group->cfg)
 994                 return 1;
 995
 996         return group->num_members >= group->cfg->max_clients;
 997 }
 998
 999
1000 const u8 * p2p_iterate_group_members(struct p2p_group *group, void **next)
1001 {
1002         struct p2p_group_member *iter = *next;
1003
1004         if (!iter)
1005                 iter = group->members;
1006         else
1007                 iter = iter->next;
1008
1009         *next = iter;
1010
1011         if (!iter)
1012                 return NULL;
1013
1014         return iter->dev_addr;
1015 }
1016
1017
1018 int p2p_group_is_client_connected(struct p2p_group *group, const u8 *dev_addr)
1019 {
1020         struct p2p_group_member *m;
1021
1022         for (m = group->members; m; m = m->next) {
1023                 if (os_memcmp(m->dev_addr, dev_addr, ETH_ALEN) == 0)
1024                         return 1;
1025         }
1026
1027         return 0;
1028 }
1029
1030
1031 int p2p_group_is_group_id_match(struct p2p_group *group, const u8 *group_id,
1032                                 size_t group_id_len)
1033 {
1034         if (group_id_len != ETH_ALEN + group->cfg->ssid_len)
1035                 return 0;
1036         if (os_memcmp(group_id, group->p2p->cfg->dev_addr, ETH_ALEN) != 0)
1037                 return 0;
1038         return os_memcmp(group_id + ETH_ALEN, group->cfg->ssid,
1039                          group->cfg->ssid_len) == 0;
1040 }
1041
1042
1043 void p2p_group_force_beacon_update_ies(struct p2p_group *group)
1044 {
1045         group->beacon_update = 1;
1046         p2p_group_update_ies(group);
1047 }
1048
1049
1050 int p2p_group_get_freq(struct p2p_group *group)
1051 {
1052         return group->cfg->freq;
1053 }
1054
1055
1056 const struct p2p_group_config * p2p_group_get_config(struct p2p_group *group)
1057 {
1058         return group->cfg;
1059 }
1060
1061
1062 void p2p_loop_on_all_groups(struct p2p_data *p2p,
1063                             int (*group_callback)(struct p2p_group *group,
1064                                                   void *user_data),
1065                             void *user_data)
1066 {
1067         unsigned int i;
1068
1069         for (i = 0; i < p2p->num_groups; i++) {
1070                 if (!group_callback(p2p->groups[i], user_data))
1071                         break;
1072         }
1073 }
1074
1075
1076 int p2p_group_get_common_freqs(struct p2p_group *group, int *common_freqs,
1077                                unsigned int *num)
1078
1079 {
1080         struct p2p_channels intersect, res;
1081         struct p2p_group_member *m;
1082
1083         if (!group || !common_freqs || !num)
1084                 return -1;
1085
1086         os_memset(&intersect, 0, sizeof(intersect));
1087         os_memset(&res, 0, sizeof(res));
1088
1089         p2p_channels_union(&intersect, &group->p2p->cfg->channels,
1090                            &intersect);
1091
1092         p2p_channels_dump(group->p2p,
1093                           "Group common freqs before iterating members",
1094                           &intersect);
1095
1096         for (m = group->members; m; m = m->next) {
1097                 struct p2p_device *dev;
1098
1099                 dev = p2p_get_device(group->p2p, m->dev_addr);
1100                 if (!dev)
1101                         continue;
1102
1103                 p2p_channels_intersect(&intersect, &dev->channels, &res);
1104                 intersect = res;
1105         }
1106
1107         p2p_channels_dump(group->p2p, "Group common channels", &intersect);
1108
1109         os_memset(common_freqs, 0, *num * sizeof(int));
1110         *num = p2p_channels_to_freqs(&intersect, common_freqs, *num);
1111
1112         return 0;
1113 }