From 9e7321eea4816fd9f30368adcd76657251c7b227 Mon Sep 17 00:00:00 2001
From: Krishna Vamsi <vamsin@qti.qualcomm.com>
Date: Tue, 9 Dec 2014 19:55:29 +0530
Subject: [PATCH] P2PS: Parse Probe Request frames for matching ASP hashes

Signed-off-by: Jouni Malinen <jouni@qca.qualcomm.com>
---
 src/p2p/p2p.c   | 87 +++++++++++++++++++++++++++++++++++++++++++++----
 src/p2p/p2p_i.h |  2 ++
 2 files changed, 82 insertions(+), 7 deletions(-)

diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c
index 59ab71af2..ae4f5fc65 100644
--- a/src/p2p/p2p.c
+++ b/src/p2p/p2p.c
@@ -2154,6 +2154,28 @@ struct wpabuf * p2p_build_probe_resp_ies(struct p2p_data *p2p)
 }
 
 
+static int p2p_service_find_asp(struct p2p_data *p2p, const u8 *hash)
+{
+	struct p2ps_advertisement *adv_data;
+
+	p2p_dbg(p2p, "ASP find - ASP list: %p", p2p->p2ps_adv_list);
+
+	/* Wildcard always matches if we have actual services */
+	if (os_memcmp(hash, p2p->wild_card_hash, P2PS_HASH_LEN) == 0)
+		return p2p->p2ps_adv_list != NULL;
+
+	adv_data = p2p->p2ps_adv_list;
+	while (adv_data) {
+		p2p_dbg(p2p, "ASP hash: %x =? %x", hash[0], adv_data->hash[0]);
+		if (os_memcmp(hash, adv_data->hash, P2PS_HASH_LEN) == 0)
+			return 1;
+		adv_data = adv_data->next;
+	}
+
+	return 0;
+}
+
+
 static enum p2p_probe_req_status
 p2p_reply_probe(struct p2p_data *p2p, const u8 *addr, const u8 *dst,
 		const u8 *bssid, const u8 *ie, size_t ie_len)
@@ -2164,13 +2186,6 @@ p2p_reply_probe(struct p2p_data *p2p, const u8 *addr, const u8 *dst,
 	struct p2p_message msg;
 	struct wpabuf *ies;
 
-	if (!p2p->in_listen || !p2p->drv_in_listen) {
-		/* not in Listen state - ignore Probe Request */
-		p2p_dbg(p2p, "Not in Listen state (in_listen=%d drv_in_listen=%d) - ignore Probe Request",
-			p2p->in_listen, p2p->drv_in_listen);
-		return P2P_PREQ_NOT_LISTEN;
-	}
-
 	if (ieee802_11_parse_elems((u8 *) ie, ie_len, &elems, 0) ==
 	    ParseFailed) {
 		/* Ignore invalid Probe Request frames */
@@ -2221,6 +2236,64 @@ p2p_reply_probe(struct p2p_data *p2p, const u8 *addr, const u8 *dst,
 		return P2P_PREQ_NOT_P2P;
 	}
 
+	p2p->p2ps_svc_found = 0;
+
+	if (msg.service_hash && msg.service_hash_count) {
+		const u8 *hash = msg.service_hash;
+		u8 *dest = p2p->query_hash;
+		u8 i;
+
+		p2p->query_count = 0;
+		for (i = 0; i < msg.service_hash_count; i++) {
+			if (p2p_service_find_asp(p2p, hash)) {
+				p2p->p2ps_svc_found = 1;
+
+				if (!os_memcmp(hash, p2p->wild_card_hash,
+					       P2PS_HASH_LEN)) {
+					/* We found match(es) but wildcard
+					 * will return all */
+					p2p->query_count = 1;
+					os_memcpy(p2p->query_hash, hash,
+						  P2PS_HASH_LEN);
+					break;
+				}
+
+				/* Save each matching hash */
+				if (p2p->query_count < P2P_MAX_QUERY_HASH) {
+					os_memcpy(dest, hash, P2PS_HASH_LEN);
+					dest += P2PS_HASH_LEN;
+					p2p->query_count++;
+				} else {
+					/* We found match(es) but too many to
+					 * return all */
+					p2p->query_count = 0;
+					break;
+				}
+			}
+			hash += P2PS_HASH_LEN;
+		}
+
+		p2p_dbg(p2p, "ASP adv found: %d", p2p->p2ps_svc_found);
+
+		/* Probed hash unknown */
+		if (!p2p->p2ps_svc_found) {
+			p2p_parse_free(&msg);
+			return P2P_PREQ_NOT_PROCESSED;
+		}
+	} else {
+		/* This is not a P2PS Probe Request */
+		p2p->query_count = 0;
+		p2p_dbg(p2p, "No P2PS Hash in Probe Request");
+
+		if (!p2p->in_listen || !p2p->drv_in_listen) {
+			/* not in Listen state - ignore Probe Request */
+			p2p_dbg(p2p, "Not in Listen state (in_listen=%d drv_in_listen=%d) - ignore Probe Request",
+				p2p->in_listen, p2p->drv_in_listen);
+			p2p_parse_free(&msg);
+			return P2P_PREQ_NOT_LISTEN;
+		}
+	}
+
 	if (msg.device_id &&
 	    os_memcmp(msg.device_id, p2p->cfg->dev_addr, ETH_ALEN) != 0) {
 		/* Device ID did not match */
diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h
index dcaae07aa..bb9ff4dc8 100644
--- a/src/p2p/p2p_i.h
+++ b/src/p2p/p2p_i.h
@@ -496,8 +496,10 @@ struct p2p_data {
 	struct p2ps_advertisement *p2ps_adv_list;
 	u8 wild_card_hash[P2PS_HASH_LEN];
 	u8 query_hash[P2P_MAX_QUERY_HASH * P2PS_HASH_LEN];
+	u8 query_count;
 	u8 p2ps_seek;
 	u8 p2ps_seek_count;
+	u8 p2ps_svc_found;
 
 #ifdef CONFIG_WIFI_DISPLAY
 	struct wpabuf *wfd_ie_beacon;