[thirdparty/hostap.git] / src / p2p / p2p_group.c

/*
 * Wi-Fi Direct - P2P group operations
 * Copyright (c) 2009-2010, Atheros Communications
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * Alternatively, this software may be distributed under the terms of BSD
 * license.
 *
 * See README and COPYING for more details.
 */

#include "includes.h"

#include "common.h"
#include "common/ieee802_11_defs.h"
#include "common/ieee802_11_common.h"
#include "wps/wps_defs.h"
#include "wps/wps_i.h"
#include "p2p_i.h"
#include "p2p.h"


struct p2p_group_member {
	struct p2p_group_member *next;
	u8 addr[ETH_ALEN]; /* P2P Interface Address */
	u8 dev_addr[ETH_ALEN]; /* P2P Device Address */
	struct wpabuf *p2p_ie;
	struct wpabuf *client_info;
	u8 dev_capab;
};

/**
 * struct p2p_group - Internal P2P module per-group data
 */
struct p2p_group {
	struct p2p_data *p2p;
	struct p2p_group_config *cfg;
	struct p2p_group_member *members;
	unsigned int num_members;
	int group_formation;
	int beacon_update;
	struct wpabuf *noa;
};


static void p2p_group_update_ies(struct p2p_group *group);


struct p2p_group * p2p_group_init(struct p2p_data *p2p,
				  struct p2p_group_config *config)
{
	struct p2p_group *group, **groups;

	group = os_zalloc(sizeof(*group));
	if (group == NULL)
		return NULL;

	groups = os_realloc(p2p->groups, (p2p->num_groups + 1) *
			    sizeof(struct p2p_group *));
	if (groups == NULL) {
		os_free(group);
		return NULL;
	}
	groups[p2p->num_groups++] = group;
	p2p->groups = groups;

	group->p2p = p2p;
	group->cfg = config;
	group->group_formation = 1;
	group->beacon_update = 1;
	p2p_group_update_ies(group);
	group->cfg->idle_update(group->cfg->cb_ctx, 1);

	return group;
}


static void p2p_group_free_member(struct p2p_group_member *m)
{
	wpabuf_free(m->p2p_ie);
	wpabuf_free(m->client_info);
	os_free(m);
}


static void p2p_group_free_members(struct p2p_group *group)
{
	struct p2p_group_member *m, *prev;
	m = group->members;
	group->members = NULL;
	group->num_members = 0;
	while (m) {
		prev = m;
		m = m->next;
		p2p_group_free_member(prev);
	}
}


void p2p_group_deinit(struct p2p_group *group)
{
	size_t g;
	struct p2p_data *p2p;

	if (group == NULL)
		return;

	p2p = group->p2p;

	for (g = 0; g < p2p->num_groups; g++) {
		if (p2p->groups[g] == group) {
			while (g + 1 < p2p->num_groups) {
				p2p->groups[g] = p2p->groups[g + 1];
				g++;
			}
			p2p->num_groups--;
			break;
		}
	}

	p2p_group_free_members(group);
	os_free(group->cfg);
	wpabuf_free(group->noa);
	os_free(group);
}


static void p2p_client_info(struct wpabuf *ie, struct p2p_group_member *m)
{
	if (m->client_info == NULL)
		return;
	if (wpabuf_tailroom(ie) < wpabuf_len(m->client_info) + 1)
		return;
	wpabuf_put_buf(ie, m->client_info);
}


static void p2p_group_add_common_ies(struct p2p_group *group,
				     struct wpabuf *ie)
{
	u8 dev_capab = 0, group_capab = 0;

	/* P2P Capability */
	dev_capab |= P2P_DEV_CAPAB_SERVICE_DISCOVERY;
	dev_capab |= P2P_DEV_CAPAB_INVITATION_PROCEDURE;
	group_capab |= P2P_GROUP_CAPAB_GROUP_OWNER;
	if (group->cfg->persistent_group)
		group_capab |= P2P_GROUP_CAPAB_PERSISTENT_GROUP;
	if (group->p2p->cfg->p2p_intra_bss)
		group_capab |= P2P_GROUP_CAPAB_INTRA_BSS_DIST;
	if (group->group_formation)
		group_capab |= P2P_GROUP_CAPAB_GROUP_FORMATION;
	if (group->p2p->cross_connect)
		group_capab |= P2P_GROUP_CAPAB_CROSS_CONN;
	if (group->num_members >= group->cfg->max_clients)
		group_capab |= P2P_GROUP_CAPAB_GROUP_LIMIT;
	p2p_buf_add_capability(ie, dev_capab, group_capab);
}


static void p2p_group_add_noa(struct wpabuf *ie, struct wpabuf *noa)
{
	if (noa == NULL)
		return;
	/* Notice of Absence */
	wpabuf_put_u8(ie, P2P_ATTR_NOTICE_OF_ABSENCE);
	wpabuf_put_le16(ie, wpabuf_len(noa));
	wpabuf_put_buf(ie, noa);
}


static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group)
{
	struct wpabuf *ie;
	u8 *len;

	ie = wpabuf_alloc(257);
	if (ie == NULL)
		return NULL;

	len = p2p_buf_add_ie_hdr(ie);
	p2p_group_add_common_ies(group, ie);
	p2p_buf_add_device_id(ie, group->p2p->cfg->dev_addr);
	p2p_group_add_noa(ie, group->noa);
	p2p_buf_update_ie_hdr(ie, len);

	return ie;
}


