[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->cfg->persistent_group == 2)
			group_capab |= P2P_GROUP_CAPAB_PERSISTENT_RECONN;
	}
	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;
}


static int p2p_group_remove_member(struct p2p_group *group, const u8 *addr)
{
	struct p2p_group_member *m, *prev;

	if (group == NULL)
		return 0;

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

	if (m == NULL)
		return 0;

	if (prev)
		prev->next = m->next;
	else
		group->members = m->next;
	p2p_group_free_member(m);
	group->num_members--;

	return 1;
}


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);
	}

	p2p_group_remove_member(group, 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)
{
	if (p2p_group_remove_member(group, addr)) {
		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->used = 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;
}


const u8 * p2p_group_get_dev_addr(struct p2p_group *group, const u8 *addr)
{
	struct p2p_group_member *m;

	if (group == NULL)
		return NULL;
	m = p2p_group_get_client_iface(group, addr);
	if (m && !is_zero_ether_addr(m->dev_addr))
		return m->dev_addr;
	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;
}


unsigned int p2p_get_group_num_members(struct p2p_group *group)
{
	return group->num_members;
}


const u8 * p2p_iterate_group_members(struct p2p_group *group, void **next)
{
	struct p2p_group_member *iter = *next;

	if (!iter)
		iter = group->members;
	else
		iter = iter->next;

	*next = iter;

	if (!iter)
		return NULL;

	return iter->addr;
}
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;
acc247b2	150	if (group->cfg->persistent_group) {
b22128ef	151	group_capab \|= P2P_GROUP_CAPAB_PERSISTENT_GROUP;
acc247b2 JM	152	if (group->cfg->persistent_group == 2)
	153	group_capab \|= P2P_GROUP_CAPAB_PERSISTENT_RECONN;
	154	}
0f66abd2 SS	155	if (group->p2p->cfg->p2p_intra_bss)
0f66abd2 SS	156	group_capab \|= P2P_GROUP_CAPAB_INTRA_BSS_DIST;
b22128ef JM	157	if (group->group_formation)
b22128ef JM	158	group_capab \|= P2P_GROUP_CAPAB_GROUP_FORMATION;
72044390 JM	159	if (group->p2p->cross_connect)
72044390 JM	160	group_capab \|= P2P_GROUP_CAPAB_CROSS_CONN;
3f4ce13f JM	161	if (group->num_members >= group->cfg->max_clients)
3f4ce13f JM	162	group_capab \|= P2P_GROUP_CAPAB_GROUP_LIMIT;
b22128ef JM	163	p2p_buf_add_capability(ie, dev_capab, group_capab);
	164	}
	165
	166
	167	static void p2p_group_add_noa(struct wpabuf ie, struct wpabuf noa)
	168	{
	169	if (noa == NULL)
	170	return;
	171	/* Notice of Absence */
	172	wpabuf_put_u8(ie, P2P_ATTR_NOTICE_OF_ABSENCE);
	173	wpabuf_put_le16(ie, wpabuf_len(noa));
	174	wpabuf_put_buf(ie, noa);
	175	}
	176
	177
	178	static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group)
	179	{
	180	struct wpabuf *ie;
	181	u8 *len;
	182
	183	ie = wpabuf_alloc(257);
	184	if (ie == NULL)
	185	return NULL;
	186
	187	len = p2p_buf_add_ie_hdr(ie);
	188	p2p_group_add_common_ies(group, ie);
	189	p2p_buf_add_device_id(ie, group->p2p->cfg->dev_addr);
	190	p2p_group_add_noa(ie, group->noa);
	191	p2p_buf_update_ie_hdr(ie, len);
	192
	193	return ie;
	194	}
	195
	196
	197	static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group)
	198	{
	199	u8 *group_info;
	200	struct wpabuf *ie;
	201	struct p2p_group_member *m;
	202	u8 *len;
	203
	204	ie = wpabuf_alloc(257);
	205	if (ie == NULL)
	206	return NULL;
	207
	208	len = p2p_buf_add_ie_hdr(ie);
	209
	210	p2p_group_add_common_ies(group, ie);
	211	p2p_group_add_noa(ie, group->noa);
	212
	213	/* P2P Device Info */
	214	p2p_buf_add_device_info(ie, group->p2p, NULL);
	215
55a62579 JM	216	/* P2P Group Info */
	217	group_info = wpabuf_put(ie, 0);
	218	wpabuf_put_u8(ie, P2P_ATTR_GROUP_INFO);
	219	wpabuf_put_le16(ie, 0); /* Length to be filled */
	220	for (m = group->members; m; m = m->next)
	221	p2p_client_info(ie, m);
	222	WPA_PUT_LE16(group_info + 1,
	223	(u8 *) wpabuf_put(ie, 0) - group_info - 3);
b22128ef JM	224
	225	p2p_buf_update_ie_hdr(ie, len);
	226	return ie;
	227	}
	228
	229
	230	static void p2p_group_update_ies(struct p2p_group *group)
	231	{
	232	struct wpabuf *beacon_ie;
	233	struct wpabuf *probe_resp_ie;
	234
	235	probe_resp_ie = p2p_group_build_probe_resp_ie(group);
	236	if (probe_resp_ie == NULL)
	237	return;
	238	wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Probe Response P2P IE",
	239	probe_resp_ie);
	240
	241	if (group->beacon_update) {
	242	beacon_ie = p2p_group_build_beacon_ie(group);
	243	if (beacon_ie)
	244	group->beacon_update = 0;
	245	wpa_hexdump_buf(MSG_MSGDUMP, "P2P: Update GO Beacon P2P IE",
	246	beacon_ie);
	247	} else
	248	beacon_ie = NULL;
	249
	250	group->cfg->ie_update(group->cfg->cb_ctx, beacon_ie, probe_resp_ie);
	251	}
	252
	253
	254	/**
	255	* p2p_build_client_info - Build P2P Client Info Descriptor
	256	* @addr: MAC address of the peer device
	257	* @p2p_ie: P2P IE from (Re)Association Request
	258	* @dev_capab: Buffer for returning Device Capability
	259	* @dev_addr: Buffer for returning P2P Device Address
	260	* Returns: P2P Client Info Descriptor or %NULL on failure
	261	*
	262	* This function builds P2P Client Info Descriptor based on the information
	263	* available from (Re)Association Request frame. Group owner can use this to
	264	* build the P2P Group Info attribute for Probe Response frames.
	265	*/
	266	static struct wpabuf * p2p_build_client_info(const u8 *addr,
	267	struct wpabuf *p2p_ie,
	268	u8 dev_capab, u8 dev_addr)
	269	{
	270	const u8 *spos;
	271	struct p2p_message msg;
	272	u8 *len_pos;
	273	struct wpabuf *buf;
	274
	275	if (p2p_ie == NULL)
	276	return NULL;
	277
	278	os_memset(&msg, 0, sizeof(msg));
	279	if (p2p_parse_p2p_ie(p2p_ie, &msg) \|\|
	280	msg.capability == NULL \|\| msg.p2p_device_info == NULL)
	281	return NULL;
	282
	283	buf = wpabuf_alloc(ETH_ALEN + 1 + 1 + msg.p2p_device_info_len);
	284	if (buf == NULL)
	285	return NULL;
	286
	287	*dev_capab = msg.capability[0];
288	os_memcpy(dev_addr, msg.p2p_device_addr, ETH_ALEN);
289
290	spos = msg.p2p_device_info; /* P2P Device address */
291
292	/* P2P Client Info Descriptor */
293	/* Length to be set */
294	len_pos = wpabuf_put(buf, 1);
295	/* P2P Device address */
296	wpabuf_put_data(buf, spos, ETH_ALEN);
297	/* P2P Interface address */
298	wpabuf_put_data(buf, addr, ETH_ALEN);
299	/* Device Capability Bitmap */
300	wpabuf_put_u8(buf, msg.capability[0]);
301	/*
302	* Config Methods, Primary Device Type, Number of Secondary Device
303	* Types, Secondary Device Type List, Device Name copied from
304	* Device Info
305	*/
306	wpabuf_put_data(buf, spos + ETH_ALEN,
307	msg.p2p_device_info_len - ETH_ALEN);
308
309	*len_pos = wpabuf_len(buf) - 1;
310
311
312	return buf;
313	}
314
315
38dcca9a JB	316	static int p2p_group_remove_member(struct p2p_group group, const u8 addr)
	317	{
	318	struct p2p_group_member m, prev;
	319
	320	if (group == NULL)
	321	return 0;
	322
	323	m = group->members;
	324	prev = NULL;
	325	while (m) {
	326	if (os_memcmp(m->addr, addr, ETH_ALEN) == 0)
	327	break;
	328	prev = m;
	329	m = m->next;
	330	}
	331
	332	if (m == NULL)
	333	return 0;
	334
	335	if (prev)
	336	prev->next = m->next;
	337	else
	338	group->members = m->next;
	339	p2p_group_free_member(m);
	340	group->num_members--;
	341
	342	return 1;
	343	}
	344
	345
