[PATCH] P2P: WFD: Add wfd_dev_info= field for device found event

[Dmitry Shmidt]

This field allows to get from P2P-DEVICE-FOUND event if device
is supporting WFD.

Signed-off-by: Dmitry Shmidt <dimitrysh at google.com>
---
 wpa_supplicant/p2p_supplicant.c | 18 +++++++++++++++---
 wpa_supplicant/wifi_display.c   | 34 ++++++++++++++++++++++++++++++++++
 wpa_supplicant/wifi_display.h   |  1 +
 3 files changed, 50 insertions(+), 3 deletions(-)

diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c
index eb2648a..db60b18 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,29 @@ 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 */
 
+	if (wfd_dev_info_hex)
+		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 92ca536..7a6ae3b 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	3
+
+
 int wifi_display_init(struct wpa_global *global)
 {
 	global->wifi_display = 1;
@@ -249,3 +252,34 @@ int wifi_display_subelem_get(struct wpa_global *global, char *cmd,
 				1,
 				wpabuf_len(global->wfd_subelem[subelem]) - 1);
 }
+
+
+char *wifi_display_subelem_hex(struct wpabuf *wfd_subelems, u8 id)
+{
+	char *subelem = NULL;
+	u8 *buf;
+	size_t buflen;
+	size_t i = 0;
+	u16 elen;
+
+	if (!wfd_subelems)
+		return subelem;
+	buf = (u8 *)wpabuf_head_u8(wfd_subelems);
+	buflen = wpabuf_len(wfd_subelems);
+	if (!buf)
+		return subelem;
+	while (i + WIFI_DISPLAY_SUBELEM_HEADER < buflen) {
+		elen = WPA_GET_BE16(buf + i + 1);
+		if (buf[i] == id) {
+			subelem = os_zalloc(2 * elen + 1);
+			if (!subelem)
+				return subelem;
+			wpa_snprintf_hex(subelem, 2 * elen + 1,
+					 buf + i + WIFI_DISPLAY_SUBELEM_HEADER,
+					 elen);
+			break;
+		}
+		i += (elen + WIFI_DISPLAY_SUBELEM_HEADER);
+	}
+	return subelem;
+}
diff --git a/wpa_supplicant/wifi_display.h b/wpa_supplicant/wifi_display.h
index b75d4f2..dd06391 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(struct wpabuf *wfd_subelems, u8 id);
 
 #endif /* WIFI_DISPLAY_H */
-- 
1.8.4.1