Merge hostapd / wpa_supplicant 2.0.

Reviewed by:	adrian (driver_bsd + usr.sbin/wpa)

1 files changed, 947 insertions, 0 deletions

diff --git a/contrib/wpa/src/p2p/p2p_sd.c b/contrib/wpa/src/p2p/p2p_sd.c
new file mode 100644
index 0000000..abe1d6b
--- /dev/null
+++ b/contrib/wpa/src/p2p/p2p_sd.c
@@ -0,0 +1,947 @@
+/*
+ * Wi-Fi Direct - P2P service discovery
+ * Copyright (c) 2009, Atheros Communications
+ *
+ * This software may be distributed under the terms of the BSD license.
+ * See README for more details.
+ */
+
+#include "includes.h"
+
+#include "common.h"
+#include "common/ieee802_11_defs.h"
+#include "common/gas.h"
+#include "p2p_i.h"
+#include "p2p.h"
+
+
+#ifdef CONFIG_WIFI_DISPLAY
+static int wfd_wsd_supported(struct wpabuf *wfd)
+{
+	const u8 *pos, *end;
+	u8 subelem;
+	u16 len;
+
+	if (wfd == NULL)
+		return 0;
+
+	pos = wpabuf_head(wfd);
+	end = pos + wpabuf_len(wfd);
+
+	while (pos + 3 <= end) {
+		subelem = *pos++;
+		len = WPA_GET_BE16(pos);
+		pos += 2;
+		if (pos + len > end)
+			break;
+
+		if (subelem == WFD_SUBELEM_DEVICE_INFO && len >= 6) {
+			u16 info = WPA_GET_BE16(pos);
+			return !!(info & 0x0040);
+		}
+
+		pos += len;
+	}
+
+	return 0;
+}
+#endif /* CONFIG_WIFI_DISPLAY */
+
+struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p,
+					 struct p2p_device *dev)
+{
+	struct p2p_sd_query *q;
+	int wsd = 0;
+
+	if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY))
+		return NULL; /* peer does not support SD */
+#ifdef CONFIG_WIFI_DISPLAY
+	if (wfd_wsd_supported(dev->info.wfd_subelems))
+		wsd = 1;
+#endif /* CONFIG_WIFI_DISPLAY */
+
+	for (q = p2p->sd_queries; q; q = q->next) {
+		/* Use WSD only if the peer indicates support or it */
+		if (q->wsd && !wsd)
+			continue;
+		if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO))
+			return q;
+		if (!q->for_all_peers &&
+		    os_memcmp(q->peer, dev->info.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;
+
+	buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs));
+	if (buf == NULL)
+		return NULL;
+
+	/* ANQP Query Request Frame */
+	len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
+	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);
+	gas_anqp_set_element_len(buf, len_pos);
+
+	gas_anqp_set_len(buf);
+
+	return buf;
+}
+
+
+static void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst,
+				      u8 dialog_token, int freq)
+{
+	struct wpabuf *req;
+
+	req = gas_build_comeback_req(dialog_token);
+	if (req == NULL)
+		return;
+
+	p2p->pending_action_state = P2P_NO_PENDING_ACTION;
+	if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst,
+			    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);
+}
+
+
+static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
+					     u16 comeback_delay,
+					     u16 update_indic,
+					     const struct wpabuf *tlvs)
+{
+	struct wpabuf *buf;
+	u8 *len_pos;
+
+	buf = gas_anqp_build_initial_resp(dialog_token, status_code,
+					  comeback_delay,
+					  100 + (tlvs ? wpabuf_len(tlvs) : 0));
+	if (buf == NULL)
+		return NULL;
+
+	if (tlvs) {
+		/* ANQP Query Response Frame */
+		len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
+		wpabuf_put_be24(buf, OUI_WFA);
+		wpabuf_put_u8(buf, P2P_OUI_TYPE);
+		 /* Service Update Indicator */
+		wpabuf_put_le16(buf, update_indic);
+		wpabuf_put_buf(buf, tlvs);
+		gas_anqp_set_element_len(buf, len_pos);
+	}
+
+	gas_anqp_set_len(buf);
+
+	return buf;
+}
+
+
+static struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token,
+						   u16 status_code,
+						   u16 update_indic,
+						   const u8 *data, size_t len,
+						   u8 frag_id, u8 more,
+						   u16 total_len)
+{
+	struct wpabuf *buf;
+
+	buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id,
+					   more, 0, 100 + len);
+	if (buf == NULL)
+		return NULL;
+
+	if (frag_id == 0) {
+		/* ANQP Query Response Frame */
+		wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */
+		wpabuf_put_le16(buf, 3 + 1 + 2 + total_len);
+		wpabuf_put_be24(buf, OUI_WFA);
+		wpabuf_put_u8(buf, P2P_OUI_TYPE);
+		/* Service Update Indicator */
+		wpabuf_put_le16(buf, update_indic);
+	}
+
+	wpabuf_put_data(buf, data, len);
+	gas_anqp_set_len(buf);
+
+	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->info.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->info.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_send_action(p2p, freq, dev->info.p2p_device_addr,
+			    p2p->cfg->dev_addr, dev->info.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 != ACCESS_NETWORK_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;
+
+	/* ANQP Query Request */
+	if (pos + 4 > end)
+		return;
+	if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Unsupported ANQP 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 ANQP Query Request length");
+		return;
+	}
+
+	if (WPA_GET_BE24(pos) != OUI_WFA) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
+		return;
+	}
+	pos += 3;
+
+	if (*pos != P2P_OUI_TYPE) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Unsupported ANQP 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;
+
+	/* TODO: fix the length limit to match with the maximum frame length */
+	if (wpabuf_len(resp_tlvs) > 1400) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response long "
+			"enough to require fragmentation");
+		if (p2p->sd_resp) {
+			/*
+			 * TODO: Could consider storing the fragmented response
+			 * separately for each peer to avoid having to drop old
+			 * one if there is more than one pending SD query.
+			 * Though, that would eat more memory, so there are
+			 * also benefits to just using a single buffer.
+			 */
+			wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop "
+				"previous SD response");
+			wpabuf_free(p2p->sd_resp);
+		}
+		p2p->sd_resp = wpabuf_dup(resp_tlvs);
+		if (p2p->sd_resp == NULL) {
+			wpa_msg(p2p->cfg->msg_ctx, MSG_ERROR, "P2P: Failed to "
+				"allocate SD response fragmentation area");
+			return;
+		}
+		os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
+		p2p->sd_resp_dialog_token = dialog_token;
+		p2p->sd_resp_pos = 0;
+		p2p->sd_frag_id = 0;
+		resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
+					     1, p2p->srv_update_indic, NULL);
+	} else {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response fits "
+			"in initial response");
+		resp = p2p_build_sd_response(dialog_token,
+					     WLAN_STATUS_SUCCESS, 0,
+					     p2p->srv_update_indic, resp_tlvs);
+	}
+	if (resp == NULL)
+		return;
+
+	p2p->pending_action_state = P2P_NO_PENDING_ACTION;
+	if (p2p_send_action(p2p, 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, int rx_freq)
+{
+	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->info.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 " (len=%d)",
+		MAC2STR(sa), (int) len);
+
+	if (len < 5 + 2) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Too short GAS Initial Response frame");
+		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 (status_code) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Service Discovery failed: status code %u",
+			status_code);
+		return;
+	}
+
+	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 != ACCESS_NETWORK_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) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
+			"Response");
+		return;
+	}
+	slen = WPA_GET_LE16(pos);
+	pos += 2;
+	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
+		slen);
+	if (pos + slen > end) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
+			"Response data");
+		return;
+	}
+	end = pos + slen;
+
+	if (comeback_delay) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Fragmented "
+			"response - request fragments");
+		if (p2p->sd_rx_resp) {
+			wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop "
+				"old SD reassembly buffer");
+			wpabuf_free(p2p->sd_rx_resp);
+			p2p->sd_rx_resp = NULL;
+		}
+		p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
+		return;
+	}
+
+	/* ANQP Query Response */
+	if (pos + 4 > end)
+		return;
+	if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Unsupported ANQP 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 ANQP Query Response length");
+		return;
+	}
+
+	if (WPA_GET_BE24(pos) != OUI_WFA) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
+		return;
+	}
+	pos += 3;
+
+	if (*pos != P2P_OUI_TYPE) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Unsupported ANQP 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_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
+			     const u8 *data, size_t len, int rx_freq)
+{
+	struct wpabuf *resp;
+	u8 dialog_token;
+	size_t frag_len;
+	int more = 0;
+
+	wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
+	if (len < 1)
+		return;
+	dialog_token = *data;
+	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Dialog Token: %u",
+		dialog_token);
+	if (dialog_token != p2p->sd_resp_dialog_token) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
+			"response fragment for dialog token %u", dialog_token);
+		return;
+	}
+
+	if (p2p->sd_resp == NULL) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
+			"response fragment available");
+		return;
+	}
+	if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
+			"response fragment for " MACSTR, MAC2STR(sa));
+		return;
+	}
+
+	frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
+	if (frag_len > 1400) {
+		frag_len = 1400;
+		more = 1;
+	}
+	resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
+					   p2p->srv_update_indic,
+					   wpabuf_head_u8(p2p->sd_resp) +
+					   p2p->sd_resp_pos, frag_len,
+					   p2p->sd_frag_id, more,
+					   wpabuf_len(p2p->sd_resp));
+	if (resp == NULL)
+		return;
+	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Send GAS Comeback "
+		"Response (frag_id %d more=%d frag_len=%d)",
+		p2p->sd_frag_id, more, (int) frag_len);
+	p2p->sd_frag_id++;
+	p2p->sd_resp_pos += frag_len;
+
+	if (more) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: %d more bytes "
+			"remain to be sent",
+			(int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
+	} else {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: All fragments of "
+			"SD response sent");
+		wpabuf_free(p2p->sd_resp);
+		p2p->sd_resp = NULL;
+	}
+
+	p2p->pending_action_state = P2P_NO_PENDING_ACTION;
+	if (p2p_send_action(p2p, rx_freq, sa, 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_comeback_resp(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 status_code;
+	u8 frag_id;
+	u8 more_frags;
+	u16 comeback_delay;
+	u16 slen;
+
+	wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
+
+	if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
+	    os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Ignore unexpected GAS Comeback 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 Comeback Response from " MACSTR " (len=%d)",
+		MAC2STR(sa), (int) len);
+
+	if (len < 6 + 2) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Too short GAS Comeback Response frame");
+		return;
+	}
+
+	dialog_token = *pos++;
+	/* TODO: check dialog_token match */
+	status_code = WPA_GET_LE16(pos);
+	pos += 2;
+	frag_id = *pos & 0x7f;
+	more_frags = (*pos & 0x80) >> 7;
+	pos++;
+	comeback_delay = WPA_GET_LE16(pos);
+	pos += 2;
+	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+		"P2P: dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
+		"comeback_delay=%u",
+		dialog_token, status_code, frag_id, more_frags,
+		comeback_delay);
+	/* TODO: check frag_id match */
+	if (status_code) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Service Discovery failed: status code %u",
+			status_code);
+		return;
+	}
+
+	if (*pos != WLAN_EID_ADV_PROTO) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Unexpected IE in GAS Comeback 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 Comeback Response");
+		return;
+	}
+	pos++; /* skip QueryRespLenLimit and PAME-BI */
+
+	if (*pos != ACCESS_NETWORK_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) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
+			"Response");
+		return;
+	}
+	slen = WPA_GET_LE16(pos);
+	pos += 2;
+	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
+		slen);
+	if (pos + slen > end) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
+			"Response data");
+		return;
+	}
+	if (slen == 0) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No Query Response "
+			"data");
+		return;
+	}
+	end = pos + slen;
+
+	if (p2p->sd_rx_resp) {
+		 /*
+		  * ANQP header is only included in the first fragment; rest of
+		  * the fragments start with continue TLVs.
+		  */
+		goto skip_nqp_header;
+	}
+
+	/* ANQP Query Response */
+	if (pos + 4 > end)
+		return;
+	if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
+		return;
+	}
+	pos += 2;
+
+	slen = WPA_GET_LE16(pos);
+	pos += 2;
+	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: ANQP Query Response "
+		"length: %u", slen);
+	if (slen < 3 + 1) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Invalid ANQP Query Response length");
+		return;
+	}
+	if (pos + 4 > end)
+		return;
+
+	if (WPA_GET_BE24(pos) != OUI_WFA) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
+		return;
+	}
+	pos += 3;
+
+	if (*pos != P2P_OUI_TYPE) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+			"P2P: Unsupported ANQP vendor type %u", *pos);
+		return;
+	}
+	pos++;
+
+	if (pos + 2 > end)
+		return;
+	p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
+	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+		"P2P: Service Update Indicator: %u", p2p->sd_rx_update_indic);
+	pos += 2;
+
+skip_nqp_header:
+	if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
+		return;
+	wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
+	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Current SD reassembly "
+		"buffer length: %u",
+		(unsigned int) wpabuf_len(p2p->sd_rx_resp));
+
+	if (more_frags) {
+		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: More fragments "
+			"remains");
+		/* TODO: what would be a good size limit? */
+		if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
+			wpabuf_free(p2p->sd_rx_resp);
+			p2p->sd_rx_resp = NULL;
+			wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too long "
+				"SD response - drop it");
+			return;
+		}
+		p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
+		return;
+	}
+
+	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,
+				      p2p->sd_rx_update_indic,
+				      wpabuf_head(p2p->sd_rx_resp),
+				      wpabuf_len(p2p->sd_rx_resp));
+	wpabuf_free(p2p->sd_rx_resp);
+	p2p->sd_rx_resp = NULL;
+
+	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);
+
+	if (dst == NULL) {
+		struct p2p_device *dev;
+		dl_list_for_each(dev, &p2p->devices, struct p2p_device, list)
+			dev->flags &= ~P2P_DEV_SD_INFO;
+	}
+
+	return q;
+}
+
+
+#ifdef CONFIG_WIFI_DISPLAY
+void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
+			  const struct wpabuf *tlvs)
+{
+	struct p2p_sd_query *q;
+	q = p2p_sd_request(p2p, dst, tlvs);
+	if (q)
+		q->wsd = 1;
+	return q;
+}
+#endif /* CONFIG_WIFI_DISPLAY */
+
+
+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;
+}