From a1fde16cf8e0a4f476e75a0b186f5f3662e5b080 Mon Sep 17 00:00:00 2001
From: Jouni Malinen <jouni@qca.qualcomm.com>
Date: Fri, 2 Mar 2012 17:26:01 +0200
Subject: WFD: Add Wi-Fi Display support

This commit adds control interface commands and internal storage of
Wi-Fi Display related configuration. In addition, WFD IE is now added
to various P2P frames, Probe Request/Response, and (Re)Association
Request/Response frames. WFD subelements from peers are stored in the
P2P peer table.

Following control interface commands are now available:
SET wifi_display <0/1>
GET wifi_display
WFD_SUBELEM_SET <subelem> [hexdump of length+body]
WFD_SUBELEM_GET <subelem>

Signed-hostap: Jouni Malinen <jouni@qca.qualcomm.com>
[vishalm@ti.com: Backported it to curent tree]
Signed-off-by: Vishal Mahaveer <vishalm@ti.com>

Change-Id: Ief0cd61903facd9209927d9dc84c8f093f3b6bac
---
 src/ap/ap_drv_ops.c               |  11 ++
 src/common/ieee802_11_common.c    |   5 +
 src/common/ieee802_11_common.h    |   2 +
 src/common/ieee802_11_defs.h      |  16 +++
 src/p2p/p2p.c                     | 218 ++++++++++++++++++++++++++++++++-
 src/p2p/p2p.h                     |  19 +++
 src/p2p/p2p_go_neg.c              |  42 ++++++-
 src/p2p/p2p_group.c               | 227 ++++++++++++++++++++++++++++++++--
 src/p2p/p2p_i.h                   |  17 +++
 src/p2p/p2p_invitation.c          |  55 ++++++++-
 src/p2p/p2p_parse.c               |  11 ++
 src/p2p/p2p_pd.c                  |  52 +++++++-
 wpa_supplicant/Android.mk         |   5 +
 wpa_supplicant/Makefile           |   5 +
 wpa_supplicant/ctrl_iface.c       |  21 ++++
 wpa_supplicant/wifi_display.c     | 251 ++++++++++++++++++++++++++++++++++++++
 wpa_supplicant/wifi_display.h     |  20 +++
 wpa_supplicant/wpa_cli.c          |  51 +++++++-
 wpa_supplicant/wpa_supplicant.c   |  12 ++
 wpa_supplicant/wpa_supplicant_i.h |   6 +
 20 files changed, 1023 insertions(+), 23 deletions(-)
 create mode 100644 wpa_supplicant/wifi_display.c
 create mode 100644 wpa_supplicant/wifi_display.h

diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c
index fbeaf9c..8488f11 100644
--- a/src/ap/ap_drv_ops.c
+++ b/src/ap/ap_drv_ops.c
@@ -12,6 +12,7 @@
 #include "drivers/driver.h"
 #include "common/ieee802_11_defs.h"
 #include "wps/wps.h"
+#include "p2p/p2p.h"
 #include "hostapd.h"
 #include "ieee802_11.h"
 #include "sta_info.h"
@@ -147,6 +148,16 @@ int hostapd_build_ap_extra_ies(struct hostapd_data *hapd,
 	}
 #endif /* CONFIG_P2P_MANAGER */
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (hapd->p2p_group) {
+		struct wpabuf *a;
+		a = p2p_group_assoc_resp_ie(hapd->p2p_group, P2P_SC_SUCCESS);
+		if (a && wpabuf_resize(&assocresp, wpabuf_len(a)) == 0)
+			wpabuf_put_buf(assocresp, a);
+		wpabuf_free(a);
+	}
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	*beacon_ret = beacon;
 	*proberesp_ret = proberesp;
 	*assocresp_ret = assocresp;
