From 095b3c4069a905939e5ddd06918e73892526891c Mon Sep 17 00:00:00 2001
From: Brian Gix <bgix@qce.qualcomm.com>
Date: Thu, 11 Sep 2014 18:18:50 +0300
Subject: [PATCH] P2PS: Add Application Service Info to device found events

Signed-off-by: Jouni Malinen <jouni@qca.qualcomm.com>
---
 src/p2p/p2p.c                   | 14 +++++++++-
 src/p2p/p2p.h                   |  5 ++++
 src/p2p/p2p_i.h                 |  1 +
 wpa_supplicant/p2p_supplicant.c | 47 +++++++++++++++++++++++++++++++++
 4 files changed, 66 insertions(+), 1 deletion(-)

diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c
index f339e2259..7190760c4 100644
--- a/src/p2p/p2p.c
+++ b/src/p2p/p2p.c
@@ -772,6 +772,12 @@ int p2p_add_device(struct p2p_data *p2p, const u8 *addr, int freq,
 		dev->oper_ssid_len = msg.ssid[1];
 	}
 
+	if (msg.adv_service_instance && msg.adv_service_instance_len) {
+		wpabuf_free(dev->info.p2ps_instance);
+		dev->info.p2ps_instance = wpabuf_alloc_copy(
+			msg.adv_service_instance, msg.adv_service_instance_len);
+	}
+
 	if (freq >= 2412 && freq <= 2484 && msg.ds_params &&
 	    *msg.ds_params >= 1 && *msg.ds_params <= 14) {
 		int ds_freq;
@@ -831,7 +837,9 @@ int p2p_add_device(struct p2p_data *p2p, const u8 *addr, int freq,
 
 	p2p_update_peer_vendor_elems(dev, ies, ies_len);
 
-	if (dev->flags & P2P_DEV_REPORTED && !wfd_changed)
+	if (dev->flags & P2P_DEV_REPORTED && !wfd_changed &&
+	    (!msg.adv_service_instance ||
+	     (dev->flags & P2P_DEV_P2PS_REPORTED)))
 		return 0;
 
 	p2p_dbg(p2p, "Peer found with Listen frequency %d MHz (rx_time=%u.%06u)",
@@ -867,6 +875,9 @@ int p2p_add_device(struct p2p_data *p2p, const u8 *addr, int freq,
 			    !(dev->flags & P2P_DEV_REPORTED_ONCE));
 	dev->flags |= P2P_DEV_REPORTED | P2P_DEV_REPORTED_ONCE;
 
+	if (msg.adv_service_instance)
+		dev->flags |= P2P_DEV_P2PS_REPORTED;
+
 	return 0;
 }
 
@@ -901,6 +912,7 @@ static void p2p_device_free(struct p2p_data *p2p, struct p2p_device *dev)
 	wpabuf_free(dev->info.wfd_subelems);
 	wpabuf_free(dev->info.vendor_elems);
 	wpabuf_free(dev->go_neg_conf);
+	wpabuf_free(dev->info.p2ps_instance);
 
 	os_free(dev);
 }
diff --git a/src/p2p/p2p.h b/src/p2p/p2p.h
index 1ea13dae6..a6c6110c9 100644
--- a/src/p2p/p2p.h
+++ b/src/p2p/p2p.h
@@ -289,6 +289,11 @@ struct p2p_peer_info {
 	 * IE(s) from the frame that was used to discover the peer.
 	 */
 	struct wpabuf *vendor_elems;
+
+	/**
+	 * p2ps_instance - P2PS Application Service Info
+	 */
+	struct wpabuf *p2ps_instance;
 };
 
 enum p2p_prov_disc_status {
diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h
index 91c6c5820..127e484c2 100644
--- a/src/p2p/p2p_i.h
+++ b/src/p2p/p2p_i.h
@@ -105,6 +105,7 @@ struct p2p_device {
 #define P2P_DEV_PD_BEFORE_GO_NEG BIT(17)
 #define P2P_DEV_NO_PREF_CHAN BIT(18)
 #define P2P_DEV_WAIT_INV_REQ_ACK BIT(19)
+#define P2P_DEV_P2PS_REPORTED BIT(20)
 	unsigned int flags;
 
 	int status; /* enum p2p_status_code */
diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c
index 4325a12ae..bdc287394 100644
--- a/wpa_supplicant/p2p_supplicant.c
+++ b/wpa_supplicant/p2p_supplicant.c
@@ -1849,6 +1849,52 @@ static void wpas_dev_found(void *ctx, const u8 *addr,
 						    WFD_SUBELEM_DEVICE_INFO);
 #endif /* CONFIG_WIFI_DISPLAY */
 
+	if (info->p2ps_instance) {
+		char str[256];
+		const u8 *buf = wpabuf_head(info->p2ps_instance);
+		size_t len = wpabuf_len(info->p2ps_instance);
+
+		while (len) {
+			u32 id;
+			u16 methods;
+			u8 str_len;
+
+			if (len < 4 + 2 + 1)
+				break;
+			id = WPA_GET_LE32(buf);
+			buf += sizeof(u32);
+			methods = WPA_GET_BE16(buf);
+			buf += sizeof(u16);
+			str_len = *buf++;
+			if (str_len > len - 4 - 2 - 1)
+				break;
+			os_memcpy(str, buf, str_len);
+			str[str_len] = '\0';
+			buf += str_len;
+			len -= str_len + sizeof(u32) + sizeof(u16) + sizeof(u8);
+
+			wpa_msg_global(wpa_s, MSG_INFO,
+				       P2P_EVENT_DEVICE_FOUND MACSTR
+				       " p2p_dev_addr=" MACSTR
+				       " pri_dev_type=%s name='%s'"
+				       " config_methods=0x%x"
+				       " dev_capab=0x%x"
+				       " group_capab=0x%x"
+				       " adv_id=%x asp_svc=%s%s",
+				       MAC2STR(addr),
+				       MAC2STR(info->p2p_device_addr),
+				       wps_dev_type_bin2str(
+					       info->pri_dev_type,
+					       devtype, sizeof(devtype)),
+				       info->device_name, methods,
+				       info->dev_capab, info->group_capab,
+				       id, str,
+				       info->vendor_elems ?
+				       " vendor_elems=1" : "");
+		}
+		goto done;
+	}
+
 	wpa_msg_global(wpa_s, MSG_INFO, P2P_EVENT_DEVICE_FOUND MACSTR
 		       " p2p_dev_addr=" MACSTR
 		       " pri_dev_type=%s name='%s' config_methods=0x%x "
@@ -1863,6 +1909,7 @@ static void wpas_dev_found(void *ctx, const u8 *addr,
 		       info->vendor_elems ? " vendor_elems=1" : "",
 		       new_device);
 
+done:
 	os_free(wfd_dev_info_hex);
 #endif /* CONFIG_NO_STDOUT_DEBUG */