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