b22128ef JM	346	int p2p_group_notif_assoc(struct p2p_group group, const u8 addr,
	347	const u8 *ie, size_t len)
	348	{
	349	struct p2p_group_member *m;
	350
	351	if (group == NULL)
	352	return -1;
	353
	354	m = os_zalloc(sizeof(*m));
	355	if (m == NULL)
	356	return -1;
	357	os_memcpy(m->addr, addr, ETH_ALEN);
	358	m->p2p_ie = ieee802_11_vendor_ie_concat(ie, len, P2P_IE_VENDOR_TYPE);
3f4ce13f JM	359	if (m->p2p_ie) {
	360	m->client_info = p2p_build_client_info(addr, m->p2p_ie,
	361	&m->dev_capab,
	362	m->dev_addr);
b22128ef JM	363	}
b22128ef JM	364
38dcca9a JB	365	p2p_group_remove_member(group, addr);
38dcca9a JB	366
b22128ef JM	367	m->next = group->members;
b22128ef JM	368	group->members = m;
3f4ce13f JM	369	group->num_members++;
	370	wpa_msg(group->p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Add client " MACSTR
	371	" to group (p2p=%d client_info=%d); num_members=%u/%u",
	372	MAC2STR(addr), m->p2p_ie ? 1 : 0, m->client_info ? 1 : 0,
	373	group->num_members, group->cfg->max_clients);
	374	if (group->num_members == group->cfg->max_clients)
	375	group->beacon_update = 1;
b22128ef	376	p2p_group_update_ies(group);
3071e181 JM	377	if (group->num_members == 1)
3071e181 JM	378	group->cfg->idle_update(group->cfg->cb_ctx, 0);
b22128ef JM	379
	380	return 0;
	381	}
	382
	383
	384	struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status)
	385	{
	386	struct wpabuf *resp;
	387	u8 *rlen;
	388
	389	/*
	390	* (Re)Association Response - P2P IE
	391	* Status attribute (shall be present when association request is
	392	* denied)
	393	* Extended Listen Timing (may be present)
	394	*/
	395	resp = wpabuf_alloc(20);
	396	if (resp == NULL)
	397	return NULL;
	398	rlen = p2p_buf_add_ie_hdr(resp);
	399	if (status != P2P_SC_SUCCESS)
	400	p2p_buf_add_status(resp, status);
	401	p2p_buf_update_ie_hdr(resp, rlen);
	402
	403	return resp;
	404	}
	405
	406
	407	void p2p_group_notif_disassoc(struct p2p_group group, const u8 addr)
	408	{
38dcca9a	409	if (p2p_group_remove_member(group, addr)) {
3f4ce13f JM	410	wpa_msg(group->p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Remove "
	411	"client " MACSTR " from group; num_members=%u/%u",
	412	MAC2STR(addr), group->num_members,
	413	group->cfg->max_clients);
	414	if (group->num_members == group->cfg->max_clients - 1)
	415	group->beacon_update = 1;
b22128ef	416	p2p_group_update_ies(group);
3071e181 JM	417	if (group->num_members == 0)
3071e181 JM	418	group->cfg->idle_update(group->cfg->cb_ctx, 1);
b22128ef JM	419	}
	420	}
	421
	422
	423	/**
	424	* p2p_match_dev_type_member - Match client device type with requested type
	425	* @m: Group member
	426	* @wps: WPS TLVs from Probe Request frame (concatenated WPS IEs)
	427	* Returns: 1 on match, 0 on mismatch
	428	*
	429	* This function can be used to match the Requested Device Type attribute in
	430	* WPS IE with the device types of a group member for deciding whether a GO
	431	* should reply to a Probe Request frame.
	432	*/
	433	static int p2p_match_dev_type_member(struct p2p_group_member *m,
	434	struct wpabuf *wps)
	435	{
	436	const u8 pos, end;
	437	struct wps_parse_attr attr;
	438	u8 num_sec;
	439
	440	if (m->client_info == NULL \|\| wps == NULL)
	441	return 0;
	442
	443	pos = wpabuf_head(m->client_info);
	444	end = pos + wpabuf_len(m->client_info);
	445
	446	pos += 1 + 2 * ETH_ALEN + 1 + 2;
	447	if (end - pos < WPS_DEV_TYPE_LEN + 1)
	448	return 0;
	449
	450	if (wps_parse_msg(wps, &attr))
	451	return 1; /* assume no Requested Device Type attributes */
	452
	453	if (attr.num_req_dev_type == 0)
	454	return 1; /* no Requested Device Type attributes -> match */
	455
	456	if (dev_type_list_match(pos, attr.req_dev_type, attr.num_req_dev_type))
	457	return 1; /* Match with client Primary Device Type */
	458
	459	pos += WPS_DEV_TYPE_LEN;
	460	num_sec = *pos++;
	461	if (end - pos < num_sec * WPS_DEV_TYPE_LEN)
	462	return 0;
	463	while (num_sec > 0) {
	464	num_sec--;
	465	if (dev_type_list_match(pos, attr.req_dev_type,
	466	attr.num_req_dev_type))
	467	return 1; /* Match with client Secondary Device Type */
	468	pos += WPS_DEV_TYPE_LEN;
	469	}
	470
	471	/* No matching device type found */
	472	return 0;
	473	}
	474
	475
	476	int p2p_group_match_dev_type(struct p2p_group group, struct wpabuf wps)
	477	{
	478	struct p2p_group_member *m;
	479
	480	if (p2p_match_dev_type(group->p2p, wps))
	481	return 1; /* Match with own device type */
	482
483	for (m = group->members; m; m = m->next) {
484	if (p2p_match_dev_type_member(m, wps))
485	return 1; /* Match with group client device type */
486	}
487
488	/* No match with Requested Device Type */
489	return 0;
490	}
491
492
493	void p2p_group_notif_formation_done(struct p2p_group *group)
494	{
495	if (group == NULL)
496	return;
497	group->group_formation = 0;
498	group->beacon_update = 1;
499	p2p_group_update_ies(group);
500	}
501
502
503	int p2p_group_notif_noa(struct p2p_group group, const u8 noa,
504	size_t noa_len)
505	{
506	if (noa == NULL) {
507	wpabuf_free(group->noa);
508	group->noa = NULL;
509	} else {
510	if (group->noa) {
511	if (wpabuf_size(group->noa) >= noa_len) {
f1afcb39	512	group->noa->used = 0;
b22128ef JM	513	wpabuf_put_data(group->noa, noa, noa_len);
	514	} else {
	515	wpabuf_free(group->noa);
	516	group->noa = NULL;
	517	}
	518	}
	519
	520	if (!group->noa) {
	521	group->noa = wpabuf_alloc_copy(noa, noa_len);
	522	if (group->noa == NULL)
	523	return -1;
	524	}
	525	}
	526
	527	group->beacon_update = 1;
	528	p2p_group_update_ies(group);
	529	return 0;
	530	}
	531
	532
	533	static struct p2p_group_member * p2p_group_get_client(struct p2p_group *group,
	534	const u8 *dev_id)
	535	{
	536	struct p2p_group_member *m;
	537
	538	for (m = group->members; m; m = m->next) {
	539	if (os_memcmp(dev_id, m->dev_addr, ETH_ALEN) == 0)
	540	return m;
	541	}
	542
	543	return NULL;
	544	}
	545
	546
	547	static struct p2p_group_member * p2p_group_get_client_iface(
	548	struct p2p_group group, const u8 interface_addr)
	549	{
	550	struct p2p_group_member *m;
	551
	552	for (m = group->members; m; m = m->next) {
	553	if (os_memcmp(interface_addr, m->addr, ETH_ALEN) == 0)
	554	return m;
	555	}
	556
	557	return NULL;
	558	}
	559
	560
