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
20struct 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 */
33struct 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
45struct 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
74static 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
83static 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
97void 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
126static 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
136static 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
161static 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
172static 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
205static 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
237struct wpabuf * p2p_group_get_wfd_ie(struct p2p_group *g)
238{
239	return g->wfd_ie;
240}
241
242
243struct 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
273static 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
341static struct wpabuf *
342wifi_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
391static 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
400static 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
442void 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 */
482static 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
532static 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
562int 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
606struct 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
641void 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 */
667static 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
710int 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
727int 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
752void 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
762int 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
792static 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
806static 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
820const 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
833static 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
847int 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
892const u8 * p2p_group_get_interface_addr(struct p2p_group *group)
893{
894	return group->cfg->interface_addr;
895}
896
897
898u8 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
936unsigned int p2p_get_group_num_members(struct p2p_group *group)
937{
938	return group->num_members;
939}
940
941
942const 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
960int 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
973int 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