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>

1 files changed, 54 insertions, 0 deletions

diff --git a/wpa_supplicant/wifi_display.c b/wpa_supplicant/wifi_display.c
index 80c4e13..6dc41de 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)
 {