WFD: Add support for sending Wi-Fi Display service discovery requests

wpa_cli p2p_serv_disc_req command can now be used to request WSD
request to be sent to specified or all peers who support WSD.

format: wifi-display <list of roles> <list of subelements>
examples:
p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [source] 2,3,4,5
p2p_serv_disc_req 02:01:02:03:04:05 wifi-display [pri-sink] 3
p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [sec-source] 2
p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [source+sink] 2,3,4,5
p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [source][pri-sink] 2,3,4,5

Change-Id: I14f8a92aa9a2161d7db97f5670d2dfe3b63ef1e3
Signed-hostap: Jouni Malinen <jouni@qca.qualcomm.com>
Signed-off-by: Vishal Mahaveer <vishalm@ti.com>

3 files changed, 59 insertions, 0 deletions

diff --git a/src/p2p/p2p.h b/src/p2p/p2p.h
index ff15aee..b5d3ef9 100644
--- a/src/p2p/p2p.h
+++ b/src/p2p/p2p.h
@@ -963,6 +963,11 @@ int p2p_prov_disc_req(struct p2p_data *p2p, const u8 *peer_addr,
 void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
 		      const struct wpabuf *tlvs);
 
+#ifdef CONFIG_WIFI_DISPLAY
+void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
+			  const struct wpabuf *tlvs);
+#endif /* CONFIG_WIFI_DISPLAY */
+
 /**
  * p2p_sd_cancel_request - Cancel a pending service discovery query
  * @p2p: P2P module context from p2p_init()
diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h
index 1a15325..602fe18 100644
--- a/src/p2p/p2p_i.h
+++ b/src/p2p/p2p_i.h
@@ -109,6 +109,7 @@ struct p2p_sd_query {
 	struct p2p_sd_query *next;
 	u8 peer[ETH_ALEN];
 	int for_all_peers;
+	int wsd; /* Wi-Fi Display Service Discovery Request */
 	struct wpabuf *tlvs;
 };
 
diff --git a/src/p2p/p2p_sd.c b/src/p2p/p2p_sd.c
index 4b027fa..3e49fdc 100644
--- a/src/p2p/p2p_sd.c
+++ b/src/p2p/p2p_sd.c
@@ -15,15 +15,55 @@
 #include "p2p.h"
 
 
+#ifdef CONFIG_WIFI_DISPLAY
+static int wfd_wsd_supported(struct wpabuf *wfd)
+{
+	const u8 *pos, *end;
+	u8 subelem;
+	u16 len;
+
+	if (wfd == NULL)
+		return 0;
+
+	pos = wpabuf_head(wfd);
+	end = pos + wpabuf_len(wfd);
+
+	while (pos + 3 <= end) {
+		subelem = *pos++;
+		len = WPA_GET_BE16(pos);
+		pos += 2;
+		if (pos + len > end)
+			break;
+
+		if (subelem == WFD_SUBELEM_DEVICE_INFO && len >= 6) {
+			u16 info = WPA_GET_BE16(pos);
+			return !!(info & 0x0040);
+		}
+
+		pos += len;
+	}
+
+	return 0;
+}
+#endif /* CONFIG_WIFI_DISPLAY */
+
 struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p,
 					 struct p2p_device *dev)
 {
 	struct p2p_sd_query *q;
+	int wsd = 0;
 
 	if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY))
 		return NULL; /* peer does not support SD */
+#ifdef CONFIG_WIFI_DISPLAY
+	if (wfd_wsd_supported(dev->info.wfd_subelems))
+		wsd = 1;
+#endif /* CONFIG_WIFI_DISPLAY */
 
 	for (q = p2p->sd_queries; q; q = q->next) {
+		/* Use WSD only if the peer indicates support or it */
+		if (q->wsd && !wsd)
+			continue;
 		if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO))
 			return q;
 		if (!q->for_all_peers &&
@@ -889,6 +929,19 @@ void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
 }
 
 
+#ifdef CONFIG_WIFI_DISPLAY
+void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
+			  const struct wpabuf *tlvs)
+{
+	struct p2p_sd_query *q;
+	q = p2p_sd_request(p2p, dst, tlvs);
+	if (q)
+		q->wsd = 1;
+	return q;
+}
+#endif /* CONFIG_WIFI_DISPLAY */
+
+
 void p2p_sd_service_update(struct p2p_data *p2p)
 {
 	p2p->srv_update_indic++;