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