diff --git a/src/common/ieee802_11_common.c b/src/common/ieee802_11_common.c
index d65675c..f90870a 100644
--- a/src/common/ieee802_11_common.c
+++ b/src/common/ieee802_11_common.c
@@ -97,6 +97,11 @@ static int ieee802_11_parse_vendor_specific(const u8 *pos, size_t elen,
 			elems->p2p = pos;
 			elems->p2p_len = elen;
 			break;
+		case WFD_OUI_TYPE:
+			/* Wi-Fi Alliance - WFD IE */
+			elems->wfd = pos;
+			elems->wfd_len = elen;
+			break;
 		case HS20_INDICATION_OUI_TYPE:
 			/* Hotspot 2.0 */
 			elems->hs20 = pos;
diff --git a/src/common/ieee802_11_common.h b/src/common/ieee802_11_common.h
index d9b2b6c..66c7935 100644
--- a/src/common/ieee802_11_common.h
+++ b/src/common/ieee802_11_common.h
@@ -35,6 +35,7 @@ struct ieee802_11_elems {
 	const u8 *ht_operation;
 	const u8 *vendor_ht_cap;
 	const u8 *p2p;
+	const u8 *wfd;
 	const u8 *link_id;
 	const u8 *interworking;
 	const u8 *hs20;
@@ -65,6 +66,7 @@ struct ieee802_11_elems {
 	u8 ht_operation_len;
 	u8 vendor_ht_cap_len;
 	u8 p2p_len;
+	u8 wfd_len;
 	u8 interworking_len;
 	u8 hs20_len;
 	u8 ext_capab_len;
diff --git a/src/common/ieee802_11_defs.h b/src/common/ieee802_11_defs.h
index b4f9275..8e68c2a 100644
--- a/src/common/ieee802_11_defs.h
+++ b/src/common/ieee802_11_defs.h
@@ -701,6 +701,8 @@ struct ieee80211_vht_operation {
 #define WPS_IE_VENDOR_TYPE 0x0050f204
 #define OUI_WFA 0x506f9a
 #define P2P_IE_VENDOR_TYPE 0x506f9a09
+#define WFD_IE_VENDOR_TYPE 0x506f9a0a
+#define WFD_OUI_TYPE 10
 #define HS20_IE_VENDOR_TYPE 0x506f9a10
 
 #define WMM_OUI_TYPE 2
@@ -934,6 +936,20 @@ enum p2p_sd_status {
 };
 
 
+enum wifi_display_subelem {
+	WFD_SUBELEM_DEVICE_INFO = 0,
+	WFD_SUBELEM_ASSOCIATED_BSSID = 1,
+	WFD_SUBELEM_AUDIO_FORMATS = 2,
+	WFD_SUBELEM_VIDEO_FORMATS = 3,
+	WFD_SUBELEM_3D_VIDEO_FORMATS = 4,
+	WFD_SUBELEM_CONTENT_PROTECTION = 5,
+	WFD_SUBELEM_COUPLED_SINK = 6,
+	WFD_SUBELEM_EXT_CAPAB = 7,
+	WFD_SUBELEM_LOCAL_IP_ADDRESS = 8,
+	WFD_SUBELEM_SESSION_INFO = 9
+};
+
+
 #define OUI_BROADCOM 0x00904c /* Broadcom (Epigram) */
 
 #define VENDOR_HT_CAPAB_OUI_TYPE 0x33 /* 00-90-4c:0x33 */
diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c
index 824a59e..df6697a 100644
--- a/src/p2p/p2p.c
+++ b/src/p2p/p2p.c
@@ -753,6 +753,11 @@ int p2p_add_device(struct p2p_data *p2p, const u8 *addr, int freq,
 			break;
 	}
 
+	if (msg.wfd_subelems) {
+		wpabuf_free(dev->info.wfd_subelems);
+		dev->info.wfd_subelems = wpabuf_dup(msg.wfd_subelems);
+	}
+
 	if (scan_res) {
 		p2p_add_group_clients(p2p, p2p_dev_addr, addr, freq,
 				      msg.group_info, msg.group_info_len);
@@ -810,6 +815,8 @@ static void p2p_device_free(struct p2p_data *p2p, struct p2p_device *dev)
 		dev->info.wps_vendor_ext[i] = NULL;
 	}
 
+	wpabuf_free(dev->info.wfd_subelems);
+
 	os_free(dev);
 }
 
@@ -1454,6 +1461,11 @@ void p2p_add_dev_info(struct p2p_data *p2p, const u8 *addr,
 		}
 	}
 
+	if (msg->wfd_subelems) {
+		wpabuf_free(dev->info.wfd_subelems);
+		dev->info.wfd_subelems = wpabuf_dup(msg->wfd_subelems);
+	}
+
 	if (dev->flags & P2P_DEV_PROBE_REQ_ONLY) {
 		dev->flags &= ~P2P_DEV_PROBE_REQ_ONLY;
 		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
@@ -1791,6 +1803,11 @@ static void p2p_add_dev_from_probe_req(struct p2p_data *p2p, const u8 *addr,
 
 	p2p_copy_wps_info(dev, 1, &msg);
 
+	if (msg.wfd_subelems) {
+		wpabuf_free(dev->info.wfd_subelems);
+		dev->info.wfd_subelems = wpabuf_dup(msg.wfd_subelems);
+	}
+
 	p2p_parse_free(&msg);
 
 	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
@@ -1889,8 +1906,14 @@ struct wpabuf * p2p_build_probe_resp_ies(struct p2p_data *p2p)
 	struct wpabuf *buf;
 	u8 *len;
 	int pw_id = -1;
+	size_t extra = 0;
 
-	buf = wpabuf_alloc(1000);
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_probe_resp)
+		extra = wpabuf_len(p2p->wfd_ie_probe_resp);
+#endif /* CONFIG_WIFI_DISPLAY */
+
+	buf = wpabuf_alloc(1000 + extra);
 	if (buf == NULL)
 		return NULL;
 
@@ -1901,6 +1924,11 @@ struct wpabuf * p2p_build_probe_resp_ies(struct p2p_data *p2p)
 
 	p2p_build_wps_ie(p2p, buf, pw_id, 1);
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_probe_resp)
+		wpabuf_put_buf(buf, p2p->wfd_ie_probe_resp);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	/* P2P IE */
 	len = p2p_buf_add_ie_hdr(buf);
 	p2p_buf_add_capability(buf, p2p->dev_capab &
@@ -2185,20 +2213,31 @@ int p2p_assoc_req_ie(struct p2p_data *p2p, const u8 *bssid, u8 *buf,
 	struct p2p_device *peer;
 	size_t tmplen;
 	int res;
+	size_t extra = 0;
 
 	if (!p2p_group)
 		return p2p_assoc_req_ie_wlan_ap(p2p, bssid, buf, len, p2p_ie);
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_assoc_req)
+		extra = wpabuf_len(p2p->wfd_ie_assoc_req);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	/*
 	 * (Re)Association Request - P2P IE
 	 * P2P Capability attribute (shall be present)
 	 * Extended Listen Timing (may be present)
 	 * P2P Device Info attribute (shall be present)
 	 */
-	tmp = wpabuf_alloc(200);
+	tmp = wpabuf_alloc(200 + extra);
 	if (tmp == NULL)
 		return -1;
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_assoc_req)
+		wpabuf_put_buf(tmp, p2p->wfd_ie_assoc_req);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	peer = bssid ? p2p_get_device(p2p, bssid) : NULL;
 
 	lpos = p2p_buf_add_ie_hdr(tmp);
@@ -2391,6 +2430,20 @@ struct p2p_data * p2p_init(const struct p2p_config *cfg)
 
 void p2p_deinit(struct p2p_data *p2p)
 {
+#ifdef CONFIG_WIFI_DISPLAY
+	wpabuf_free(p2p->wfd_ie_beacon);
+	wpabuf_free(p2p->wfd_ie_probe_req);
+	wpabuf_free(p2p->wfd_ie_probe_resp);
+	wpabuf_free(p2p->wfd_ie_assoc_req);
+	wpabuf_free(p2p->wfd_ie_invitation);
+	wpabuf_free(p2p->wfd_ie_prov_disc_req);
+	wpabuf_free(p2p->wfd_ie_prov_disc_resp);
+	wpabuf_free(p2p->wfd_ie_go_neg);
+	wpabuf_free(p2p->wfd_dev_info);
+	wpabuf_free(p2p->wfd_assoc_bssid);
+	wpabuf_free(p2p->wfd_coupled_sink_info);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	eloop_cancel_timeout(p2p_expiration_timeout, p2p, NULL);
 	eloop_cancel_timeout(p2p_ext_listen_timeout, p2p, NULL);
 	eloop_cancel_timeout(p2p_scan_timeout, p2p, NULL);
@@ -2797,7 +2850,14 @@ void p2p_scan_res_handled(struct p2p_data *p2p)
 
 void p2p_scan_ie(struct p2p_data *p2p, struct wpabuf *ies, const u8 *dev_id)
 {
-	u8 *len = p2p_buf_add_ie_hdr(ies);
+	u8 *len;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_probe_req)
+		wpabuf_put_buf(ies, p2p->wfd_ie_probe_req);
+#endif /* CONFIG_WIFI_DISPLAY */
+
+	len = p2p_buf_add_ie_hdr(ies);
 	p2p_buf_add_capability(ies, p2p->dev_capab &
 			       ~P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY, 0);
 	if (dev_id)
@@ -2816,7 +2876,14 @@ void p2p_scan_ie(struct p2p_data *p2p, struct wpabuf *ies, const u8 *dev_id)
 
 size_t p2p_scan_ie_buf_len(struct p2p_data *p2p)
 {
-	return 100;
+	size_t len = 100;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p && p2p->wfd_ie_probe_req)
+		len += wpabuf_len(p2p->wfd_ie_probe_req);
+#endif /* CONFIG_WIFI_DISPLAY */
+
+	return len;
 }
 
 
@@ -3492,6 +3559,24 @@ int p2p_get_peer_info_txt(const struct p2p_peer_info *info,
 		pos += res;
 	}
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (dev->info.wfd_subelems) {
+		res = os_snprintf(pos, end - pos, "wfd_subelems=");
+		if (res < 0 || res >= end - pos)
+			return pos - buf;
+		pos += res;
+
+		pos += wpa_snprintf_hex(pos, end - pos,
+					wpabuf_head(dev->info.wfd_subelems),
+					wpabuf_len(dev->info.wfd_subelems));
+
+		res = os_snprintf(pos, end - pos, "\n");
+		if (res < 0 || res >= end - pos)
+			return pos - buf;
+		pos += res;
+	}
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	return pos - buf;
 }
 
@@ -4110,3 +4195,128 @@ void p2p_set_config_timeout(struct p2p_data *p2p, u8 go_timeout,
 		p2p->client_timeout = client_timeout;
 	}
 }
