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