static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group)
{
	u8 *group_info;
	struct wpabuf *ie;
	struct p2p_group_member *m;
	u8 *len;

	ie = wpabuf_alloc(257);
	if (ie == NULL)
		return NULL;

	len = p2p_buf_add_ie_hdr(ie);

	p2p_group_add_common_ies(group, ie);
	p2p_group_add_noa(ie, group->noa);

	/* P2P Device Info */
	p2p_buf_add_device_info(ie, group->p2p, NULL);

	/* P2P Group Info */
	group_info = wpabuf_put(ie, 0);
	wpabuf_put_u8(ie, P2P_ATTR_GROUP_INFO);
	wpabuf_put_le16(ie, 0); /* Length to be filled */
	for (m = group->members; m; m = m->next)
		p2p_client_info(ie, m);
	WPA_PUT_LE16(group_info + 1,
		     (u8 *) wpabuf_put(ie, 0) - group_info - 3);

	p2p_buf_update_ie_hdr(ie, len);
	return ie;
}


static void p2p_group_update_ies(struct p2p_group *group)
{
	struct wpabuf *beacon_ie;
	struct wpabuf *probe_resp_ie;

	probe_resp_ie = p2p_group_build_probe_resp_ie(group);
	if (probe_resp_ie == NULL)
		return;
	wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Probe Response P2P IE",
			probe_resp_ie);

	if (group->beacon_update) {
		beacon_ie = p2p_group_build_beacon_ie(group);
		if (beacon_ie)
			group->beacon_update = 0;
		wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Beacon P2P IE",
				beacon_ie);
	} else
		beacon_ie = NULL;

	group->cfg->ie_update(group->cfg->cb_ctx, beacon_ie, probe_resp_ie);
}


/**
 * p2p_build_client_info - Build P2P Client Info Descriptor
 * @addr: MAC address of the peer device
 * @p2p_ie: P2P IE from (Re)Association Request
 * @dev_capab: Buffer for returning Device Capability
 * @dev_addr: Buffer for returning P2P Device Address
 * Returns: P2P Client Info Descriptor or %NULL on failure
 *
 * This function builds P2P Client Info Descriptor based on the information
 * available from (Re)Association Request frame. Group owner can use this to
 * build the P2P Group Info attribute for Probe Response frames.
 */
static struct wpabuf * p2p_build_client_info(const u8 *addr,
					     struct wpabuf *p2p_ie,
					     u8 *dev_capab, u8 *dev_addr)
{
	const u8 *spos;
	struct p2p_message msg;
	u8 *len_pos;
	struct wpabuf *buf;

	if (p2p_ie == NULL)
		return NULL;

	os_memset(&msg, 0, sizeof(msg));
	if (p2p_parse_p2p_ie(p2p_ie, &msg) ||
	    msg.capability == NULL || msg.p2p_device_info == NULL)
		return NULL;

	buf = wpabuf_alloc(ETH_ALEN + 1 + 1 + msg.p2p_device_info_len);
	if (buf == NULL)
		return NULL;

	*dev_capab = msg.capability[0];
	os_memcpy(dev_addr, msg.p2p_device_addr, ETH_ALEN);

	spos = msg.p2p_device_info; /* P2P Device address */

	/* P2P Client Info Descriptor */
	/* Length to be set */
	len_pos = wpabuf_put(buf, 1);
	/* P2P Device address */
	wpabuf_put_data(buf, spos, ETH_ALEN);
	/* P2P Interface address */
	wpabuf_put_data(buf, addr, ETH_ALEN);
	/* Device Capability Bitmap */
	wpabuf_put_u8(buf, msg.capability[0]);
	/*
	 * Config Methods, Primary Device Type, Number of Secondary Device
	 * Types, Secondary Device Type List, Device Name copied from
	 * Device Info
	 */
	wpabuf_put_data(buf, spos + ETH_ALEN,
			msg.p2p_device_info_len - ETH_ALEN);

	*len_pos = wpabuf_len(buf) - 1;


	return buf;
}


int p2p_group_notif_assoc(struct p2p_group *group, const u8 *addr,
			  const u8 *ie, size_t len)
{
	struct p2p_group_member *m;

	if (group == NULL)
		return -1;

	m = os_zalloc(sizeof(*m));
	if (m == NULL)
		return -1;
	os_memcpy(m->addr, addr, ETH_ALEN);
	m->p2p_ie = ieee802_11_vendor_ie_concat(ie, len, P2P_IE_VENDOR_TYPE);
	if (m->p2p_ie) {
		m->client_info = p2p_build_client_info(addr, m->p2p_ie,
						       &m->dev_capab,
						       m->dev_addr);
	}

	m->next = group->members;
	group->members = m;
	group->num_members++;
	wpa_msg(group->p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Add client " MACSTR
		" to group (p2p=%d client_info=%d); num_members=%u/%u",
		MAC2STR(addr), m->p2p_ie ? 1 : 0, m->client_info ? 1 : 0,
		group->num_members, group->cfg->max_clients);
	if (group->num_members == group->cfg->max_clients)
		group->beacon_update = 1;
	p2p_group_update_ies(group);
	if (group->num_members == 1)
		group->cfg->idle_update(group->cfg->cb_ctx, 0);

	return 0;
}


struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status)
{
	struct wpabuf *resp;
	u8 *rlen;

	/*
	 * (Re)Association Response - P2P IE
	 * Status attribute (shall be present when association request is
	 *	denied)
	 * Extended Listen Timing (may be present)
	 */
	resp = wpabuf_alloc(20);
	if (resp == NULL)
		return NULL;
	rlen = p2p_buf_add_ie_hdr(resp);
	if (status != P2P_SC_SUCCESS)
		p2p_buf_add_status(resp, status);
	p2p_buf_update_ie_hdr(resp, rlen);

	return resp;
}


void p2p_group_notif_disassoc(struct p2p_group *group, const u8 *addr)
{
	struct p2p_group_member *m, *prev;

	if (group == NULL)
		return;

	m = group->members;
	prev = NULL;
	while (m) {
		if (os_memcmp(m->addr, addr, ETH_ALEN) == 0)
			break;
		prev = m;
		m = m->next;
	}

	if (m) {
		if (prev)
			prev->next = m->next;
		else
			group->members = m->next;
		p2p_group_free_member(m);
		group->num_members--;
		wpa_msg(group->p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Remove "
			"client " MACSTR " from group; num_members=%u/%u",
			MAC2STR(addr), group->num_members,
			group->cfg->max_clients);
		if (group->num_members == group->cfg->max_clients - 1)
			group->beacon_update = 1;
		p2p_group_update_ies(group);
		if (group->num_members == 0)
			group->cfg->idle_update(group->cfg->cb_ctx, 1);
	}
}


/**
 * p2p_match_dev_type_member - Match client device type with requested type
 * @m: Group member
 * @wps: WPS TLVs from Probe Request frame (concatenated WPS IEs)
 * Returns: 1 on match, 0 on mismatch
 *
 * This function can be used to match the Requested Device Type attribute in
 * WPS IE with the device types of a group member for deciding whether a GO
 * should reply to a Probe Request frame.
 */
static int p2p_match_dev_type_member(struct p2p_group_member *m,
				     struct wpabuf *wps)
{
	const u8 *pos, *end;
	struct wps_parse_attr attr;
	u8 num_sec;

	if (m->client_info == NULL || wps == NULL)
		return 0;

	pos = wpabuf_head(m->client_info);
	end = pos + wpabuf_len(m->client_info);

	pos += 1 + 2 * ETH_ALEN + 1 + 2;
	if (end - pos < WPS_DEV_TYPE_LEN + 1)
		return 0;

	if (wps_parse_msg(wps, &attr))
		return 1; /* assume no Requested Device Type attributes */

	if (attr.num_req_dev_type == 0)
		return 1; /* no Requested Device Type attributes -> match */

	if (dev_type_list_match(pos, attr.req_dev_type, attr.num_req_dev_type))
		return 1; /* Match with client Primary Device Type */

	pos += WPS_DEV_TYPE_LEN;
	num_sec = *pos++;
	if (end - pos < num_sec * WPS_DEV_TYPE_LEN)
		return 0;
	while (num_sec > 0) {
		num_sec--;
		if (dev_type_list_match(pos, attr.req_dev_type,
					attr.num_req_dev_type))
			return 1; /* Match with client Secondary Device Type */
		pos += WPS_DEV_TYPE_LEN;
	}

	/* No matching device type found */
	return 0;
}


int p2p_group_match_dev_type(struct p2p_group *group, struct wpabuf *wps)
{
	struct p2p_group_member *m;

	if (p2p_match_dev_type(group->p2p, wps))
		return 1; /* Match with own device type */

	for (m = group->members; m; m = m->next) {
		if (p2p_match_dev_type_member(m, wps))
			return 1; /* Match with group client device type */
	}

	/* No match with Requested Device Type */
	return 0;
}


void p2p_group_notif_formation_done(struct p2p_group *group)
{
	if (group == NULL)
		return;
	group->group_formation = 0;
	group->beacon_update = 1;
	p2p_group_update_ies(group);
}


int p2p_group_notif_noa(struct p2p_group *group, const u8 *noa,
			size_t noa_len)
{
	if (noa == NULL) {
		wpabuf_free(group->noa);
		group->noa = NULL;
	} else {
		if (group->noa) {
			if (wpabuf_size(group->noa) >= noa_len) {
				group->noa->size = 0;
				wpabuf_put_data(group->noa, noa, noa_len);
			} else {
				wpabuf_free(group->noa);
				group->noa = NULL;
			}
		}

		if (!group->noa) {
			group->noa = wpabuf_alloc_copy(noa, noa_len);
			if (group->noa == NULL)
				return -1;
		}
	}

	group->beacon_update = 1;
	p2p_group_update_ies(group);
	return 0;
}


static struct p2p_group_member * p2p_group_get_client(struct p2p_group *group,
						      const u8 *dev_id)
{
	struct p2p_group_member *m;

	for (m = group->members; m; m = m->next) {
		if (os_memcmp(dev_id, m->dev_addr, ETH_ALEN) == 0)
			return m;
	}

	return NULL;
}


static struct p2p_group_member * p2p_group_get_client_iface(
	struct p2p_group *group, const u8 *interface_addr)
{
	struct p2p_group_member *m;