+
+
+#ifdef CONFIG_WIFI_DISPLAY
+
+static void p2p_update_wfd_ie_groups(struct p2p_data *p2p)
+{
+	size_t g;
+	struct p2p_group *group;
+
+	for (g = 0; g < p2p->num_groups; g++) {
+		group = p2p->groups[g];
+		p2p_group_update_ies(group);
+	}
+}
+
+
+int p2p_set_wfd_ie_beacon(struct p2p_data *p2p, struct wpabuf *ie)
+{
+	wpabuf_free(p2p->wfd_ie_beacon);
+	p2p->wfd_ie_beacon = ie;
+	p2p_update_wfd_ie_groups(p2p);
+	return 0;
+}
+
+
+int p2p_set_wfd_ie_probe_req(struct p2p_data *p2p, struct wpabuf *ie)
+{
+	wpabuf_free(p2p->wfd_ie_probe_req);
+	p2p->wfd_ie_probe_req = ie;
+	return 0;
+}
+
+
+int p2p_set_wfd_ie_probe_resp(struct p2p_data *p2p, struct wpabuf *ie)
+{
+	wpabuf_free(p2p->wfd_ie_probe_resp);
+	p2p->wfd_ie_probe_resp = ie;
+	p2p_update_wfd_ie_groups(p2p);
+	return 0;
+}
+
+
+int p2p_set_wfd_ie_assoc_req(struct p2p_data *p2p, struct wpabuf *ie)
+{
+	wpabuf_free(p2p->wfd_ie_assoc_req);
+	p2p->wfd_ie_assoc_req = ie;
+	return 0;
+}
+
+
+int p2p_set_wfd_ie_invitation(struct p2p_data *p2p, struct wpabuf *ie)
+{
+	wpabuf_free(p2p->wfd_ie_invitation);
+	p2p->wfd_ie_invitation = ie;
+	return 0;
+}
+
+
+int p2p_set_wfd_ie_prov_disc_req(struct p2p_data *p2p, struct wpabuf *ie)
+{
+	wpabuf_free(p2p->wfd_ie_prov_disc_req);
+	p2p->wfd_ie_prov_disc_req = ie;
+	return 0;
+}
+
+
+int p2p_set_wfd_ie_prov_disc_resp(struct p2p_data *p2p, struct wpabuf *ie)
+{
+	wpabuf_free(p2p->wfd_ie_prov_disc_resp);
+	p2p->wfd_ie_prov_disc_resp = ie;
+	return 0;
+}
+
+
+int p2p_set_wfd_ie_go_neg(struct p2p_data *p2p, struct wpabuf *ie)
+{
+	wpabuf_free(p2p->wfd_ie_go_neg);
+	p2p->wfd_ie_go_neg = ie;
+	return 0;
+}
+
+
+int p2p_set_wfd_dev_info(struct p2p_data *p2p, const struct wpabuf *elem)
+{
+	wpabuf_free(p2p->wfd_dev_info);
+	if (elem) {
+		p2p->wfd_dev_info = wpabuf_dup(elem);
+		if (p2p->wfd_dev_info == NULL)
+			return -1;
+	} else
+		p2p->wfd_dev_info = NULL;
+
+	return 0;
+}
+
+
+int p2p_set_wfd_assoc_bssid(struct p2p_data *p2p, const struct wpabuf *elem)
+{
+	wpabuf_free(p2p->wfd_assoc_bssid);
+	if (elem) {
+		p2p->wfd_assoc_bssid = wpabuf_dup(elem);
+		if (p2p->wfd_assoc_bssid == NULL)
+			return -1;
+	} else
+		p2p->wfd_assoc_bssid = NULL;
+
+	return 0;
+}
+
+
+int p2p_set_wfd_coupled_sink_info(struct p2p_data *p2p,
+				  const struct wpabuf *elem)
+{
+	wpabuf_free(p2p->wfd_coupled_sink_info);
+	if (elem) {
+		p2p->wfd_coupled_sink_info = wpabuf_dup(elem);
+		if (p2p->wfd_coupled_sink_info == NULL)
+			return -1;
+	} else
+		p2p->wfd_coupled_sink_info = NULL;
+
+	return 0;
+}
+
+#endif /* CONFIG_WIFI_DISPLAY */
diff --git a/src/p2p/p2p.h b/src/p2p/p2p.h
index a60974f..ff15aee 100644
--- a/src/p2p/p2p.h
+++ b/src/p2p/p2p.h
@@ -211,6 +211,11 @@ struct p2p_peer_info {
 	size_t wps_sec_dev_type_list_len;
 
 	struct wpabuf *wps_vendor_ext[P2P_MAX_WPS_VENDOR_EXT];
+
+	/**
+	 * wfd_subelems - Wi-Fi Display subelements from WFD IE(s)
+	 */
+	struct wpabuf *wfd_subelems;
 };
 
 enum p2p_prov_disc_status {
@@ -1731,4 +1736,18 @@ void p2p_set_config_timeout(struct p2p_data *p2p, u8 go_timeout,
  * @group: P2P group context from p2p_group_init()
  */
 const u8 *p2p_group_get_interface_addr(struct p2p_group *group);
+int p2p_set_wfd_ie_beacon(struct p2p_data *p2p, struct wpabuf *ie);
+int p2p_set_wfd_ie_probe_req(struct p2p_data *p2p, struct wpabuf *ie);
+int p2p_set_wfd_ie_probe_resp(struct p2p_data *p2p, struct wpabuf *ie);
+int p2p_set_wfd_ie_assoc_req(struct p2p_data *p2p, struct wpabuf *ie);
+int p2p_set_wfd_ie_invitation(struct p2p_data *p2p, struct wpabuf *ie);
+int p2p_set_wfd_ie_prov_disc_req(struct p2p_data *p2p, struct wpabuf *ie);
+int p2p_set_wfd_ie_prov_disc_resp(struct p2p_data *p2p, struct wpabuf *ie);
+int p2p_set_wfd_ie_go_neg(struct p2p_data *p2p, struct wpabuf *ie);
+int p2p_set_wfd_dev_info(struct p2p_data *p2p, const struct wpabuf *elem);
+int p2p_set_wfd_assoc_bssid(struct p2p_data *p2p, const struct wpabuf *elem);
+int p2p_set_wfd_coupled_sink_info(struct p2p_data *p2p,
+				  const struct wpabuf *elem);
+struct wpabuf * wifi_display_encaps(struct wpabuf *subelems);
+
 #endif /* P2P_H */
diff --git a/src/p2p/p2p_go_neg.c b/src/p2p/p2p_go_neg.c
index 60c19c6..3573fb7 100644
--- a/src/p2p/p2p_go_neg.c
+++ b/src/p2p/p2p_go_neg.c
@@ -134,8 +134,14 @@ static struct wpabuf * p2p_build_go_neg_req(struct p2p_data *p2p,
 	struct wpabuf *buf;
 	u8 *len;
 	u8 group_capab;
+	size_t extra = 0;
 
-	buf = wpabuf_alloc(1000);
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_go_neg)
+		extra = wpabuf_len(p2p->wfd_ie_go_neg);
+#endif /* CONFIG_WIFI_DISPLAY */
+
+	buf = wpabuf_alloc(1000 + extra);
 	if (buf == NULL)
 		return NULL;
 
@@ -177,6 +183,11 @@ static struct wpabuf * p2p_build_go_neg_req(struct p2p_data *p2p,
 	/* WPS IE with Device Password ID attribute */
 	p2p_build_wps_ie(p2p, buf, p2p_wps_method_pw_id(peer->wps_method), 0);
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_go_neg)
+		wpabuf_put_buf(buf, p2p->wfd_ie_go_neg);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	return buf;
 }
 
@@ -246,10 +257,17 @@ static struct wpabuf * p2p_build_go_neg_resp(struct p2p_data *p2p,
 	struct wpabuf *buf;
 	u8 *len;
 	u8 group_capab;
+	size_t extra = 0;
 
 	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 		"P2P: Building GO Negotiation Response");
-	buf = wpabuf_alloc(1000);
+
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_go_neg)
+		extra = wpabuf_len(p2p->wfd_ie_go_neg);
+#endif /* CONFIG_WIFI_DISPLAY */
+
+	buf = wpabuf_alloc(1000 + extra);
 	if (buf == NULL)
 		return NULL;
 
@@ -308,6 +326,12 @@ static struct wpabuf * p2p_build_go_neg_resp(struct p2p_data *p2p,
 			 p2p_wps_method_pw_id(peer ? peer->wps_method :
 					      WPS_NOT_READY), 0);
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_go_neg)
+		wpabuf_put_buf(buf, p2p->wfd_ie_go_neg);
+#endif /* CONFIG_WIFI_DISPLAY */
+
+
 	return buf;
 }
 
@@ -748,10 +772,17 @@ static struct wpabuf * p2p_build_go_neg_conf(struct p2p_data *p2p,
 	u8 *len;
 	struct p2p_channels res;
 	u8 group_capab;
+	size_t extra = 0;
 
 	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 		"P2P: Building GO Negotiation Confirm");
-	buf = wpabuf_alloc(1000);
+
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_go_neg)
+		extra = wpabuf_len(p2p->wfd_ie_go_neg);
+#endif /* CONFIG_WIFI_DISPLAY */
+
+	buf = wpabuf_alloc(1000 + extra);
 	if (buf == NULL)
 		return NULL;
 
@@ -790,6 +821,11 @@ static struct wpabuf * p2p_build_go_neg_conf(struct p2p_data *p2p,
 	}
 	p2p_buf_update_ie_hdr(buf, len);
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_go_neg)
+		wpabuf_put_buf(buf, p2p->wfd_ie_go_neg);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	return buf;
 }
 