c9aab274 JJ	561	const u8 * p2p_group_get_dev_addr(struct p2p_group group, const u8 addr)
	562	{
	563	struct p2p_group_member *m;
	564
	565	if (group == NULL)
	566	return NULL;
	567	m = p2p_group_get_client_iface(group, addr);
	568	if (m && !is_zero_ether_addr(m->dev_addr))
	569	return m->dev_addr;
	570	return NULL;
	571	}
	572
	573
b22128ef JM	574	static struct wpabuf * p2p_build_go_disc_req(void)
	575	{
	576	struct wpabuf *buf;
	577
	578	buf = wpabuf_alloc(100);
	579	if (buf == NULL)
	580	return NULL;
	581
	582	p2p_buf_add_action_hdr(buf, P2P_GO_DISC_REQ, 0);
	583
	584	return buf;
	585	}
	586
	587
	588	int p2p_group_go_discover(struct p2p_group group, const u8 dev_id,
	589	const u8 *searching_dev, int rx_freq)
	590	{
	591	struct p2p_group_member *m;
	592	struct wpabuf *req;
	593	struct p2p_data *p2p = group->p2p;
	594	int freq;
	595
	596	m = p2p_group_get_client(group, dev_id);
3f4ce13f	597	if (m == NULL \|\| m->client_info == NULL) {
b22128ef JM	598	wpa_printf(MSG_DEBUG, "P2P: Requested client was not in this "
	599	"group " MACSTR,
	600	MAC2STR(group->cfg->interface_addr));
	601	return -1;
	602	}
	603
	604	if (!(m->dev_capab & P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY)) {
	605	wpa_printf(MSG_DEBUG, "P2P: Requested client does not support "
	606	"client discoverability");
	607	return -1;
	608	}
	609
	610	wpa_printf(MSG_DEBUG, "P2P: Schedule GO Discoverability Request to be "
	611	"sent to " MACSTR, MAC2STR(dev_id));
	612
	613	req = p2p_build_go_disc_req();
	614	if (req == NULL)
	615	return -1;
	616
	617	/* TODO: Should really use group operating frequency here */
	618	freq = rx_freq;
	619
	620	p2p->pending_action_state = P2P_PENDING_GO_DISC_REQ;
	621	if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, m->addr,
	622	group->cfg->interface_addr,
	623	group->cfg->interface_addr,
	624	wpabuf_head(req), wpabuf_len(req), 200) < 0)
	625	{
	626	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
	627	"P2P: Failed to send Action frame");
	628	}
	629
	630	wpabuf_free(req);
	631
	632	return 0;
	633	}
	634
	635
	636	const u8 * p2p_group_get_interface_addr(struct p2p_group *group)
	637	{
	638	return group->cfg->interface_addr;
	639	}
	640
	641
	642	u8 p2p_group_presence_req(struct p2p_group *group,
	643	const u8 *client_interface_addr,
	644	const u8 *noa, size_t noa_len)
	645	{
	646	struct p2p_group_member *m;
	647	u8 curr_noa[50];
	648	int curr_noa_len;
	649
	650	m = p2p_group_get_client_iface(group, client_interface_addr);
3f4ce13f	651	if (m == NULL \|\| m->client_info == NULL) {
b22128ef JM	652	wpa_printf(MSG_DEBUG, "P2P: Client was not in this group");
	653	return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
	654	}
	655
	656	wpa_hexdump(MSG_DEBUG, "P2P: Presence Request NoA", noa, noa_len);
	657
	658	if (group->p2p->cfg->get_noa)
	659	curr_noa_len = group->p2p->cfg->get_noa(
	660	group->p2p->cfg->cb_ctx, group->cfg->interface_addr,
	661	curr_noa, sizeof(curr_noa));
	662	else
	663	curr_noa_len = -1;
	664	if (curr_noa_len < 0)
	665	wpa_printf(MSG_DEBUG, "P2P: Failed to fetch current NoA");
	666	else if (curr_noa_len == 0)
	667	wpa_printf(MSG_DEBUG, "P2P: No NoA being advertized");
	668	else
	669	wpa_hexdump(MSG_DEBUG, "P2P: Current NoA", curr_noa,
	670	curr_noa_len);
	671
	672	/* TODO: properly process request and store copy */
	673	if (curr_noa_len > 0)
	674	return P2P_SC_FAIL_UNABLE_TO_ACCOMMODATE;
	675
	676	return P2P_SC_SUCCESS;
	677	}
5efa9e2a JB	678
	679
	680	unsigned int p2p_get_group_num_members(struct p2p_group *group)
	681	{
	682	return group->num_members;
	683	}
	684
	685
	686	const u8 * p2p_iterate_group_members(struct p2p_group group, void *next)
	687	{
	688	struct p2p_group_member iter = next;
	689
	690	if (!iter)
	691	iter = group->members;
	692	else
	693	iter = iter->next;
	694
	695	*next = iter;
	696
	697	if (!iter)
	698	return NULL;
	699
	700	return iter->addr;
	701	}