	for (m = group->members; m; m = m->next) {
		if (os_memcmp(interface_addr, m->addr, ETH_ALEN) == 0)
			return m;
	}

	return NULL;
}


static struct wpabuf * p2p_build_go_disc_req(void)
{
	struct wpabuf *buf;

	buf = wpabuf_alloc(100);
	if (buf == NULL)
		return NULL;

	p2p_buf_add_action_hdr(buf, P2P_GO_DISC_REQ, 0);

	return buf;
}


int p2p_group_go_discover(struct p2p_group *group, const u8 *dev_id,
			  const u8 *searching_dev, int rx_freq)
{
	struct p2p_group_member *m;
	struct wpabuf *req;
	struct p2p_data *p2p = group->p2p;
	int freq;

	m = p2p_group_get_client(group, dev_id);
	if (m == NULL || m->client_info == NULL) {
		wpa_printf(MSG_DEBUG, "P2P: Requested client was not in this "
			   "group " MACSTR,
			   MAC2STR(group->cfg->interface_addr));
		return -1;
	}

	if (!(m->dev_capab & P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY)) {
		wpa_printf(MSG_DEBUG, "P2P: Requested client does not support "
			   "client discoverability");
		return -1;
	}

	wpa_printf(MSG_DEBUG, "P2P: Schedule GO Discoverability Request to be "
		   "sent to " MACSTR, MAC2STR(dev_id));

	req = p2p_build_go_disc_req();
	if (req == NULL)
		return -1;

	/* TODO: Should really use group operating frequency here */
	freq = rx_freq;

	p2p->pending_action_state = P2P_PENDING_GO_DISC_REQ;
	if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, m->addr,
				  group->cfg->interface_addr,
				  group->cfg->interface_addr,
				  wpabuf_head(req), wpabuf_len(req), 200) < 0)
	{
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Failed to send Action frame");
	}

	wpabuf_free(req);

	return 0;
}


const u8 * p2p_group_get_interface_addr(struct p2p_group *group)
{
	return group->cfg->interface_addr;
}


