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>

4 files changed, 96 insertions, 0 deletions

diff --git a/wpa_supplicant/README-P2P b/wpa_supplicant/README-P2P
index bb4c2ad..c95009d 100644
--- a/wpa_supplicant/README-P2P
+++ b/wpa_supplicant/README-P2P
@@ -250,6 +250,14 @@ p2p_serv_disc_req 00:00:00:00:00:00 upnp 10 urn:schemas-upnp-org:service:Content
 p2p_serv_disc_req 00:00:00:00:00:00 upnp 10 uuid:6859dede-8574-59ab-9332-123456789012
 p2p_serv_disc_req 00:00:00:00:00:00 upnp 10 urn:schemas-upnp-org:device:InternetGatewayDevice:1
 
+# Wi-Fi Display examples
+# format: wifi-display <list of roles> <list of subelements>
+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
+
 p2p_serv_disc_cancel_req <query identifier>
 
 Cancel a pending P2P service discovery request. This command takes a
diff --git a/wpa_supplicant/ctrl_iface.c b/wpa_supplicant/ctrl_iface.c
index 110b610..f26bd64 100644
--- a/wpa_supplicant/ctrl_iface.c
+++ b/wpa_supplicant/ctrl_iface.c
@@ -3295,6 +3295,10 @@ static int p2p_ctrl_serv_disc_req(struct wpa_supplicant *wpa_s, char *cmd,
 			return -1;
 		pos++;
 		ref = wpas_p2p_sd_request_upnp(wpa_s, dst, version, pos);
+#ifdef CONFIG_WIFI_DISPLAY
+	} else if (os_strncmp(pos, "wifi-display ", 13) == 0) {
+		ref = wpas_p2p_sd_request_wifi_display(wpa_s, dst, pos + 13);
+#endif /* CONFIG_WIFI_DISPLAY */
 	} else {
 		len = os_strlen(pos);
 		if (len & 1)
diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c
index 42c062f..82e2192 100644
--- a/wpa_supplicant/p2p_supplicant.c
+++ b/wpa_supplicant/p2p_supplicant.c
@@ -1659,6 +1659,88 @@ u64 wpas_p2p_sd_request_upnp(struct wpa_supplicant *wpa_s, const u8 *dst,
 }
 
 
+#ifdef CONFIG_WIFI_DISPLAY
+
+static u64 wpas_p2p_sd_request_wfd(struct wpa_supplicant *wpa_s, const u8 *dst,
+				   const struct wpabuf *tlvs)
+{
+	if (wpa_s->drv_flags & WPA_DRIVER_FLAGS_P2P_MGMT)
+		return 0;
+	if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL)
+		return 0;
+	return (uintptr_t) p2p_sd_request_wfd(wpa_s->global->p2p, dst, tlvs);
+}
+
+
+#define MAX_WFD_SD_SUBELEMS 20
+
+static void wfd_add_sd_req_role(struct wpabuf *tlvs, u8 id, u8 role,
+				const char *subelems)
+{
+	u8 *len;
+	const char *pos;
+	int val;
+	int count = 0;
+
+	len = wpabuf_put(tlvs, 2);
+	wpabuf_put_u8(tlvs, P2P_SERV_WIFI_DISPLAY); /* Service Protocol Type */
+	wpabuf_put_u8(tlvs, id); /* Service Transaction ID */
+
+	wpabuf_put_u8(tlvs, role);
+
+	pos = subelems;
+	while (*pos) {
+		val = atoi(pos);
+		if (val >= 0 && val < 256) {
+			wpabuf_put_u8(tlvs, val);
+			count++;
+			if (count == MAX_WFD_SD_SUBELEMS)
+				break;
+		}
+		pos = os_strchr(pos + 1, ',');
+		if (pos == NULL)
+			break;
+		pos++;
+	}
+
+	WPA_PUT_LE16(len, (u8 *) wpabuf_put(tlvs, 0) - len - 2);
+}
+
+
+u64 wpas_p2p_sd_request_wifi_display(struct wpa_supplicant *wpa_s,
+				     const u8 *dst, const char *role)
+{
+	struct wpabuf *tlvs;
+	u64 ret;
+	const char *subelems;
+	u8 id = 1;
+
+	subelems = os_strchr(role, ' ');
+	if (subelems == NULL)
+		return 0;
+	subelems++;
+
+	tlvs = wpabuf_alloc(4 * (2 + 1 + 1 + 1 + MAX_WFD_SD_SUBELEMS));
+	if (tlvs == NULL)
+		return 0;
+
+	if (os_strstr(role, "[source]"))
+		wfd_add_sd_req_role(tlvs, id++, 0x00, subelems);
+	if (os_strstr(role, "[pri-sink]"))
+		wfd_add_sd_req_role(tlvs, id++, 0x01, subelems);
+	if (os_strstr(role, "[sec-sink]"))
+		wfd_add_sd_req_role(tlvs, id++, 0x02, subelems);
+	if (os_strstr(role, "[source+sink]"))
+		wfd_add_sd_req_role(tlvs, id++, 0x03, subelems);
+
+	ret = wpas_p2p_sd_request_wfd(wpa_s, dst, tlvs);
+	wpabuf_free(tlvs);
+	return ret;
+}
+
+#endif /* CONFIG_WIFI_DISPLAY */
+
+
 int wpas_p2p_sd_cancel_request(struct wpa_supplicant *wpa_s, u64 req)
 {
 	if (wpa_s->drv_flags & WPA_DRIVER_FLAGS_P2P_MGMT)
diff --git a/wpa_supplicant/p2p_supplicant.h b/wpa_supplicant/p2p_supplicant.h
index 8d3bff1..5aa479d 100644
--- a/wpa_supplicant/p2p_supplicant.h
+++ b/wpa_supplicant/p2p_supplicant.h
@@ -90,6 +90,8 @@ u64 wpas_p2p_sd_request(struct wpa_supplicant *wpa_s, const u8 *dst,
 			const struct wpabuf *tlvs);
 u64 wpas_p2p_sd_request_upnp(struct wpa_supplicant *wpa_s, const u8 *dst,
 			     u8 version, const char *query);
+u64 wpas_p2p_sd_request_wifi_display(struct wpa_supplicant *wpa_s,
+				     const u8 *dst, const char *role);
 int wpas_p2p_sd_cancel_request(struct wpa_supplicant *wpa_s, u64 req);
 void wpas_p2p_sd_response(struct wpa_supplicant *wpa_s, int freq,
 			  const u8 *dst, u8 dialog_token,