From b125c48fce823f28d22ebd68297c5b94366c6aa1 Mon Sep 17 00:00:00 2001
From: Dmitry Shmidt <dimitrysh@google.com>
Date: Mon, 25 Nov 2013 14:56:38 -0800
Subject: [PATCH] P2P: Add wfd_dev_info= field for device found event

This field allows adds enough information into the P2P-DEVICE-FOUND
events to figure out if the peer supports Wi-Fi Display.

Signed-off-by: Dmitry Shmidt <dimitrysh@google.com>
---
 wpa_supplicant/p2p_supplicant.c | 17 +++++++++++---
 wpa_supplicant/wifi_display.c   | 41 +++++++++++++++++++++++++++++++++
 wpa_supplicant/wifi_display.h   |  1 +
 3 files changed, 56 insertions(+), 3 deletions(-)

diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c
index eb2648a3a..6f5b81670 100644
--- a/wpa_supplicant/p2p_supplicant.c
+++ b/wpa_supplicant/p2p_supplicant.c
@@ -33,6 +33,7 @@
 #include "offchannel.h"
 #include "wps_supplicant.h"
 #include "p2p_supplicant.h"
+#include "wifi_display.h"
 
 
 /*
@@ -1442,18 +1443,28 @@ void wpas_dev_found(void *ctx, const u8 *addr,
 #ifndef CONFIG_NO_STDOUT_DEBUG
 	struct wpa_supplicant *wpa_s = ctx;
 	char devtype[WPS_DEV_TYPE_BUFSIZE];
+	char *wfd_dev_info_hex = NULL;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	wfd_dev_info_hex = wifi_display_subelem_hex(info->wfd_subelems,
+						    WFD_SUBELEM_DEVICE_INFO);
+#endif /* CONFIG_WIFI_DISPLAY */
 
 	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",
+		       "dev_capab=0x%x group_capab=0x%x%s%s",
 		       MAC2STR(addr), MAC2STR(info->p2p_device_addr),
 		       wps_dev_type_bin2str(info->pri_dev_type, devtype,
 					    sizeof(devtype)),
-		info->device_name, info->config_methods,
-		info->dev_capab, info->group_capab);
+		       info->device_name, info->config_methods,
+		       info->dev_capab, info->group_capab,
+		       wfd_dev_info_hex ? " wfd_dev_info=0x" : "",
+		       wfd_dev_info_hex ? wfd_dev_info_hex : "");
 #endif /* CONFIG_NO_STDOUT_DEBUG */
 
+	os_free(wfd_dev_info_hex);
+
 	wpas_notify_p2p_device_found(ctx, info->p2p_device_addr, new_device);
 }
 
diff --git a/wpa_supplicant/wifi_display.c b/wpa_supplicant/wifi_display.c
index 92ca5360e..578199ebe 100644
--- a/wpa_supplicant/wifi_display.c
+++ b/wpa_supplicant/wifi_display.c
@@ -16,6 +16,9 @@
 #include "wifi_display.h"
 
 
+#define WIFI_DISPLAY_SUBELEM_HEADER_LEN 3
+
+
 int wifi_display_init(struct wpa_global *global)
 {
 	global->wifi_display = 1;
@@ -249,3 +252,41 @@ int wifi_display_subelem_get(struct wpa_global *global, char *cmd,
 				1,
 				wpabuf_len(global->wfd_subelem[subelem]) - 1);
 }
+
+
+char * wifi_display_subelem_hex(const struct wpabuf *wfd_subelems, u8 id)
+{
+	char *subelem = NULL;
+	const u8 *buf;
+	size_t buflen;
+	size_t i = 0;
+	u16 elen;
+
+	if (!wfd_subelems)
+		return NULL;
+
+	buf = wpabuf_head_u8(wfd_subelems);
+	if (!buf)
+		return NULL;
+
+	buflen = wpabuf_len(wfd_subelems);
+
+	while (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN < buflen) {
+		elen = WPA_GET_BE16(buf + i + 1);
+
+		if (buf[i] == id) {
+			subelem = os_zalloc(2 * elen + 1);
+			if (!subelem)
+				return NULL;
+			wpa_snprintf_hex(subelem, 2 * elen + 1,
+					 buf + i +
+					 WIFI_DISPLAY_SUBELEM_HEADER_LEN,
+					 elen);
+			break;
+		}
+
+		i += elen + WIFI_DISPLAY_SUBELEM_HEADER_LEN;
+	}
+
+	return subelem;
+}
diff --git a/wpa_supplicant/wifi_display.h b/wpa_supplicant/wifi_display.h
index b75d4f2db..755481755 100644
--- a/wpa_supplicant/wifi_display.h
+++ b/wpa_supplicant/wifi_display.h
@@ -16,5 +16,6 @@ void wifi_display_enable(struct wpa_global *global, int enabled);
 int wifi_display_subelem_set(struct wpa_global *global, char *cmd);
 int wifi_display_subelem_get(struct wpa_global *global, char *cmd,
 			     char *buf, size_t buflen);
+char * wifi_display_subelem_hex(const struct wpabuf *wfd_subelems, u8 id);
 
 #endif /* WIFI_DISPLAY_H */