u8 p2p_group_presence_req(struct p2p_group *group,
			  const u8 *client_interface_addr,
			  const u8 *noa, size_t noa_len)
{
	struct p2p_group_member *m;
	u8 curr_noa[50];
	int curr_noa_len;

	m = p2p_group_get_client_iface(group, client_interface_addr);
	if (m == NULL || m->client_info == NULL) {
		wpa_printf(MSG_DEBUG, "P2P: Client was not in this group");
		return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
	}

	wpa_hexdump(MSG_DEBUG, "P2P: Presence Request NoA", noa, noa_len);

	if (group->p2p->cfg->get_noa)
		curr_noa_len = group->p2p->cfg->get_noa(
			group->p2p->cfg->cb_ctx, group->cfg->interface_addr,
			curr_noa, sizeof(curr_noa));
	else
		curr_noa_len = -1;
	if (curr_noa_len < 0)
		wpa_printf(MSG_DEBUG, "P2P: Failed to fetch current NoA");
	else if (curr_noa_len == 0)
		wpa_printf(MSG_DEBUG, "P2P: No NoA being advertized");
	else
		wpa_hexdump(MSG_DEBUG, "P2P: Current NoA", curr_noa,
			    curr_noa_len);

	/* TODO: properly process request and store copy */
	if (curr_noa_len > 0)
		return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;

	return P2P_SC_SUCCESS;
}
Commit	Line	Data
b22128ef JM	1	/*
	2	* Wi-Fi Direct - P2P group operations
	3	* Copyright (c) 2009-2010, Atheros Communications
	4	*
	5	* This program is free software; you can redistribute it and/or modify
	6	* it under the terms of the GNU General Public License version 2 as
	7	* published by the Free Software Foundation.
	8	*
	9	* Alternatively, this software may be distributed under the terms of BSD
	10	* license.
	11	*
	12	* See README and COPYING for more details.
	13	*/
	14
	15	#include "includes.h"
	16
	17	#include "common.h"
	18	#include "common/ieee802_11_defs.h"
	19	#include "common/ieee802_11_common.h"
	20	#include "wps/wps_defs.h"
	21	#include "wps/wps_i.h"
	22	#include "p2p_i.h"
	23	#include "p2p.h"
	24
	25
	26	struct p2p_group_member {
	27	struct p2p_group_member *next;
	28	u8 addr[ETH_ALEN]; /* P2P Interface Address */
	29	u8 dev_addr[ETH_ALEN]; /* P2P Device Address */
	30	struct wpabuf *p2p_ie;
	31	struct wpabuf *client_info;
	32	u8 dev_capab;
	33	};
	34
	35	/**
	36	* struct p2p_group - Internal P2P module per-group data
	37	*/
	38	struct p2p_group {
	39	struct p2p_data *p2p;
	40	struct p2p_group_config *cfg;
	41	struct p2p_group_member *members;
3f4ce13f	42	unsigned int num_members;
b22128ef JM	43	int group_formation;
	44	int beacon_update;
	45	struct wpabuf *noa;
	46	};
	47
	48
	49	static void p2p_group_update_ies(struct p2p_group *group);
	50
	51
	52	struct p2p_group * p2p_group_init(struct p2p_data *p2p,
	53	struct p2p_group_config *config)
	54	{
	55	struct p2p_group group, *groups;
	56
	57	group = os_zalloc(sizeof(*group));
	58	if (group == NULL)
	59	return NULL;
	60
	61	groups = os_realloc(p2p->groups, (p2p->num_groups + 1) *
	62	sizeof(struct p2p_group *));
	63	if (groups == NULL) {
	64	os_free(group);
	65	return NULL;
	66	}
	67	groups[p2p->num_groups++] = group;
	68	p2p->groups = groups;
	69
	70	group->p2p = p2p;
	71	group->cfg = config;
	72	group->group_formation = 1;
	73	group->beacon_update = 1;
	74	p2p_group_update_ies(group);
3071e181	75	group->cfg->idle_update(group->cfg->cb_ctx, 1);
b22128ef JM	76
	77	return group;
	78	}
	79
	80
	81	static void p2p_group_free_member(struct p2p_group_member *m)
	82	{
	83	wpabuf_free(m->p2p_ie);
	84	wpabuf_free(m->client_info);
	85	os_free(m);
	86	}
	87
	88
	89	static void p2p_group_free_members(struct p2p_group *group)
	90	{
	91	struct p2p_group_member m, prev;
	92	m = group->members;
	93	group->members = NULL;
3f4ce13f	94	group->num_members = 0;
b22128ef JM	95	while (m) {
	96	prev = m;
	97	m = m->next;
	98	p2p_group_free_member(prev);
	99	}
	100	}
	101
	102
	103	void p2p_group_deinit(struct p2p_group *group)
	104	{
	105	size_t g;
0cc8be3e	106	struct p2p_data *p2p;
b22128ef JM	107
	108	if (group == NULL)
	109	return;
	110
0cc8be3e JM	111	p2p = group->p2p;
0cc8be3e JM	112
b22128ef JM	113	for (g = 0; g < p2p->num_groups; g++) {
	114	if (p2p->groups[g] == group) {
	115	while (g + 1 < p2p->num_groups) {
	116	p2p->groups[g] = p2p->groups[g + 1];
	117	g++;
	118	}
	119	p2p->num_groups--;
	120	break;
	121	}
	122	}
	123
	124	p2p_group_free_members(group);
	125	os_free(group->cfg);
	126	wpabuf_free(group->noa);
	127	os_free(group);
	128	}
	129
	130
	131	static void p2p_client_info(struct wpabuf ie, struct p2p_group_member m)
	132	{
3f4ce13f JM	133	if (m->client_info == NULL)
3f4ce13f JM	134	return;
b22128ef JM	135	if (wpabuf_tailroom(ie) < wpabuf_len(m->client_info) + 1)
	136	return;
	137	wpabuf_put_buf(ie, m->client_info);
	138	}
	139
	140
	141	static void p2p_group_add_common_ies(struct p2p_group *group,
	142	struct wpabuf *ie)
	143	{
	144	u8 dev_capab = 0, group_capab = 0;
	145
	146	/* P2P Capability */
	147	dev_capab \|= P2P_DEV_CAPAB_SERVICE_DISCOVERY;
	148	dev_capab \|= P2P_DEV_CAPAB_INVITATION_PROCEDURE;
	149	group_capab \|= P2P_GROUP_CAPAB_GROUP_OWNER;
	150	if (group->cfg->persistent_group)
	151	group_capab \|= P2P_GROUP_CAPAB_PERSISTENT_GROUP;
0f66abd2 SS	152	if (group->p2p->cfg->p2p_intra_bss)
0f66abd2 SS	153	group_capab \|= P2P_GROUP_CAPAB_INTRA_BSS_DIST;
b22128ef JM	154	if (group->group_formation)
b22128ef JM	155	group_capab \|= P2P_GROUP_CAPAB_GROUP_FORMATION;
72044390 JM	156	if (group->p2p->cross_connect)
72044390 JM	157	group_capab \|= P2P_GROUP_CAPAB_CROSS_CONN;
3f4ce13f JM	158	if (group->num_members >= group->cfg->max_clients)
3f4ce13f JM	159	group_capab \|= P2P_GROUP_CAPAB_GROUP_LIMIT;
b22128ef JM	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_build_beacon_ie(struct p2p_group *group)
	176	{
	177	struct wpabuf *ie;
	178	u8 *len;
	179
	180	ie = wpabuf_alloc(257);
	181	if (ie == NULL)
	182	return NULL;
	183
	184	len = p2p_buf_add_ie_hdr(ie);
	185	p2p_group_add_common_ies(group, ie);
	186	p2p_buf_add_device_id(ie, group->p2p->cfg->dev_addr);
	187	p2p_group_add_noa(ie, group->noa);
	188	p2p_buf_update_ie_hdr(ie, len);
	189
	190	return ie;
	191	}
	192
	193
	194	static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group)
	195	{
	196	u8 *group_info;
	197	struct wpabuf *ie;
	198	struct p2p_group_member *m;
	199	u8 *len;
	200
	201	ie = wpabuf_alloc(257);
	202	if (ie == NULL)
	203	return NULL;
	204
	205	len = p2p_buf_add_ie_hdr(ie);
	206
	207	p2p_group_add_common_ies(group, ie);
	208	p2p_group_add_noa(ie, group->noa);
	209
	210	/* P2P Device Info */
	211	p2p_buf_add_device_info(ie, group->p2p, NULL);
	212
55a62579 JM	213	/* P2P Group Info */
	214	group_info = wpabuf_put(ie, 0);
	215	wpabuf_put_u8(ie, P2P_ATTR_GROUP_INFO);
	216	wpabuf_put_le16(ie, 0); /* Length to be filled */
	217	for (m = group->members; m; m = m->next)
	218	p2p_client_info(ie, m);
	219	WPA_PUT_LE16(group_info + 1,
	220	(u8 *) wpabuf_put(ie, 0) - group_info - 3);
b22128ef JM	221
	222	p2p_buf_update_ie_hdr(ie, len);
	223	return ie;
	224	}
	225
	226
	227	static void p2p_group_update_ies(struct p2p_group *group)
	228	{
	229	struct wpabuf *beacon_ie;
	230	struct wpabuf *probe_resp_ie;
	231
	232	probe_resp_ie = p2p_group_build_probe_resp_ie(group);
	233	if (probe_resp_ie == NULL)
	234	return;
	235	wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Probe Response P2P IE",
	236	probe_resp_ie);
	237
	238	if (group->beacon_update) {
	239	beacon_ie = p2p_group_build_beacon_ie(group);
	240	if (beacon_ie)
	241	group->beacon_update = 0;
	242	wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Beacon P2P IE",
	243	beacon_ie);
	244	} else
	245	beacon_ie = NULL;
	246
	247	group->cfg->ie_update(group->cfg->cb_ctx, beacon_ie, probe_resp_ie);
	248	}
	249
	250
	251	/**
	252	* p2p_build_client_info - Build P2P Client Info Descriptor
	253	* @addr: MAC address of the peer device
	254	* @p2p_ie: P2P IE from (Re)Association Request
	255	* @dev_capab: Buffer for returning Device Capability
	256	* @dev_addr: Buffer for returning P2P Device Address
	257	* Returns: P2P Client Info Descriptor or %NULL on failure
	258	*
	259	* This function builds P2P Client Info Descriptor based on the information
	260	* available from (Re)Association Request frame. Group owner can use this to
	261	* build the P2P Group Info attribute for Probe Response frames.
	262	*/
	263	static struct wpabuf * p2p_build_client_info(const u8 *addr,
	264	struct wpabuf *p2p_ie,
	265	u8 dev_capab, u8 dev_addr)
	266	{
	267	const u8 *spos;
	268	struct p2p_message msg;
	269	u8 *len_pos;
	270	struct wpabuf *buf;
	271
	272	if (p2p_ie == NULL)
	273	return NULL;
	274
	275	os_memset(&msg, 0, sizeof(msg));
	276	if (p2p_parse_p2p_ie(p2p_ie, &msg) \|\|
	277	msg.capability == NULL \|\| msg.p2p_device_info == NULL)
	278	return NULL;
	279
	280	buf = wpabuf_alloc(ETH_ALEN + 1 + 1 + msg.p2p_device_info_len);
	281	if (buf == NULL)
	282	return NULL;
	283
	284	*dev_capab = msg.capability[0];
285	os_memcpy(dev_addr, msg.p2p_device_addr, ETH_ALEN);
286
287	spos = msg.p2p_device_info; /* P2P Device address */
288
289	/* P2P Client Info Descriptor */
290	/* Length to be set */
291	len_pos = wpabuf_put(buf, 1);
292	/* P2P Device address */
293	wpabuf_put_data(buf, spos, ETH_ALEN);
294	/* P2P Interface address */
295	wpabuf_put_data(buf, addr, ETH_ALEN);
296	/* Device Capability Bitmap */
297	wpabuf_put_u8(buf, msg.capability[0]);
298	/*
299	* Config Methods, Primary Device Type, Number of Secondary Device
300	* Types, Secondary Device Type List, Device Name copied from
301	* Device Info
302	*/
303	wpabuf_put_data(buf, spos + ETH_ALEN,
304	msg.p2p_device_info_len - ETH_ALEN);
305
306	*len_pos = wpabuf_len(buf) - 1;
307
308
309	return buf;
310	}
311
312
313	int p2p_group_notif_assoc(struct p2p_group group, const u8 addr,
314	const u8 *ie, size_t len)
315	{
316	struct p2p_group_member *m;
317
318	if (group == NULL)
319	return -1;
320
321	m = os_zalloc(sizeof(*m));
322	if (m == NULL)
323	return -1;
324	os_memcpy(m->addr, addr, ETH_ALEN);
325	m->p2p_ie = ieee802_11_vendor_ie_concat(ie, len, P2P_IE_VENDOR_TYPE);
3f4ce13f JM	326	if (m->p2p_ie) {
	327	m->client_info = p2p_build_client_info(addr, m->p2p_ie,
	328	&m->dev_capab,
	329	m->dev_addr);
b22128ef JM	330	}
	331
	332	m->next = group->members;
	333	group->members = m;
3f4ce13f JM	334	group->num_members++;
	335	wpa_msg(group->p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Add client " MACSTR
	336	" to group (p2p=%d client_info=%d); num_members=%u/%u",
	337	MAC2STR(addr), m->p2p_ie ? 1 : 0, m->client_info ? 1 : 0,
	338	group->num_members, group->cfg->max_clients);
	339	if (group->num_members == group->cfg->max_clients)
	340	group->beacon_update = 1;
b22128ef	341	p2p_group_update_ies(group);
3071e181 JM	342	if (group->num_members == 1)
3071e181 JM	343	group->cfg->idle_update(group->cfg->cb_ctx, 0);
b22128ef JM	344
	345	return 0;
	346	}
	347
	348
	349	struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status)
	350	{
	351	struct wpabuf *resp;
	352	u8 *rlen;
	353
	354	/*
	355	* (Re)Association Response - P2P IE
	356	* Status attribute (shall be present when association request is
	357	* denied)
	358	* Extended Listen Timing (may be present)
	359	*/
	360	resp = wpabuf_alloc(20);
	361	if (resp == NULL)
	362	return NULL;
	363	rlen = p2p_buf_add_ie_hdr(resp);
	364	if (status != P2P_SC_SUCCESS)
	365	p2p_buf_add_status(resp, status);
	366	p2p_buf_update_ie_hdr(resp, rlen);
	367
	368	return resp;
	369	}
	370
	371
	372	void p2p_group_notif_disassoc(struct p2p_group group, const u8 addr)
	373	{
	374	struct p2p_group_member m, prev;
	375
	376	if (group == NULL)
	377	return;
	378
	379	m = group->members;
	380	prev = NULL;
	381	while (m) {
	382	if (os_memcmp(m->addr, addr, ETH_ALEN) == 0)
	383	break;
	384	prev = m;
	385	m = m->next;
	386	}
	387
	388	if (m) {
	389	if (prev)
	390	prev->next = m->next;
	391	else
	392	group->members = m->next;
	393	p2p_group_free_member(m);
3f4ce13f JM	394	group->num_members--;
	395	wpa_msg(group->p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Remove "
	396	"client " MACSTR " from group; num_members=%u/%u",
	397	MAC2STR(addr), group->num_members,
	398	group->cfg->max_clients);
	399	if (group->num_members == group->cfg->max_clients - 1)
	400	group->beacon_update = 1;
b22128ef	401	p2p_group_update_ies(group);
3071e181 JM	402	if (group->num_members == 0)
3071e181 JM	403	group->cfg->idle_update(group->cfg->cb_ctx, 1);
b22128ef JM	404	}
	405	}
	406
	407
	408	/**
	409	* p2p_match_dev_type_member - Match client device type with requested type
	410	* @m: Group member
	411	* @wps: WPS TLVs from Probe Request frame (concatenated WPS IEs)
	412	* Returns: 1 on match, 0 on mismatch
	413	*
	414	* This function can be used to match the Requested Device Type attribute in
	415	* WPS IE with the device types of a group member for deciding whether a GO
	416	* should reply to a Probe Request frame.
	417	*/
	418	static int p2p_match_dev_type_member(struct p2p_group_member *m,
	419	struct wpabuf *wps)
	420	{
	421	const u8 pos, end;
	422	struct wps_parse_attr attr;
	423	u8 num_sec;
	424
	425	if (m->client_info == NULL \|\| wps == NULL)
	426	return 0;
	427
	428	pos = wpabuf_head(m->client_info);
	429	end = pos + wpabuf_len(m->client_info);
	430
	431	pos += 1 + 2 * ETH_ALEN + 1 + 2;
	432	if (end - pos < WPS_DEV_TYPE_LEN + 1)
	433	return 0;
	434
	435	if (wps_parse_msg(wps, &attr))
	436	return 1; /* assume no Requested Device Type attributes */
	437
	438	if (attr.num_req_dev_type == 0)
	439	return 1; /* no Requested Device Type attributes -> match */
	440
	441	if (dev_type_list_match(pos, attr.req_dev_type, attr.num_req_dev_type))
	442	return 1; /* Match with client Primary Device Type */
	443
	444	pos += WPS_DEV_TYPE_LEN;
	445	num_sec = *pos++;
	446	if (end - pos < num_sec * WPS_DEV_TYPE_LEN)
	447	return 0;
	448	while (num_sec > 0) {
	449	num_sec--;
	450	if (dev_type_list_match(pos, attr.req_dev_type,
	451	attr.num_req_dev_type))
	452	return 1; /* Match with client Secondary Device Type */
	453	pos += WPS_DEV_TYPE_LEN;
	454	}
	455
	456	/* No matching device type found */
	457	return 0;
	458	}
	459
	460
	461	int p2p_group_match_dev_type(struct p2p_group group, struct wpabuf wps)
	462	{
	463	struct p2p_group_member *m;
	464
	465	if (p2p_match_dev_type(group->p2p, wps))
	466	return 1; /* Match with own device type */
	467
468	for (m = group->members; m; m = m->next) {
469	if (p2p_match_dev_type_member(m, wps))
470	return 1; /* Match with group client device type */
471	}
472
473	/* No match with Requested Device Type */
474	return 0;
475	}
476
477
478	void p2p_group_notif_formation_done(struct p2p_group *group)
479	{
480	if (group == NULL)
481	return;
482	group->group_formation = 0;
483	group->beacon_update = 1;
484	p2p_group_update_ies(group);
485	}
486
487
488	int p2p_group_notif_noa(struct p2p_group group, const u8 noa,
489	size_t noa_len)
490	{
491	if (noa == NULL) {
492	wpabuf_free(group->noa);
493	group->noa = NULL;
494	} else {
495	if (group->noa) {
496	if (wpabuf_size(group->noa) >= noa_len) {
497	group->noa->size = 0;
498	wpabuf_put_data(group->noa, noa, noa_len);
499	} else {
500	wpabuf_free(group->noa);
501	group->noa = NULL;
502	}
503	}
504
505	if (!group->noa) {
506	group->noa = wpabuf_alloc_copy(noa, noa_len);
507	if (group->noa == NULL)
508	return -1;
509	}
510	}
511
512	group->beacon_update = 1;
513	p2p_group_update_ies(group);
514	return 0;
515	}
516
517
518	static struct p2p_group_member * p2p_group_get_client(struct p2p_group *group,
519	const u8 *dev_id)
520	{
521	struct p2p_group_member *m;
522
523	for (m = group->members; m; m = m->next) {
524	if (os_memcmp(dev_id, m->dev_addr, ETH_ALEN) == 0)
525	return m;
526	}
527
528	return NULL;
529	}
530
531
532	static struct p2p_group_member * p2p_group_get_client_iface(
533	struct p2p_group group, const u8 interface_addr)
534	{
535	struct p2p_group_member *m;
536
537	for (m = group->members; m; m = m->next) {
538	if (os_memcmp(interface_addr, m->addr, ETH_ALEN) == 0)
539	return m;
540	}
541
542	return NULL;
543	}
544
545
546	static struct wpabuf * p2p_build_go_disc_req(void)
547	{
548	struct wpabuf *buf;
549
550	buf = wpabuf_alloc(100);
551	if (buf == NULL)
552	return NULL;
553
554	p2p_buf_add_action_hdr(buf, P2P_GO_DISC_REQ, 0);
555
556	return buf;
557	}
558
559
560	int p2p_group_go_discover(struct p2p_group group, const u8 dev_id,
561	const u8 *searching_dev, int rx_freq)
562	{
563	struct p2p_group_member *m;
564	struct wpabuf *req;
565	struct p2p_data *p2p = group->p2p;
566	int freq;
567
568	m = p2p_group_get_client(group, dev_id);
3f4ce13f	569	if (m == NULL \|\| m->client_info == NULL) {
b22128ef JM	570	wpa_printf(MSG_DEBUG, "P2P: Requested client was not in this "
	571	"group " MACSTR,
	572	MAC2STR(group->cfg->interface_addr));
	573	return -1;
	574	}
	575
	576	if (!(m->dev_capab & P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY)) {
	577	wpa_printf(MSG_DEBUG, "P2P: Requested client does not support "
	578	"client discoverability");
	579	return -1;
	580	}
	581
	582	wpa_printf(MSG_DEBUG, "P2P: Schedule GO Discoverability Request to be "
	583	"sent to " MACSTR, MAC2STR(dev_id));
	584
	585	req = p2p_build_go_disc_req();
	586	if (req == NULL)
	587	return -1;
	588
	589	/* TODO: Should really use group operating frequency here */
	590	freq = rx_freq;
	591
	592	p2p->pending_action_state = P2P_PENDING_GO_DISC_REQ;
	593	if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, m->addr,
	594	group->cfg->interface_addr,
	595	group->cfg->interface_addr,
	596	wpabuf_head(req), wpabuf_len(req), 200) < 0)
	597	{
	598	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
	599	"P2P: Failed to send Action frame");
	600	}
	601
	602	wpabuf_free(req);
	603
	604	return 0;
	605	}
	606
	607
	608	const u8 * p2p_group_get_interface_addr(struct p2p_group *group)
	609	{
	610	return group->cfg->interface_addr;
	611	}
	612
	613
	614	u8 p2p_group_presence_req(struct p2p_group *group,
	615	const u8 *client_interface_addr,
	616	const u8 *noa, size_t noa_len)
	617	{
	618	struct p2p_group_member *m;
	619	u8 curr_noa[50];
	620	int curr_noa_len;
	621
	622	m = p2p_group_get_client_iface(group, client_interface_addr);
3f4ce13f	623	if (m == NULL \|\| m->client_info == NULL) {
b22128ef JM	624	wpa_printf(MSG_DEBUG, "P2P: Client was not in this group");
	625	return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
	626	}
	627
	628	wpa_hexdump(MSG_DEBUG, "P2P: Presence Request NoA", noa, noa_len);
	629
	630	if (group->p2p->cfg->get_noa)
	631	curr_noa_len = group->p2p->cfg->get_noa(
	632	group->p2p->cfg->cb_ctx, group->cfg->interface_addr,
	633	curr_noa, sizeof(curr_noa));
	634	else
	635	curr_noa_len = -1;
	636	if (curr_noa_len < 0)
	637	wpa_printf(MSG_DEBUG, "P2P: Failed to fetch current NoA");
	638	else if (curr_noa_len == 0)
	639	wpa_printf(MSG_DEBUG, "P2P: No NoA being advertized");
	640	else
	641	wpa_hexdump(MSG_DEBUG, "P2P: Current NoA", curr_noa,
	642	curr_noa_len);
	643
	644	/* TODO: properly process request and store copy */
	645	if (curr_noa_len > 0)
	646	return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
	647
	648	return P2P_SC_SUCCESS;
	649	}