diff --git a/src/p2p/p2p_group.c b/src/p2p/p2p_group.c
index 8d4a3cb..514b5f1 100644
--- a/src/p2p/p2p_group.c
+++ b/src/p2p/p2p_group.c
@@ -22,6 +22,7 @@ struct p2p_group_member {
 	u8 addr[ETH_ALEN]; /* P2P Interface Address */
 	u8 dev_addr[ETH_ALEN]; /* P2P Device Address */
 	struct wpabuf *p2p_ie;
+	struct wpabuf *wfd_ie;
 	struct wpabuf *client_info;
 	u8 dev_capab;
 };
@@ -37,12 +38,10 @@ struct p2p_group {
 	int group_formation;
 	int beacon_update;
 	struct wpabuf *noa;
+	struct wpabuf *wfd_ie;
 };
 
 
-static void p2p_group_update_ies(struct p2p_group *group);
-
-
 struct p2p_group * p2p_group_init(struct p2p_data *p2p,
 				  struct p2p_group_config *config)
 {
@@ -74,6 +73,7 @@ struct p2p_group * p2p_group_init(struct p2p_data *p2p,
 
 static void p2p_group_free_member(struct p2p_group_member *m)
 {
+	wpabuf_free(m->wfd_ie);
 	wpabuf_free(m->p2p_ie);
 	wpabuf_free(m->client_info);
 	os_free(m);
@@ -118,6 +118,7 @@ void p2p_group_deinit(struct p2p_group *group)
 	p2p_group_free_members(group);
 	os_free(group->cfg);
 	wpabuf_free(group->noa);
+	wpabuf_free(group->wfd_ie);
 	os_free(group);
 }
 
@@ -172,11 +173,22 @@ static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group)
 {
 	struct wpabuf *ie;
 	u8 *len;
+	size_t extra = 0;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	if (group->p2p->wfd_ie_beacon)
+		extra = wpabuf_len(group->p2p->wfd_ie_beacon);
+#endif /* CONFIG_WIFI_DISPLAY */
 
-	ie = wpabuf_alloc(257);
+	ie = wpabuf_alloc(257 + extra);
 	if (ie == NULL)
 		return NULL;
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (group->p2p->wfd_ie_beacon)
+		wpabuf_put_buf(ie, group->p2p->wfd_ie_beacon);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	len = p2p_buf_add_ie_hdr(ie);
 	p2p_group_add_common_ies(group, ie);
 	p2p_buf_add_device_id(ie, group->p2p->cfg->dev_addr);
@@ -187,17 +199,193 @@ static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group)
 }
 
 
+#ifdef CONFIG_WIFI_DISPLAY
+
+struct wpabuf * p2p_group_get_wfd_ie(struct p2p_group *g)
+{
+	return g->wfd_ie;
+}
+
+
+struct wpabuf * wifi_display_encaps(struct wpabuf *subelems)
+{
+	struct wpabuf *ie;
+	const u8 *pos, *end;
+
+	if (subelems == NULL)
+		return NULL;
+
+	ie = wpabuf_alloc(wpabuf_len(subelems) + 100);
+	if (ie == NULL)
+		return NULL;
+
+	pos = wpabuf_head(subelems);
+	end = pos + wpabuf_len(subelems);
+
+	while (end > pos) {
+		size_t frag_len = end - pos;
+		if (frag_len > 251)
+			frag_len = 251;
+		wpabuf_put_u8(ie, WLAN_EID_VENDOR_SPECIFIC);
+		wpabuf_put_u8(ie, 4 + frag_len);
+		wpabuf_put_be32(ie, WFD_IE_VENDOR_TYPE);
+		wpabuf_put_data(ie, pos, frag_len);
+		pos += frag_len;
+	}
+
+	return ie;
+}
+
+
+static int wifi_display_add_dev_info_descr(struct wpabuf *buf,
+					   struct p2p_group_member *m)
+{
+	const u8 *pos, *end;
+	const u8 *dev_info = NULL;
+	const u8 *assoc_bssid = NULL;
+	const u8 *coupled_sink = NULL;
+	u8 zero_addr[ETH_ALEN];
+
+	if (m->wfd_ie == NULL)
+		return 0;
+
+	os_memset(zero_addr, 0, ETH_ALEN);
+	pos = wpabuf_head_u8(m->wfd_ie);
+	end = pos + wpabuf_len(m->wfd_ie);
+	while (pos + 1 < end) {
+		u8 id;
+		u16 len;
+
+		id = *pos++;
+		len = WPA_GET_BE16(pos);
+		pos += 2;
+		if (pos + len > end)
+			break;
+
+		switch (id) {
+		case WFD_SUBELEM_DEVICE_INFO:
+			if (len < 6)
+				break;
+			dev_info = pos;
+			break;
+		case WFD_SUBELEM_ASSOCIATED_BSSID:
+			if (len < ETH_ALEN)
+				break;
+			assoc_bssid = pos;
+			break;
+		case WFD_SUBELEM_COUPLED_SINK:
+			if (len < 1 + ETH_ALEN)
+				break;
+			coupled_sink = pos;
+			break;
+		}
+
+		pos += len;
+	}
+
+	if (dev_info == NULL)
+		return 0;
+
+	wpabuf_put_u8(buf, 23);
+	wpabuf_put_data(buf, m->dev_addr, ETH_ALEN);
+	if (assoc_bssid)
+		wpabuf_put_data(buf, assoc_bssid, ETH_ALEN);
+	else
+		wpabuf_put_data(buf, zero_addr, ETH_ALEN);
+	wpabuf_put_data(buf, dev_info, 2); /* WFD Device Info */
+	wpabuf_put_data(buf, dev_info + 4, 2); /* WFD Device Max Throughput */
+	if (coupled_sink) {
+		wpabuf_put_data(buf, coupled_sink, 1 + ETH_ALEN);
+	} else {
+		wpabuf_put_u8(buf, 0);
+		wpabuf_put_data(buf, zero_addr, ETH_ALEN);
+	}
+
+	return 1;
+}
+
+
+static struct wpabuf *
+wifi_display_build_go_ie(struct p2p_group *group)
+{
+	struct wpabuf *wfd_subelems, *wfd_ie;
+	struct p2p_group_member *m;
+	u8 *len;
+	unsigned int count = 0;
+
+	if (!group->p2p->wfd_ie_probe_resp)
+		return NULL;
+
+	wfd_subelems = wpabuf_alloc(wpabuf_len(group->p2p->wfd_ie_probe_resp) +
+				    group->num_members * 24 + 100);
+	if (wfd_subelems == NULL)
+		return NULL;
+	if (group->p2p->wfd_dev_info)
+		wpabuf_put_buf(wfd_subelems, group->p2p->wfd_dev_info);
+	if (group->p2p->wfd_assoc_bssid)
+		wpabuf_put_buf(wfd_subelems,
+			       group->p2p->wfd_assoc_bssid);
+	if (group->p2p->wfd_coupled_sink_info)
+		wpabuf_put_buf(wfd_subelems,
+			       group->p2p->wfd_coupled_sink_info);
+
+	/* Build WFD Session Info */
+	wpabuf_put_u8(wfd_subelems, WFD_SUBELEM_SESSION_INFO);
+	len = wpabuf_put(wfd_subelems, 2);
+	m = group->members;
+	while (m) {
+		if (wifi_display_add_dev_info_descr(wfd_subelems, m))
+			count++;
+		m = m->next;
+	}
+
+	if (count == 0) {
+		/* No Wi-Fi Display clients - do not include subelement */
+		wfd_subelems->used -= 3;
+	} else {
+		WPA_PUT_BE16(len, (u8 *) wpabuf_put(wfd_subelems, 0) - len -
+			     2);
+		wpa_printf(MSG_DEBUG, "WFD: WFD Session Info: %u descriptors",
+			   count);
+	}
+
+	wfd_ie = wifi_display_encaps(wfd_subelems);
+	wpabuf_free(wfd_subelems);
+
+	return wfd_ie;
+}
+
+static void wifi_display_group_update(struct p2p_group *group)
+{
+	wpabuf_free(group->wfd_ie);
+	group->wfd_ie = wifi_display_build_go_ie(group);
+}
+
+#endif /* CONFIG_WIFI_DISPLAY */
+
+
 static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group)
 {
 	u8 *group_info;
 	struct wpabuf *ie;
 	struct p2p_group_member *m;
 	u8 *len;
+	size_t extra = 0;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	if (group->wfd_ie)
+		extra += wpabuf_len(group->wfd_ie);
+#endif /* CONFIG_WIFI_DISPLAY */
 
-	ie = wpabuf_alloc(257);
+	ie = wpabuf_alloc(257 + extra);
 	if (ie == NULL)
 		return NULL;
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (group->wfd_ie)
+		wpabuf_put_buf(ie, group->wfd_ie);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	len = p2p_buf_add_ie_hdr(ie);
 
 	p2p_group_add_common_ies(group, ie);
@@ -216,15 +404,20 @@ static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group)
 		     (u8 *) wpabuf_put(ie, 0) - group_info - 3);
 
 	p2p_buf_update_ie_hdr(ie, len);
