From 5a4102ce0e7221eb8e59ec12365fa395f99b651e Mon Sep 17 00:00:00 2001
From: Krishna Vamsi <vamsin@qti.qualcomm.com>
Date: Tue, 9 Dec 2014 20:42:27 +0530
Subject: [PATCH] P2PS: Add support to send ASP service requests

This extends the P2P_SERV_DISC_REQ control interface command with a new
"asp" service type. This takes service id, service string, and optional
service info as arguments.

Signed-off-by: Jouni Malinen <jouni@qca.qualcomm.com>
---
 wpa_supplicant/ctrl_iface.c     | 34 +++++++++++++++++++++++++++++++++
 wpa_supplicant/p2p_supplicant.c | 33 ++++++++++++++++++++++++++++++++
 wpa_supplicant/p2p_supplicant.h |  2 ++
 wpa_supplicant/wpa_cli.c        |  6 ++----
 4 files changed, 71 insertions(+), 4 deletions(-)

diff --git a/wpa_supplicant/ctrl_iface.c b/wpa_supplicant/ctrl_iface.c
index 461de7e0e..2ee85efb4 100644
--- a/wpa_supplicant/ctrl_iface.c
+++ b/wpa_supplicant/ctrl_iface.c
@@ -4752,6 +4752,40 @@ static int p2p_ctrl_serv_disc_req(struct wpa_supplicant *wpa_s, char *cmd,
 	} 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 if (os_strncmp(pos, "asp ", 4) == 0) {
+		char *svc_str;
+		char *svc_info = NULL;
+		u32 id;
+
+		pos += 4;
+		if (sscanf(pos, "%x", &id) != 1 || id > 0xff)
+			return -1;
+
+		pos = os_strchr(pos, ' ');
+		if (pos == NULL || pos[1] == '\0' || pos[1] == ' ')
+			return -1;
+
+		svc_str = pos + 1;
+
+		pos = os_strchr(svc_str, ' ');
+
+		if (pos)
+			*pos++ = '\0';
+
+		/* All remaining data is the svc_info string */
+		if (pos && pos[0] && pos[0] != ' ') {
+			len = os_strlen(pos);
+
+			/* Unescape in place */
+			len = utf8_unescape(pos, len, pos, len);
+			if (len > 0xff)
+				return -1;
+
+			svc_info = pos;
+		}
+
+		ref = wpas_p2p_sd_request_asp(wpa_s, dst, (u8) id,
+					      svc_str, svc_info);
 	} else {
 		len = os_strlen(pos);
 		if (len & 1)
diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c
index bdc287394..f980c94d9 100644
--- a/wpa_supplicant/p2p_supplicant.c
+++ b/wpa_supplicant/p2p_supplicant.c
@@ -2774,6 +2774,39 @@ u64 wpas_p2p_sd_request_upnp(struct wpa_supplicant *wpa_s, const u8 *dst,
 }
 
 
+u64 wpas_p2p_sd_request_asp(struct wpa_supplicant *wpa_s, const u8 *dst, u8 id,
+			    const char *svc_str, const char *info_substr)
+{
+	struct wpabuf *tlvs;
+	size_t plen, svc_len, substr_len = 0;
+	u64 ret;
+
+	svc_len = os_strlen(svc_str);
+	if (info_substr)
+		substr_len = os_strlen(info_substr);
+
+	if (svc_len > 0xff || substr_len > 0xff)
+		return 0;
+
+	plen = 1 + 1 + 1 + svc_len + 1 + substr_len;
+	tlvs = wpabuf_alloc(2 + plen);
+	if (tlvs == NULL)
+		return 0;
+
+	wpabuf_put_le16(tlvs, plen);
+	wpabuf_put_u8(tlvs, P2P_SERV_P2PS);
+	wpabuf_put_u8(tlvs, id); /* Service Transaction ID */
+	wpabuf_put_u8(tlvs, (u8) svc_len); /* Service String Length */
+	wpabuf_put_data(tlvs, svc_str, svc_len);
+	wpabuf_put_u8(tlvs, (u8) substr_len); /* Info Substring Length */
+	wpabuf_put_data(tlvs, info_substr, substr_len);
+	ret = wpas_p2p_sd_request(wpa_s, dst, tlvs);
+	wpabuf_free(tlvs);
+
+	return ret;
+}
+
+
 #ifdef CONFIG_WIFI_DISPLAY
 
 static u64 wpas_p2p_sd_request_wfd(struct wpa_supplicant *wpa_s, const u8 *dst,
diff --git a/wpa_supplicant/p2p_supplicant.h b/wpa_supplicant/p2p_supplicant.h
index eea92d218..fad7f6fd0 100644
--- a/wpa_supplicant/p2p_supplicant.h
+++ b/wpa_supplicant/p2p_supplicant.h
@@ -66,6 +66,8 @@ void wpas_p2p_scan_ie(struct wpa_supplicant *wpa_s, struct wpabuf *ies);
 void wpas_p2p_group_formation_failed(struct wpa_supplicant *wpa_s);
 u64 wpas_p2p_sd_request(struct wpa_supplicant *wpa_s, const u8 *dst,
 			const struct wpabuf *tlvs);
+u64 wpas_p2p_sd_request_asp(struct wpa_supplicant *wpa_s, const u8 *dst, u8 id,
+			    const char *svc_str, const char *info_substr);
 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,
diff --git a/wpa_supplicant/wpa_cli.c b/wpa_supplicant/wpa_cli.c
index 6fa1489a8..fdadffa3b 100644
--- a/wpa_supplicant/wpa_cli.c
+++ b/wpa_supplicant/wpa_cli.c
@@ -1909,11 +1909,9 @@ static int wpa_cli_cmd_p2p_serv_disc_req(struct wpa_ctrl *ctrl, int argc,
 {
 	char cmd[4096];
 
-	if (argc != 2 && argc != 4) {
+	if (argc < 2) {
 		printf("Invalid P2P_SERV_DISC_REQ command: needs two "
-		       "arguments (address and TLVs) or four arguments "
-		       "(address, \"upnp\", version, search target "
-		       "(SSDP ST:)\n");
+		       "or more arguments (address and TLVs)\n");
 		return -1;
 	}