fragattacks/src/p2p/p2p_sd.c

/*
 * Wi-Fi Direct - P2P service discovery
 * Copyright (c) 2009, 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 "p2p_i.h"
#include "p2p.h"


struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p,
					 struct p2p_device *dev)
{
	struct p2p_sd_query *q;

	if (!(dev->dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY))
		return 0; /* peer does not support SD */

	for (q = p2p->sd_queries; q; q = q->next) {
		if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO))
			return q;
		if (!q->for_all_peers &&
		    os_memcmp(q->peer, dev->p2p_device_addr, ETH_ALEN) == 0)
			return q;
	}

	return NULL;
}


static int p2p_unlink_sd_query(struct p2p_data *p2p,
			       struct p2p_sd_query *query)
{
	struct p2p_sd_query *q, *prev;
	q = p2p->sd_queries;
	prev = NULL;
	while (q) {
		if (q == query) {
			if (prev)
				prev->next = q->next;
			else
				p2p->sd_queries = q->next;
			if (p2p->sd_query == query)
				p2p->sd_query = NULL;
			return 1;
		}
		prev = q;
		q = q->next;
	}
	return 0;
}


static void p2p_free_sd_query(struct p2p_sd_query *q)
{
	if (q == NULL)
		return;
	wpabuf_free(q->tlvs);
	os_free(q);
}


void p2p_free_sd_queries(struct p2p_data *p2p)
{
	struct p2p_sd_query *q, *prev;
	q = p2p->sd_queries;
	p2p->sd_queries = NULL;
	while (q) {
		prev = q;
		q = q->next;
		p2p_free_sd_query(prev);
	}
}


static struct wpabuf * p2p_build_sd_query(u16 update_indic,
					  struct wpabuf *tlvs)
{
	struct wpabuf *buf;
	u8 *len_pos, *len_pos2;

	buf = wpabuf_alloc(1000 + wpabuf_len(tlvs));
	if (buf == NULL)
		return NULL;

	wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC);
	wpabuf_put_u8(buf, WLAN_PA_GAS_INITIAL_REQ);
	wpabuf_put_u8(buf, 0); /* Dialog Token */

	/* Advertisement Protocol IE */
	wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO);
	wpabuf_put_u8(buf, 2); /* Length */
	wpabuf_put_u8(buf, 0); /* QueryRespLenLimit | PAME-BI */
	wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */

	/* Query Request */
	len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */

	/* NQP Query Request Frame */
	wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */
	len_pos2 = wpabuf_put(buf, 2); /* Length (to be filled) */
	wpabuf_put_be24(buf, OUI_WFA);
	wpabuf_put_u8(buf, P2P_OUI_TYPE);
	wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
	wpabuf_put_buf(buf, tlvs);

	WPA_PUT_LE16(len_pos2, (u8 *) wpabuf_put(buf, 0) - len_pos2 - 2);
	WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2);

	return buf;
}


static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
					     u16 update_indic,
					     const struct wpabuf *tlvs)
{
	struct wpabuf *buf;
	u8 *len_pos, *len_pos2;

	buf = wpabuf_alloc(1000 + wpabuf_len(tlvs));
	if (buf == NULL)
		return NULL;

	wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC);
	wpabuf_put_u8(buf, WLAN_PA_GAS_INITIAL_RESP);
	wpabuf_put_u8(buf, dialog_token);
	wpabuf_put_le16(buf, status_code);
	wpabuf_put_le16(buf, 0); /* GAS Comeback Delay */

	/* Advertisement Protocol IE */
	wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO);
	wpabuf_put_u8(buf, 2); /* Length */
	wpabuf_put_u8(buf, 0x7f); /* QueryRespLenLimit | PAME-BI */
	wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */

	/* Query Response */
	len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */

	/* NQP Query Response Frame */
	wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */
	len_pos2 = wpabuf_put(buf, 2); /* Length (to be filled) */
	wpabuf_put_be24(buf, OUI_WFA);
	wpabuf_put_u8(buf, P2P_OUI_TYPE);
	wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
	wpabuf_put_buf(buf, tlvs);

	WPA_PUT_LE16(len_pos2, (u8 *) wpabuf_put(buf, 0) - len_pos2 - 2);
	WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2);

	return buf;
}