+
 	return ie;
 }
 
 
-static void p2p_group_update_ies(struct p2p_group *group)
+void p2p_group_update_ies(struct p2p_group *group)
 {
 	struct wpabuf *beacon_ie;
 	struct wpabuf *probe_resp_ie;
 
+#ifdef CONFIG_WIFI_DISPLAY
+	wifi_display_group_update(group);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	probe_resp_ie = p2p_group_build_probe_resp_ie(group);
 	if (probe_resp_ie == NULL)
 		return;
@@ -354,6 +547,9 @@ int p2p_group_notif_assoc(struct p2p_group *group, const u8 *addr,
 						       &m->dev_capab,
 						       m->dev_addr);
 	}
+#ifdef CONFIG_WIFI_DISPLAY
+	m->wfd_ie = ieee802_11_vendor_ie_concat(ie, len, WFD_IE_VENDOR_TYPE);
+#endif /* CONFIG_WIFI_DISPLAY */
 
 	p2p_group_remove_member(group, addr);
 
@@ -361,8 +557,9 @@ int p2p_group_notif_assoc(struct p2p_group *group, const u8 *addr,
 	group->members = m;
 	group->num_members++;
 	wpa_msg(group->p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Add client " MACSTR
-		" to group (p2p=%d client_info=%d); num_members=%u/%u",
-		MAC2STR(addr), m->p2p_ie ? 1 : 0, m->client_info ? 1 : 0,
+		" to group (p2p=%d wfd=%d client_info=%d); num_members=%u/%u",
+		MAC2STR(addr), m->p2p_ie ? 1 : 0, m->wfd_ie ? 1 : 0,
+		m->client_info ? 1 : 0,
 		group->num_members, group->cfg->max_clients);
 	if (group->num_members == group->cfg->max_clients)
 		group->beacon_update = 1;
@@ -378,6 +575,12 @@ struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status)
 {
 	struct wpabuf *resp;
 	u8 *rlen;
+	size_t extra = 0;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	if (group->wfd_ie)
+		extra = wpabuf_len(group->wfd_ie);
+#endif /* CONFIG_WIFI_DISPLAY */
 
 	/*
 	 * (Re)Association Response - P2P IE
@@ -385,9 +588,15 @@ struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status)
 	 *	denied)
 	 * Extended Listen Timing (may be present)
 	 */
-	resp = wpabuf_alloc(20);
+	resp = wpabuf_alloc(20 + extra);
 	if (resp == NULL)
 		return NULL;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	if (group->wfd_ie)
+		wpabuf_put_buf(resp, group->wfd_ie);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	rlen = p2p_buf_add_ie_hdr(resp);
 	if (status != P2P_SC_SUCCESS)
 		p2p_buf_add_status(resp, status);
diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h
index 9e3fd6f..1a15325 100644
--- a/src/p2p/p2p_i.h
+++ b/src/p2p/p2p_i.h
@@ -435,6 +435,20 @@ struct p2p_data {
 
 	u8 go_timeout;
 	u8 client_timeout;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	struct wpabuf *wfd_ie_beacon;
+	struct wpabuf *wfd_ie_probe_req;
+	struct wpabuf *wfd_ie_probe_resp;
+	struct wpabuf *wfd_ie_assoc_req;
+	struct wpabuf *wfd_ie_invitation;
+	struct wpabuf *wfd_ie_prov_disc_req;
+	struct wpabuf *wfd_ie_prov_disc_resp;
+	struct wpabuf *wfd_ie_go_neg;
+	struct wpabuf *wfd_dev_info;
+	struct wpabuf *wfd_assoc_bssid;
+	struct wpabuf *wfd_coupled_sink_info;
+#endif /* CONFIG_WIFI_DISPLAY */
 };
 
 /**
@@ -443,6 +457,7 @@ struct p2p_data {
 struct p2p_message {
 	struct wpabuf *p2p_attributes;
 	struct wpabuf *wps_attributes;
+	struct wpabuf *wfd_subelems;
 
 	u8 dialog_token;
 
@@ -562,6 +577,8 @@ u8 p2p_group_presence_req(struct p2p_group *group,
 			  const u8 *noa, size_t noa_len);
 int p2p_group_is_group_id_match(struct p2p_group *group, const u8 *group_id,
 				size_t group_id_len);
+void p2p_group_update_ies(struct p2p_group *group);
+struct wpabuf * p2p_group_get_wfd_ie(struct p2p_group *g);
 
 
 void p2p_buf_add_action_hdr(struct wpabuf *buf, u8 subtype, u8 dialog_token);
diff --git a/src/p2p/p2p_invitation.c b/src/p2p/p2p_invitation.c
index c70fc7a..8e3f5d2 100644
--- a/src/p2p/p2p_invitation.c
+++ b/src/p2p/p2p_invitation.c
@@ -21,8 +21,27 @@ static struct wpabuf * p2p_build_invitation_req(struct p2p_data *p2p,
 	struct wpabuf *buf;
 	u8 *len;
 	const u8 *dev_addr;
+	size_t extra = 0;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	struct wpabuf *wfd_ie = p2p->wfd_ie_invitation;
+	if (wfd_ie && p2p->inv_role == P2P_INVITE_ROLE_ACTIVE_GO) {
+		size_t i;
+		for (i = 0; i < p2p->num_groups; i++) {
+			struct p2p_group *g = p2p->groups[i];
+			struct wpabuf *ie;
+			ie = p2p_group_get_wfd_ie(g);
+			if (ie) {
+				wfd_ie = ie;
+				break;
+			}
+		}
+	}
+	if (wfd_ie)
+		extra = wpabuf_len(wfd_ie);
+#endif /* CONFIG_WIFI_DISPLAY */
 
-	buf = wpabuf_alloc(1000);
+	buf = wpabuf_alloc(1000 + extra);
 	if (buf == NULL)
 		return NULL;
 
@@ -55,6 +74,11 @@ static struct wpabuf * p2p_build_invitation_req(struct p2p_data *p2p,
 	p2p_buf_add_device_info(buf, p2p, peer);
 	p2p_buf_update_ie_hdr(buf, len);
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (wfd_ie)
+		wpabuf_put_buf(buf, wfd_ie);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	return buf;
 }
 
@@ -68,8 +92,30 @@ static struct wpabuf * p2p_build_invitation_resp(struct p2p_data *p2p,
 {
 	struct wpabuf *buf;
 	u8 *len;
+	size_t extra = 0;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	struct wpabuf *wfd_ie = p2p->wfd_ie_invitation;
+	if (wfd_ie && group_bssid) {
+		size_t i;
+		for (i = 0; i < p2p->num_groups; i++) {
+			struct p2p_group *g = p2p->groups[i];
+			struct wpabuf *ie;
+			if (!p2p_group_is_group_id_match(g, group_bssid,
+							 ETH_ALEN))
+				continue;
+			ie = p2p_group_get_wfd_ie(g);
+			if (ie) {
+				wfd_ie = ie;
+				break;
+			}
+		}
+	}
+	if (wfd_ie)
+		extra = wpabuf_len(wfd_ie);
+#endif /* CONFIG_WIFI_DISPLAY */
 
-	buf = wpabuf_alloc(1000);
+	buf = wpabuf_alloc(1000 + extra);
 	if (buf == NULL)
 		return NULL;
 
@@ -88,6 +134,11 @@ static struct wpabuf * p2p_build_invitation_resp(struct p2p_data *p2p,
 		p2p_buf_add_channel_list(buf, p2p->cfg->country, channels);
 	p2p_buf_update_ie_hdr(buf, len);
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (wfd_ie)
+		wpabuf_put_buf(buf, wfd_ie);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	return buf;
 }
 
diff --git a/src/p2p/p2p_parse.c b/src/p2p/p2p_parse.c
index a3ec57d..097a31d 100644
--- a/src/p2p/p2p_parse.c
+++ b/src/p2p/p2p_parse.c
@@ -414,6 +414,13 @@ int p2p_parse_ies(const u8 *data, size_t len, struct p2p_message *msg)
 		return -1;
 	}
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (elems.wfd) {
+		msg->wfd_subelems = ieee802_11_vendor_ie_concat(
+			data, len, WFD_IE_VENDOR_TYPE);
+	}
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	return 0;
 }
 
@@ -453,6 +460,10 @@ void p2p_parse_free(struct p2p_message *msg)
 	msg->p2p_attributes = NULL;
 	wpabuf_free(msg->wps_attributes);
 	msg->wps_attributes = NULL;
+#ifdef CONFIG_WIFI_DISPLAY
+	wpabuf_free(msg->wfd_subelems);
+	msg->wfd_subelems = NULL;
+#endif /* CONFIG_WIFI_DISPLAY */
 }
 
 
