Home | History | Annotate | Download | only in p2p
      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