From 4bd7e1614f3edd859c638aaba2d9db4485fe1bb8 Mon Sep 17 00:00:00 2001
From: Tomasz Bursztyka <tomasz.bursztyka@linux.intel.com>
Date: Mon, 15 Sep 2014 08:04:30 +0300
Subject: [PATCH] wifi_display: Add a utility function to set WFD subelements
 from IEs

This will be useful to update the WFD subelements from DBus.

Signed-off-by: Tomasz Bursztyka <tomasz.bursztyka@linux.intel.com>
---
 wpa_supplicant/wifi_display.c | 54 +++++++++++++++++++++++++++++++++++
 wpa_supplicant/wifi_display.h |  2 ++
 2 files changed, 56 insertions(+)

diff --git a/wpa_supplicant/wifi_display.c b/wpa_supplicant/wifi_display.c
index 80c4e13ac..6dc41dec0 100644
--- a/wpa_supplicant/wifi_display.c
+++ b/wpa_supplicant/wifi_display.c
@@ -266,6 +266,60 @@ int wifi_display_subelem_set(struct wpa_global *global, char *cmd)
 }
 
 
+int wifi_display_subelem_set_from_ies(struct wpa_global *global,
+				      struct wpabuf *ie)
+{
+	int subelements[MAX_WFD_SUBELEMS] = {};
+	const u8 *pos, *end;
+	int len, subelem;
+	struct wpabuf *e;
+
+	wpa_printf(MSG_DEBUG, "WFD IEs set: %p - %lu",
+		   ie, ie ? (unsigned long) wpabuf_len(ie) : 0);
+
+	if (ie == NULL || wpabuf_len(ie) < 6)
+		return -1;
+
+	pos = wpabuf_head(ie);
+	end = pos + wpabuf_len(ie);
+
+	while (end > pos) {
+		if (pos + 3 > end)
+			break;
+
+		len = WPA_GET_BE16(pos + 1) + 3;
+
+		wpa_printf(MSG_DEBUG, "WFD Sub-Element ID %d - len %d",
+			   *pos, len - 3);
+
+		if (pos + len > end)
+			break;
+
+		subelem = *pos;
+		if (subelem < MAX_WFD_SUBELEMS && subelements[subelem] == 0) {
+			e = wpabuf_alloc_copy(pos, len);
+			if (e == NULL)
+				return -1;
+
+			wpabuf_free(global->wfd_subelem[subelem]);
+			global->wfd_subelem[subelem] = e;
+			subelements[subelem] = 1;
+		}
+
+		pos += len;
+	}
+
+	for (subelem = 0; subelem < MAX_WFD_SUBELEMS; subelem++) {
+		if (subelements[subelem] == 0) {
+			wpabuf_free(global->wfd_subelem[subelem]);
+			global->wfd_subelem[subelem] = NULL;
+		}
+	}
+
+	return wifi_display_update_wfd_ie(global);
+}
+
+
 int wifi_display_subelem_get(struct wpa_global *global, char *cmd,
 			     char *buf, size_t buflen)
 {
diff --git a/wpa_supplicant/wifi_display.h b/wpa_supplicant/wifi_display.h
index 67d8bc1af..0966bdb93 100644
--- a/wpa_supplicant/wifi_display.h
+++ b/wpa_supplicant/wifi_display.h
@@ -15,6 +15,8 @@ void wifi_display_deinit(struct wpa_global *global);
 void wifi_display_enable(struct wpa_global *global, int enabled);
 struct wpabuf *wifi_display_get_wfd_ie(struct wpa_global *global);
 int wifi_display_subelem_set(struct wpa_global *global, char *cmd);
+int wifi_display_subelem_set_from_ies(struct wpa_global *global,
+				      struct wpabuf *ie);
 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);