diff --git a/src/p2p/p2p_pd.c b/src/p2p/p2p_pd.c
index 007b643..610f0df 100644
--- a/src/p2p/p2p_pd.c
+++ b/src/p2p/p2p_pd.c
@@ -46,8 +46,14 @@ static struct wpabuf * p2p_build_prov_disc_req(struct p2p_data *p2p,
 {
 	struct wpabuf *buf;
 	u8 *len;
+	size_t extra = 0;
 
-	buf = wpabuf_alloc(1000);
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_prov_disc_req)
+		extra = wpabuf_len(p2p->wfd_ie_prov_disc_req);
+#endif /* CONFIG_WIFI_DISPLAY */
+
+	buf = wpabuf_alloc(1000 + extra);
 	if (buf == NULL)
 		return NULL;
 
@@ -66,17 +72,46 @@ static struct wpabuf * p2p_build_prov_disc_req(struct p2p_data *p2p,
 	/* WPS IE with Config Methods attribute */
 	p2p_build_wps_ie_config_methods(buf, config_methods);
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (p2p->wfd_ie_prov_disc_req)
+		wpabuf_put_buf(buf, p2p->wfd_ie_prov_disc_req);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	return buf;
 }
 
 
 static struct wpabuf * p2p_build_prov_disc_resp(struct p2p_data *p2p,
 						u8 dialog_token,
-						u16 config_methods)
+						u16 config_methods,
+						const u8 *group_id,
+						size_t group_id_len)
 {
 	struct wpabuf *buf;
+	size_t extra = 0;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	struct wpabuf *wfd_ie = p2p->wfd_ie_prov_disc_resp;
+	if (wfd_ie && group_id) {
+		size_t i;
+		for (i = 0; i < p2p->num_groups; i++) {
+			struct p2p_group *g = p2p->groups[i];
+			struct wpabuf *ie;
+			if (!p2p_group_is_group_id_match(g, group_id,
+							 group_id_len))
+				continue;
+			ie = p2p_group_get_wfd_ie(g);
+			if (ie) {
+				wfd_ie = ie;
+				break;
+			}
+		}
+	}
+	if (wfd_ie)
+		extra = wpabuf_len(wfd_ie);
+#endif /* CONFIG_WIFI_DISPLAY */
 
-	buf = wpabuf_alloc(100);
+	buf = wpabuf_alloc(100 + extra);
 	if (buf == NULL)
 		return NULL;
 
@@ -85,6 +120,11 @@ static struct wpabuf * p2p_build_prov_disc_resp(struct p2p_data *p2p,
 	/* WPS IE with Config Methods attribute */
 	p2p_build_wps_ie_config_methods(buf, config_methods);
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (wfd_ie)
+		wpabuf_put_buf(buf, wfd_ie);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	return buf;
 }
 
@@ -119,6 +159,9 @@ void p2p_process_prov_disc_req(struct p2p_data *p2p, const u8 *sa,
 			        "P2P: Provision Discovery Request add device "
 				"failed " MACSTR, MAC2STR(sa));
 		}