int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev)
{
	struct wpabuf *req;
	int ret = 0;
	struct p2p_sd_query *query;
	int freq;

	freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq;
	if (freq <= 0) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: No Listen/Operating frequency known for the "
			"peer " MACSTR " to send SD Request",
			MAC2STR(dev->p2p_device_addr));
		return -1;
	}

	query = p2p_pending_sd_req(p2p, dev);
	if (query == NULL)
		return -1;

	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
		"P2P: Start Service Discovery with " MACSTR,
		MAC2STR(dev->p2p_device_addr));

	req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs);
	if (req == NULL)
		return -1;

	p2p->sd_peer = dev;
	p2p->sd_query = query;
	p2p->pending_action_state = P2P_PENDING_SD;

	if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq,
				  dev->p2p_device_addr, p2p->cfg->dev_addr,
				  dev->p2p_device_addr,
				  wpabuf_head(req), wpabuf_len(req), 5000) < 0)
	{
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Failed to send Action frame");
		ret = -1;
	}

	wpabuf_free(req);

	return ret;
}


void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
			    const u8 *data, size_t len, int rx_freq)
{
	const u8 *pos = data;
	const u8 *end = data + len;
	const u8 *next;
	u8 dialog_token;
	u16 slen;
	int freq;
	u16 update_indic;


	if (p2p->cfg->sd_request == NULL)
		return;

	if (rx_freq > 0)
		freq = rx_freq;
	else
		freq = p2p_channel_to_freq(p2p->cfg->country,
					   p2p->cfg->reg_class,
					   p2p->cfg->channel);
	if (freq < 0)
		return;

	if (len < 1 + 2)
		return;

	dialog_token = *pos++;
	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
		"P2P: GAS Initial Request from " MACSTR " (dialog token %u, "
		"freq %d)",
		MAC2STR(sa), dialog_token, rx_freq);

	if (*pos != WLAN_EID_ADV_PROTO) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Unexpected IE in GAS Initial Request: %u", *pos);
		return;
	}
	pos++;

	slen = *pos++;
	next = pos + slen;
	if (next > end || slen < 2) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Invalid IE in GAS Initial Request");
		return;
	}
	pos++; /* skip QueryRespLenLimit and PAME-BI */

	if (*pos != NATIVE_QUERY_PROTOCOL) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Unsupported GAS advertisement protocol id %u",
			*pos);
		return;
	}

	pos = next;
	/* Query Request */
	if (pos + 2 > end)
		return;
	slen = WPA_GET_LE16(pos);
	pos += 2;
	if (pos + slen > end)
		return;
	end = pos + slen;

	/* NQP Query Request */
	if (pos + 4 > end)
		return;
	if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos));
		return;
	}
	pos += 2;

	slen = WPA_GET_LE16(pos);
	pos += 2;
	if (pos + slen > end || slen < 3 + 1) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Invalid NQP Query Request length");
		return;
	}

	if (WPA_GET_BE24(pos) != OUI_WFA) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos));
		return;
	}
	pos += 3;

	if (*pos != P2P_OUI_TYPE) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Unsupported NQP vendor type %u", *pos);
		return;
	}
	pos++;

	if (pos + 2 > end)
		return;
	update_indic = WPA_GET_LE16(pos);
	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
		"P2P: Service Update Indicator: %u", update_indic);
	pos += 2;

	p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token,
			     update_indic, pos, end - pos);
	/* the response will be indicated with a call to p2p_sd_response() */
}


void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
		     u8 dialog_token, const struct wpabuf *resp_tlvs)
{
	struct wpabuf *resp;
	resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
				     p2p->srv_update_indic, resp_tlvs);
	if (resp == NULL)
		return;

	p2p->pending_action_state = P2P_NO_PENDING_ACTION;
	if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq,
				  dst, p2p->cfg->dev_addr, p2p->cfg->dev_addr,
				  wpabuf_head(resp), wpabuf_len(resp), 200) <
	    0)
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Failed to send Action frame");

	wpabuf_free(resp);
}


void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
			     const u8 *data, size_t len)
{
	const u8 *pos = data;
	const u8 *end = data + len;
	const u8 *next;
	u8 dialog_token;
	u16 status_code;
	u16 comeback_delay;
	u16 slen;
	u16 update_indic;

	if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
	    os_memcmp(sa, p2p->sd_peer->p2p_device_addr, ETH_ALEN) != 0) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Ignore unexpected GAS Initial Response from "
			MACSTR, MAC2STR(sa));
		return;
	}
	p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
	p2p_clear_timeout(p2p);

	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
		"P2P: Received GAS Initial Response from " MACSTR,
		MAC2STR(sa));

	if (len < 5 + 2)
		return;

	dialog_token = *pos++;
	/* TODO: check dialog_token match */
	status_code = WPA_GET_LE16(pos);
	pos += 2;
	comeback_delay = WPA_GET_LE16(pos);
	pos += 2;
	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
		"P2P: dialog_token=%u status_code=%u comeback_delay=%u",
		dialog_token, status_code, comeback_delay);

	if (*pos != WLAN_EID_ADV_PROTO) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Unexpected IE in GAS Initial Response: %u",
			*pos);
		return;
	}
	pos++;

	slen = *pos++;
	next = pos + slen;
	if (next > end || slen < 2) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Invalid IE in GAS Initial Response");
		return;
	}
	pos++; /* skip QueryRespLenLimit and PAME-BI */

	if (*pos != NATIVE_QUERY_PROTOCOL) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Unsupported GAS advertisement protocol id %u",
			*pos);
		return;
	}

	pos = next;
	/* Query Response */
	if (pos + 2 > end)
		return;
	slen = WPA_GET_LE16(pos);
	pos += 2;
	if (pos + slen > end)
		return;
	end = pos + slen;

	/* NQP Query Response */
	if (pos + 4 > end)
		return;
	if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos));
		return;
	}
	pos += 2;

	slen = WPA_GET_LE16(pos);
	pos += 2;
	if (pos + slen > end || slen < 3 + 1) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Invalid NQP Query Response length");
		return;
	}

	if (WPA_GET_BE24(pos) != OUI_WFA) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos));
		return;
	}
	pos += 3;

	if (*pos != P2P_OUI_TYPE) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Unsupported NQP vendor type %u", *pos);
		return;
	}
	pos++;

	if (pos + 2 > end)
		return;
	update_indic = WPA_GET_LE16(pos);
	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
		"P2P: Service Update Indicator: %u", update_indic);
	pos += 2;

	p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
	p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
	p2p->sd_peer = NULL;

	if (p2p->sd_query) {
		if (!p2p->sd_query->for_all_peers) {
			struct p2p_sd_query *q;
			wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
				"P2P: Remove completed SD query %p",
				p2p->sd_query);
			q = p2p->sd_query;
			p2p_unlink_sd_query(p2p, p2p->sd_query);
			p2p_free_sd_query(q);
		}
		p2p->sd_query = NULL;
	}

	if (p2p->cfg->sd_response)
		p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
				      pos, end - pos);
	p2p_continue_find(p2p);
}


void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
		      const struct wpabuf *tlvs)
{
	struct p2p_sd_query *q;

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

	if (dst)
		os_memcpy(q->peer, dst, ETH_ALEN);
	else
		q->for_all_peers = 1;

	q->tlvs = wpabuf_dup(tlvs);
	if (q->tlvs == NULL) {
		p2p_free_sd_query(q);
		return NULL;
	}

	q->next = p2p->sd_queries;
	p2p->sd_queries = q;
	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p", q);

	return q;
}


void p2p_sd_service_update(struct p2p_data *p2p)
{
	p2p->srv_update_indic++;
}


int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
{
	if (p2p_unlink_sd_query(p2p, req)) {
		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
			"P2P: Cancel pending SD query %p", req);
		p2p_free_sd_query(req);
		return 0;
	}
	return -1;
}