+	} else if (msg.wfd_subelems) {
+		wpabuf_free(dev->info.wfd_subelems);
+		dev->info.wfd_subelems = wpabuf_dup(msg.wfd_subelems);
 	}
 
 	if (!(msg.wps_config_methods &
@@ -164,7 +207,8 @@ void p2p_process_prov_disc_req(struct p2p_data *p2p, const u8 *sa,
 
 out:
 	resp = p2p_build_prov_disc_resp(p2p, msg.dialog_token,
-					reject ? 0 : msg.wps_config_methods);
+					reject ? 0 : msg.wps_config_methods,
+					msg.group_id, msg.group_id_len);
 	if (resp == NULL) {
 		p2p_parse_free(&msg);
 		return;
diff --git a/wpa_supplicant/Android.mk b/wpa_supplicant/Android.mk
index 5455538..d3d530f 100644
--- a/wpa_supplicant/Android.mk
+++ b/wpa_supplicant/Android.mk
@@ -234,6 +234,11 @@ L_CFLAGS += -DCONFIG_P2P_STRICT
 endif
 endif
 
+ifdef CONFIG_WIFI_DISPLAY
+L_CFLAGS += -DCONFIG_WIFI_DISPLAY
+OBJS += wifi_display.c
+endif
+
 ifdef CONFIG_INTERWORKING
 OBJS += interworking.c
 L_CFLAGS += -DCONFIG_INTERWORKING
diff --git a/wpa_supplicant/Makefile b/wpa_supplicant/Makefile
index 6756e54..803250c 100644
--- a/wpa_supplicant/Makefile
+++ b/wpa_supplicant/Makefile
@@ -227,6 +227,11 @@ CFLAGS += -DCONFIG_P2P_STRICT
 endif
 endif
 
+ifdef CONFIG_WIFI_DISPLAY
+CFLAGS += -DCONFIG_WIFI_DISPLAY
+OBJS += wifi_display.o
+endif
+
 ifdef CONFIG_HS20
 OBJS += hs20_supplicant.o
 CFLAGS += -DCONFIG_HS20
diff --git a/wpa_supplicant/ctrl_iface.c b/wpa_supplicant/ctrl_iface.c
index 81cd363..b70c6f4 100644
--- a/wpa_supplicant/ctrl_iface.c
+++ b/wpa_supplicant/ctrl_iface.c
@@ -29,6 +29,7 @@
 #include "p2p_supplicant.h"
 #include "p2p/p2p.h"
 #include "hs20_supplicant.h"
+#include "wifi_display.h"
 #include "notify.h"
 #include "bss.h"
 #include "scan.h"
@@ -283,6 +284,10 @@ static int wpa_supplicant_ctrl_iface_set(struct wpa_supplicant *wpa_s,
 		}
 	} else if (os_strcasecmp(cmd, "ps") == 0) {
 		ret = wpa_drv_set_p2p_powersave(wpa_s, atoi(value), -1, -1);
+#ifdef CONFIG_WIFI_DISPLAY
+	} else if (os_strcasecmp(cmd, "wifi_display") == 0) {
+		wifi_display_enable(wpa_s->global, !!atoi(value));
+#endif /* CONFIG_WIFI_DISPLAY */
 	} else if (os_strcasecmp(cmd, "bssid_filter") == 0) {
 		ret = set_bssid_filter(wpa_s, value);
 	} else if (os_strcasecmp(cmd, "roaming_disabled") == 0) {
@@ -317,6 +322,14 @@ static int wpa_supplicant_ctrl_iface_get(struct wpa_supplicant *wpa_s,
 			res = os_snprintf(buf, buflen, "%c%c",
 					  wpa_s->conf->country[0],
 					  wpa_s->conf->country[1]);
+#ifdef CONFIG_WIFI_DISPLAY
+	} else if (os_strcasecmp(cmd, "wifi_display") == 0) {
+		res = os_snprintf(buf, buflen, "%d",
+				  wpa_s->global->wifi_display);
+		if (res < 0 || (unsigned int) res >= buflen)
+			return -1;
+		return res;
+#endif /* CONFIG_WIFI_DISPLAY */
 	}
 
 	if (res < 0 || (unsigned int) res >= buflen)
@@ -4531,6 +4544,14 @@ char * wpa_supplicant_ctrl_iface_process(struct wpa_supplicant *wpa_s,
 		if (p2p_ctrl_ext_listen(wpa_s, "") < 0)
 			reply_len = -1;
 #endif /* CONFIG_P2P */
+#ifdef CONFIG_WIFI_DISPLAY
+	} else if (os_strncmp(buf, "WFD_SUBELEM_SET ", 16) == 0) {
+		if (wifi_display_subelem_set(wpa_s->global, buf + 16) < 0)
+			reply_len = -1;
+	} else if (os_strncmp(buf, "WFD_SUBELEM_GET ", 16) == 0) {
+		reply_len = wifi_display_subelem_get(wpa_s->global, buf + 16,
+						     reply, reply_size);
+#endif /* CONFIG_WIFI_DISPLAY */
 #ifdef CONFIG_INTERWORKING
 	} else if (os_strcmp(buf, "FETCH_ANQP") == 0) {
 		if (interworking_fetch_anqp(wpa_s) < 0)
diff --git a/wpa_supplicant/wifi_display.c b/wpa_supplicant/wifi_display.c
new file mode 100644
index 0000000..92ca536
--- /dev/null
+++ b/wpa_supplicant/wifi_display.c
@@ -0,0 +1,251 @@
+/*
+ * wpa_supplicant - Wi-Fi Display
+ * Copyright (c) 2011, Atheros Communications, Inc.
+ * Copyright (c) 2011-2012, Qualcomm Atheros, Inc.
+ *
+ * This software may be distributed under the terms of the BSD license.
+ * See README for more details.
+ */
+
+#include "includes.h"
+
+#include "common.h"
+#include "p2p/p2p.h"
+#include "common/ieee802_11_defs.h"
+#include "wpa_supplicant_i.h"
+#include "wifi_display.h"
+
+
+int wifi_display_init(struct wpa_global *global)
+{
+	global->wifi_display = 1;
+	return 0;
+}
+
+
+void wifi_display_deinit(struct wpa_global *global)
+{
+	int i;
+	for (i = 0; i < MAX_WFD_SUBELEMS; i++) {
+		wpabuf_free(global->wfd_subelem[i]);
+		global->wfd_subelem[i] = NULL;
+	}
+}
+
+
+static int wifi_display_update_wfd_ie(struct wpa_global *global)
+{
+	struct wpabuf *ie, *buf;
+	size_t len, plen;
+
+	wpa_printf(MSG_DEBUG, "WFD: Update WFD IE");
+
+	if (!global->wifi_display) {
+		wpa_printf(MSG_DEBUG, "WFD: Wi-Fi Display disabled - do not "
+			   "include WFD IE");
+		p2p_set_wfd_ie_beacon(global->p2p, NULL);
+		p2p_set_wfd_ie_probe_req(global->p2p, NULL);
+		p2p_set_wfd_ie_probe_resp(global->p2p, NULL);
+		p2p_set_wfd_ie_assoc_req(global->p2p, NULL);
+		p2p_set_wfd_ie_invitation(global->p2p, NULL);
+		p2p_set_wfd_ie_prov_disc_req(global->p2p, NULL);
+		p2p_set_wfd_ie_prov_disc_resp(global->p2p, NULL);
+		p2p_set_wfd_ie_go_neg(global->p2p, NULL);
+		p2p_set_wfd_dev_info(global->p2p, NULL);
+		p2p_set_wfd_assoc_bssid(global->p2p, NULL);
+		p2p_set_wfd_coupled_sink_info(global->p2p, NULL);
+		return 0;
+	}
+
+	p2p_set_wfd_dev_info(global->p2p,
+			     global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]);
+	p2p_set_wfd_assoc_bssid(
+		global->p2p,
+		global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID]);
+	p2p_set_wfd_coupled_sink_info(
+		global->p2p, global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]);
+
+	/*
+	 * WFD IE is included in number of management frames. Two different
+	 * sets of subelements are included depending on the frame:
+	 *
+	 * Beacon, (Re)Association Request, GO Negotiation Req/Resp/Conf,
+	 * Provision Discovery Req:
+	 * WFD Device Info
+	 * [Associated BSSID]
+	 * [Coupled Sink Info]
+	 *
+	 * Probe Request:
+	 * WFD Device Info
+	 * [Associated BSSID]
+	 * [Coupled Sink Info]
+	 * [WFD Extended Capability]
+	 *
+	 * Probe Response:
+	 * WFD Device Info
+	 * [Associated BSSID]
+	 * [Coupled Sink Info]
+	 * [WFD Extended Capability]
+	 * [WFD Session Info]
+	 *
+	 * (Re)Association Response, P2P Invitation Req/Resp,
+	 * Provision Discovery Resp:
+	 * WFD Device Info
+	 * [Associated BSSID]
+	 * [Coupled Sink Info]
+	 * [WFD Session Info]
+	 */
+	len = 0;
+	if (global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO])
+		len += wpabuf_len(global->wfd_subelem[
+					  WFD_SUBELEM_DEVICE_INFO]);
+	if (global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID])
+		len += wpabuf_len(global->wfd_subelem[
+					  WFD_SUBELEM_ASSOCIATED_BSSID]);
+	if (global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK])
+		len += wpabuf_len(global->wfd_subelem[
+					  WFD_SUBELEM_COUPLED_SINK]);
+	if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO])
+		len += wpabuf_len(global->wfd_subelem[
+					  WFD_SUBELEM_SESSION_INFO]);
+	if (global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB])
+		len += wpabuf_len(global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]);
+	buf = wpabuf_alloc(len);
+	if (buf == NULL)
+		return -1;
+
+	if (global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO])
+		wpabuf_put_buf(buf,
+			       global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]);
+	if (global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID])
+		wpabuf_put_buf(buf, global->wfd_subelem[
+				       WFD_SUBELEM_ASSOCIATED_BSSID]);
+	if (global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK])
+		wpabuf_put_buf(buf,
+			       global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]);
+
+	ie = wifi_display_encaps(buf);
+	wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Beacon", ie);
+	p2p_set_wfd_ie_beacon(global->p2p, ie);
+
+	ie = wifi_display_encaps(buf);
+	wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for (Re)Association Request",
+			ie);
+	p2p_set_wfd_ie_assoc_req(global->p2p, ie);
+
+	ie = wifi_display_encaps(buf);
+	wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for GO Negotiation", ie);
+	p2p_set_wfd_ie_go_neg(global->p2p, ie);
+
+	ie = wifi_display_encaps(buf);
+	wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Provision Discovery "
+			"Request", ie);
+	p2p_set_wfd_ie_prov_disc_req(global->p2p, ie);
+
+	plen = buf->used;
+	if (global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB])
+		wpabuf_put_buf(buf,
+			       global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]);
+
+	ie = wifi_display_encaps(buf);
+	wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Probe Request", ie);
+	p2p_set_wfd_ie_probe_req(global->p2p, ie);
+
+	if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO])
+		wpabuf_put_buf(buf,
+			       global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]);
+	ie = wifi_display_encaps(buf);
+	wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Probe Response", ie);
+	p2p_set_wfd_ie_probe_resp(global->p2p, ie);
+
+	/* Remove WFD Extended Capability from buffer */
+	buf->used = plen;
+	if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO])
+		wpabuf_put_buf(buf,
+			       global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]);
+
+	ie = wifi_display_encaps(buf);
+	wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for P2P Invitation", ie);
+	p2p_set_wfd_ie_invitation(global->p2p, ie);
+
+	ie = wifi_display_encaps(buf);
+	wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Provision Discovery "
+			"Response", ie);
+	p2p_set_wfd_ie_prov_disc_resp(global->p2p, ie);
+
+	wpabuf_free(buf);
+
+	return 0;
+}
+
+
+void wifi_display_enable(struct wpa_global *global, int enabled)
+{
+	wpa_printf(MSG_DEBUG, "WFD: Wi-Fi Display %s",
+		   enabled ? "enabled" : "disabled");
+	global->wifi_display = enabled;
+	wifi_display_update_wfd_ie(global);
+}
+
+
+int wifi_display_subelem_set(struct wpa_global *global, char *cmd)
+{
+	char *pos;
+	int subelem;
+	size_t len;
+	struct wpabuf *e;
+
+	pos = os_strchr(cmd, ' ');
+	if (pos == NULL)
+		return -1;
+	*pos++ = '\0';
+	subelem = atoi(cmd);
+	if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS)
+		return -1;
+
+	len = os_strlen(pos);
+	if (len & 1)
+		return -1;
+	len /= 2;
+
+	if (len == 0) {
+		/* Clear subelement */
+		e = NULL;
+		wpa_printf(MSG_DEBUG, "WFD: Clear subelement %d", subelem);
+	} else {
+		e = wpabuf_alloc(1 + len);
+		if (e == NULL)
+			return -1;
+		wpabuf_put_u8(e, subelem);
+		if (hexstr2bin(pos, wpabuf_put(e, len), len) < 0) {
+			wpabuf_free(e);
+			return -1;
+		}
+		wpa_printf(MSG_DEBUG, "WFD: Set subelement %d", subelem);
+	}
+
+	wpabuf_free(global->wfd_subelem[subelem]);
+	global->wfd_subelem[subelem] = e;
+	wifi_display_update_wfd_ie(global);
+
+	return 0;
+}
+
+
+int wifi_display_subelem_get(struct wpa_global *global, char *cmd,
+			     char *buf, size_t buflen)
+{
+	int subelem;
+
+	subelem = atoi(cmd);
+	if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS)
+		return -1;
+
+	if (global->wfd_subelem[subelem] == NULL)
+		return 0;
+
+	return wpa_snprintf_hex(buf, buflen,
+				wpabuf_head_u8(global->wfd_subelem[subelem]) +
+				1,
+				wpabuf_len(global->wfd_subelem[subelem]) - 1);
+}
diff --git a/wpa_supplicant/wifi_display.h b/wpa_supplicant/wifi_display.h
new file mode 100644
index 0000000..b75d4f2
--- /dev/null
+++ b/wpa_supplicant/wifi_display.h
@@ -0,0 +1,20 @@
+/*
+ * wpa_supplicant - Wi-Fi Display
+ * Copyright (c) 2011, Atheros Communications, Inc.
+ * Copyright (c) 2011-2012, Qualcomm Atheros, Inc.
+ *
+ * This software may be distributed under the terms of the BSD license.
+ * See README for more details.
+ */
+
+#ifndef WIFI_DISPLAY_H
+#define WIFI_DISPLAY_H
+
+int wifi_display_init(struct wpa_global *global);
+void wifi_display_deinit(struct wpa_global *global);
+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);
+
+#endif /* WIFI_DISPLAY_H */
diff --git a/wpa_supplicant/wpa_cli.c b/wpa_supplicant/wpa_cli.c
index 3f94854..37f8e44 100644
--- a/wpa_supplicant/wpa_cli.c
+++ b/wpa_supplicant/wpa_cli.c
@@ -2712,6 +2712,50 @@ static int wpa_cli_cmd_p2p_ext_listen(struct wpa_ctrl *ctrl, int argc,
 
 #endif /* CONFIG_P2P */
 
+#ifdef CONFIG_WIFI_DISPLAY
+
+static int wpa_cli_cmd_wfd_subelem_set(struct wpa_ctrl *ctrl, int argc,
+				       char *argv[])
+{
+	char cmd[100];
+	int res;
+
+	if (argc != 1 && argc != 2) {
+		printf("Invalid WFD_SUBELEM_SET command: needs one or two "
+		       "arguments (subelem, hexdump)\n");
+		return -1;
+	}
+
+	res = os_snprintf(cmd, sizeof(cmd), "WFD_SUBELEM_SET %s %s",
+			  argv[0], argc > 1 ? argv[1] : "");
+	if (res < 0 || (size_t) res >= sizeof(cmd))
+		return -1;
+	cmd[sizeof(cmd) - 1] = '\0';
+	return wpa_ctrl_command(ctrl, cmd);
+}
+
+
+static int wpa_cli_cmd_wfd_subelem_get(struct wpa_ctrl *ctrl, int argc,
+				       char *argv[])
+{
+	char cmd[100];
+	int res;
+
+	if (argc != 1) {
+		printf("Invalid WFD_SUBELEM_GET command: needs one "
+		       "argument (subelem)\n");
+		return -1;
+	}
+
+	res = os_snprintf(cmd, sizeof(cmd), "WFD_SUBELEM_GET %s",
+			  argv[0]);
+	if (res < 0 || (size_t) res >= sizeof(cmd))
+		return -1;
+	cmd[sizeof(cmd) - 1] = '\0';
+	return wpa_ctrl_command(ctrl, cmd);
+}
+#endif /* CONFIG_WIFI_DISPLAY */
+
 
 #ifdef CONFIG_INTERWORKING
 static int wpa_cli_cmd_fetch_anqp(struct wpa_ctrl *ctrl, int argc,
@@ -3333,7 +3377,12 @@ static struct wpa_cli_cmd wpa_cli_commands[] = {
 	{ "p2p_ext_listen", wpa_cli_cmd_p2p_ext_listen, cli_cmd_flag_none,
 	  "[<period> <interval>] = set extended listen timing" },
 #endif /* CONFIG_P2P */
-
+#ifdef CONFIG_WIFI_DISPLAY
+	{ "wfd_subelem_set", wpa_cli_cmd_wfd_subelem_set, cli_cmd_flag_none,
+	  "<subelem> [contents] = set Wi-Fi Display subelement" },
+	{ "wfd_subelem_get", wpa_cli_cmd_wfd_subelem_get, cli_cmd_flag_none,
+	  "<subelem> = get Wi-Fi Display subelement" },
+#endif /* CONFIG_WIFI_DISPLAY */
 #ifdef CONFIG_INTERWORKING
 	{ "fetch_anqp", wpa_cli_cmd_fetch_anqp, cli_cmd_flag_none,
 	  "= fetch ANQP information for all APs" },
diff --git a/wpa_supplicant/wpa_supplicant.c b/wpa_supplicant/wpa_supplicant.c
index 39630ad..a924d91 100644
--- a/wpa_supplicant/wpa_supplicant.c
+++ b/wpa_supplicant/wpa_supplicant.c
@@ -40,6 +40,7 @@
 #include "gas_query.h"
 #include "ap.h"
 #include "p2p_supplicant.h"
+#include "wifi_display.h"
 #include "notify.h"
 #include "bgscan.h"
 #include "autoscan.h"
@@ -3183,6 +3184,14 @@ struct wpa_global * wpa_supplicant_init(struct wpa_params *params)
 		return NULL;
 	}
 
+#ifdef CONFIG_WIFI_DISPLAY
+	if (wifi_display_init(global) < 0) {
+		wpa_printf(MSG_ERROR, "Failed to initialize Wi-Fi Display");
+		wpa_supplicant_deinit(global);
+		return NULL;
+	}
+#endif /* CONFIG_WIFI_DISPLAY */
+
 	return global;
 }
 
@@ -3234,6 +3243,9 @@ void wpa_supplicant_deinit(struct wpa_global *global)
 	if (global == NULL)
 		return;
 
+#ifdef CONFIG_WIFI_DISPLAY
+	wifi_display_deinit(global);
+#endif /* CONFIG_WIFI_DISPLAY */
 #ifdef CONFIG_P2P
 	wpas_p2p_deinit_global(global);
 #endif /* CONFIG_P2P */
diff --git a/wpa_supplicant/wpa_supplicant_i.h b/wpa_supplicant/wpa_supplicant_i.h
index 74faada..5fa211d 100644
--- a/wpa_supplicant/wpa_supplicant_i.h
+++ b/wpa_supplicant/wpa_supplicant_i.h
@@ -239,6 +239,12 @@ struct wpa_global {
 		WPA_CONC_PREF_STA,
 		WPA_CONC_PREF_P2P
 	} conc_pref;
+
+#ifdef CONFIG_WIFI_DISPLAY
+	int wifi_display;
+#define MAX_WFD_SUBELEMS 10
+	struct wpabuf *wfd_subelem[MAX_WFD_SUBELEMS];
+#endif /* CONFIG_WIFI_DISPLAY */
 };
 
 
-- 
cgit v1.1