P2PS: Extend add/del services logic to support ASP

In addition, add a new P2P_SERVICE_REP command that can be used to
replace existing ASP advertisements.

Signed-off-by: Jouni Malinen <jouni@qca.qualcomm.com>

src/p2p/p2p.c

@@ -2475,6 +2475,132 @@ int p2p_scan_result_text(const u8 *ies, size_t ies_len, char *buf, char *end)
 }
 
 
+struct p2ps_advertisement *
+p2p_service_p2ps_id(struct p2p_data *p2p, u32 adv_id)
+{
+	struct p2ps_advertisement *adv_data;
+
+	if (!p2p)
+		return NULL;
+
+	adv_data = p2p->p2ps_adv_list;
+	while (adv_data) {
+		if (adv_data->id == adv_id)
+			return adv_data;
+		adv_data = adv_data->next;
+	}
+
+	return NULL;
+}
+
+
+int p2p_service_del_asp(struct p2p_data *p2p, u32 adv_id)
+{
+	struct p2ps_advertisement *adv_data;
+	struct p2ps_advertisement **prior;
+
+	if (!p2p)
+		return -1;
+
+	adv_data = p2p->p2ps_adv_list;
+	prior = &p2p->p2ps_adv_list;
+	while (adv_data) {
+		if (adv_data->id == adv_id) {
+			p2p_dbg(p2p, "Delete ASP adv_id=0x%x", adv_id);
+			*prior = adv_data->next;
+			os_free(adv_data);
+			return 0;
+		}
+		prior = &adv_data->next;
+		adv_data = adv_data->next;
+	}
+
+	return -1;
+}
+
+
+int p2p_service_add_asp(struct p2p_data *p2p, int auto_accept, u32 adv_id,
+			const char *adv_str, u8 svc_state, u16 config_methods,
+			const char *svc_info)
+{
+	struct p2ps_advertisement *adv_data, *tmp, **prev;
+	u8 buf[P2PS_HASH_LEN];
+	size_t adv_data_len, adv_len, info_len = 0;
+
+	if (!p2p || !adv_str || !adv_str[0])
+		return -1;
+
+	if (!(config_methods & p2p->cfg->config_methods)) {
+		p2p_dbg(p2p, "Config methods not supported svc: 0x%x dev: 0x%x",
+			config_methods, p2p->cfg->config_methods);
+		return -1;
+	}
+
+	if (!p2ps_gen_hash(p2p, adv_str, buf))
+		return -1;
+
+	if (svc_info)
+		info_len = os_strlen(svc_info);
+	adv_len = os_strlen(adv_str);
+	adv_data_len = sizeof(struct p2ps_advertisement) + adv_len + 1 +
+		info_len + 1;
+
+	adv_data = os_zalloc(adv_data_len);
+	if (!adv_data)
+		return -1;
+
+	os_memcpy(adv_data->hash, buf, P2PS_HASH_LEN);
+	adv_data->id = adv_id;
+	adv_data->state = svc_state;
+	adv_data->config_methods = config_methods & p2p->cfg->config_methods;
+	adv_data->auto_accept = (u8) auto_accept;
+	os_memcpy(adv_data->svc_name, adv_str, adv_len);
+
+	if (svc_info && info_len) {
+		adv_data->svc_info = &adv_data->svc_name[adv_len + 1];
+		os_memcpy(adv_data->svc_info, svc_info, info_len);
+	}
+
+	/*
+	 * Group Advertisements by service string. They do not need to be
+	 * sorted, but groups allow easier Probe Response instance grouping
+	 */
+	tmp = p2p->p2ps_adv_list;
+	prev = &p2p->p2ps_adv_list;
+	while (tmp) {
+		if (tmp->id == adv_data->id) {
+			if (os_strcmp(tmp->svc_name, adv_data->svc_name) != 0) {
+				os_free(adv_data);
+				return -1;
+			}
+			adv_data->next = tmp->next;
+			*prev = adv_data;
+			os_free(tmp);
+			goto inserted;
+		} else {
+			if (os_strcmp(tmp->svc_name, adv_data->svc_name) == 0) {
+				adv_data->next = tmp->next;
+				tmp->next = adv_data;
+				goto inserted;
+			}
+		}
+		prev = &tmp->next;
+		tmp = tmp->next;
+	}
+
+	/* No svc_name match found */
+	adv_data->next = p2p->p2ps_adv_list;
+	p2p->p2ps_adv_list = adv_data;
+
+inserted:
+	p2p_dbg(p2p,
+		"Added ASP advertisement adv_id=0x%x config_methods=0x%x svc_state=0x%x adv_str='%s'",
+		adv_id, adv_data->config_methods, svc_state, adv_str);
+
+	return 0;
+}
+
+
 int p2p_parse_dev_addr_in_p2p_ie(struct wpabuf *p2p_ie, u8 *dev_addr)
 {
 	struct p2p_message msg;
@@ -2623,6 +2749,8 @@ struct p2p_data * p2p_init(const struct p2p_config *cfg)
 
 void p2p_deinit(struct p2p_data *p2p)
 {
+	struct p2ps_advertisement *adv, *prev;
+
 #ifdef CONFIG_WIFI_DISPLAY
 	wpabuf_free(p2p->wfd_ie_beacon);
 	wpabuf_free(p2p->wfd_ie_probe_req);
@@ -2655,6 +2783,14 @@ void p2p_deinit(struct p2p_data *p2p)
 	os_free(p2p->after_scan_tx);
 	p2p_remove_wps_vendor_extensions(p2p);
 	os_free(p2p->no_go_freq.range);
+
+	adv = p2p->p2ps_adv_list;
+	while (adv) {
+		prev = adv;
+		adv = adv->next;
+		os_free(prev);
+	}
+
 	os_free(p2p);
 }
 

src/p2p/p2p.h

@@ -11,6 +11,12 @@
 
 #include "wps/wps_defs.h"
 
+/* P2P ASP Setup Capability */
+#define P2PS_SETUP_NONE 0
+#define P2PS_SETUP_NEW BIT(0)
+#define P2PS_SETUP_CLIENT BIT(1)
+#define P2PS_SETUP_GROUP_OWNER BIT(2)
+
 #define P2PS_WILD_HASH_STR "org.wi-fi.wfds"
 #define P2PS_HASH_LEN 6
 #define P2P_MAX_QUERY_HASH 6
@@ -147,6 +153,46 @@ struct p2p_go_neg_results {
 	unsigned int peer_config_timeout;
 };
 
+struct p2ps_advertisement {
+	struct p2ps_advertisement *next;
+
+	/**
+	 * svc_info - Pointer to (internal) Service defined information
+	 */
+	char *svc_info;
+
+	/**
+	 * id - P2PS Advertisement ID
+	 */
+	u32 id;
+
+	/**
+	 * config_methods - WPS Methods which are allowed for this service
+	 */
+	u16 config_methods;
+
+	/**
+	 * state - Current state of the service: 0 - Out Of Service, 1-255 Vendor defined
+	 */
+	u8 state;
+
+	/**
+	 * auto_accept - Automatically Accept provisioning request if possible.
+	 */
+	u8 auto_accept;
+
+	/**
+	 * hash - 6 octet Service Name has to match against incoming Probe Requests
+	 */
+	u8 hash[P2PS_HASH_LEN];
+
+	/**
+	 * svc_name - NULL Terminated UTF-8 Service Name, and svc_info storage
+	 */
+	char svc_name[0];
+};
+
+
 struct p2p_data;
 
 enum p2p_scan_type {
@@ -2038,4 +2084,11 @@ void p2p_loop_on_known_peers(struct p2p_data *p2p,
 
 void p2p_set_vendor_elems(struct p2p_data *p2p, struct wpabuf **vendor_elem);
 
+struct p2ps_advertisement *
+p2p_service_p2ps_id(struct p2p_data *p2p, u32 adv_id);
+int p2p_service_add_asp(struct p2p_data *p2p, int auto_accept, u32 adv_id,
+			const char *adv_str, u8 svc_state,
+			u16 config_methods, const char *svc_info);
+int p2p_service_del_asp(struct p2p_data *p2p, u32 adv_id);
+
 #endif /* P2P_H */

src/p2p/p2p_i.h

@@ -493,6 +493,7 @@ struct p2p_data {
 	u8 pending_channel_forced;
 
 	/* ASP Support */
+	struct p2ps_advertisement *p2ps_adv_list;
 	u8 wild_card_hash[P2PS_HASH_LEN];
 	u8 query_hash[P2P_MAX_QUERY_HASH * P2PS_HASH_LEN];
 	u8 p2ps_seek;

wpa_supplicant/ctrl_iface.c

@@ -4914,6 +4914,106 @@ static int p2p_ctrl_service_add_upnp(struct wpa_supplicant *wpa_s, char *cmd)
 }
 
 
+static int p2p_ctrl_service_add_asp(struct wpa_supplicant *wpa_s,
+				    u8 replace, char *cmd)
+{
+	char *pos;
+	char *adv_str;
+	u32 auto_accept, adv_id, svc_state, config_methods;
+	char *svc_info = NULL;
+
+	pos = os_strchr(cmd, ' ');
+	if (pos == NULL)
+		return -1;
+	*pos++ = '\0';
+
+	/* Auto-Accept value is mandatory, and must be one of the
+	 * single values (0, 1, 2, 4) */
+	auto_accept = atoi(cmd);
+	switch (auto_accept) {
+	case P2PS_SETUP_NONE: /* No auto-accept */
+	case P2PS_SETUP_NEW:
+	case P2PS_SETUP_CLIENT:
+	case P2PS_SETUP_GROUP_OWNER:
+		break;
+	default:
+		return -1;
+	}
+
+	/* Advertisement ID is mandatory */
+	cmd = pos;
+	pos = os_strchr(cmd, ' ');
+	if (pos == NULL)
+		return -1;
+	*pos++ = '\0';
+
+	/* Handle Adv_ID == 0 (wildcard "org.wi-fi.wfds") internally. */
+	if (sscanf(cmd, "%x", &adv_id) != 1 || adv_id == 0)
+		return -1;
+
+	/* Only allow replacements if exist, and adds if not */
+	if (wpas_p2p_service_p2ps_id_exists(wpa_s, adv_id)) {
+		if (!replace)
+			return -1;
+	} else {
+		if (replace)
+			return -1;
+	}
+
+	/* svc_state between 0 - 0xff is mandatory */
+	if (sscanf(pos, "%x", &svc_state) != 1 || svc_state > 0xff)
+		return -1;
+
+	pos = os_strchr(pos, ' ');
+	if (pos == NULL)
+		return -1;
+
+	/* config_methods is mandatory */
+	pos++;
+	if (sscanf(pos, "%x", &config_methods) != 1)
+		return -1;
+
+	if (!(config_methods &
+	      (WPS_CONFIG_DISPLAY | WPS_CONFIG_KEYPAD | WPS_CONFIG_P2PS)))
+		return -1;
+
+	pos = os_strchr(pos, ' ');
+	if (pos == NULL)
+		return -1;
+
+	pos++;
+	adv_str = pos;
+
+	/* Advertisement string is mandatory */
+	if (!pos[0] || pos[0] == ' ')
+		return -1;
+
+	/* Terminate svc string */
+	pos = os_strchr(pos, ' ');
+	if (pos != NULL)
+		*pos++ = '\0';
+
+	/* Service and Response Information are optional */
+	if (pos && pos[0]) {
+		size_t len;
+
+		/* Note the bare ' included, which cannot exist legally
+		 * in unescaped string. */
+		svc_info = os_strstr(pos, "svc_info='");
+
+		if (svc_info) {
+			svc_info += 9;
+			len = os_strlen(svc_info);
+			utf8_unescape(svc_info, len, svc_info, len);
+		}
+	}
+
+	return wpas_p2p_service_add_asp(wpa_s, auto_accept, adv_id, adv_str,
+					(u8) svc_state, (u16) config_methods,
+					svc_info);
+}
+
+
 static int p2p_ctrl_service_add(struct wpa_supplicant *wpa_s, char *cmd)
 {
 	char *pos;
@@ -4927,6 +5027,8 @@ static int p2p_ctrl_service_add(struct wpa_supplicant *wpa_s, char *cmd)
 		return p2p_ctrl_service_add_bonjour(wpa_s, pos);
 	if (os_strcmp(cmd, "upnp") == 0)
 		return p2p_ctrl_service_add_upnp(wpa_s, pos);
+	if (os_strcmp(cmd, "asp") == 0)
+		return p2p_ctrl_service_add_asp(wpa_s, 0, pos);
 	wpa_printf(MSG_DEBUG, "Unknown service '%s'", cmd);
 	return -1;
 }
@@ -4974,6 +5076,17 @@ static int p2p_ctrl_service_del_upnp(struct wpa_supplicant *wpa_s, char *cmd)
 }
 
 
+static int p2p_ctrl_service_del_asp(struct wpa_supplicant *wpa_s, char *cmd)
+{
+	u32 adv_id;
+
+	if (sscanf(cmd, "%x", &adv_id) != 1)
+		return -1;
+
+	return wpas_p2p_service_del_asp(wpa_s, adv_id);
+}
+
+
 static int p2p_ctrl_service_del(struct wpa_supplicant *wpa_s, char *cmd)
 {
 	char *pos;
@@ -4987,6 +5100,25 @@ static int p2p_ctrl_service_del(struct wpa_supplicant *wpa_s, char *cmd)
 		return p2p_ctrl_service_del_bonjour(wpa_s, pos);
 	if (os_strcmp(cmd, "upnp") == 0)
 		return p2p_ctrl_service_del_upnp(wpa_s, pos);
+	if (os_strcmp(cmd, "asp") == 0)
+		return p2p_ctrl_service_del_asp(wpa_s, pos);
+	wpa_printf(MSG_DEBUG, "Unknown service '%s'", cmd);
+	return -1;
+}
+
+
+static int p2p_ctrl_service_replace(struct wpa_supplicant *wpa_s, char *cmd)
+{
+	char *pos;
+
+	pos = os_strchr(cmd, ' ');
+	if (pos == NULL)
+		return -1;
+	*pos++ = '\0';
+
+	if (os_strcmp(cmd, "asp") == 0)
+		return p2p_ctrl_service_add_asp(wpa_s, 1, pos);
+
 	wpa_printf(MSG_DEBUG, "Unknown service '%s'", cmd);
 	return -1;
 }
@@ -7735,6 +7867,9 @@ char * wpa_supplicant_ctrl_iface_process(struct wpa_supplicant *wpa_s,
 	} else if (os_strncmp(buf, "P2P_SERVICE_DEL ", 16) == 0) {
 		if (p2p_ctrl_service_del(wpa_s, buf + 16) < 0)
 			reply_len = -1;
+	} else if (os_strncmp(buf, "P2P_SERVICE_REP ", 16) == 0) {
+		if (p2p_ctrl_service_replace(wpa_s, buf + 16) < 0)
+			reply_len = -1;
 	} else if (os_strncmp(buf, "P2P_REJECT ", 11) == 0) {
 		if (p2p_ctrl_reject(wpa_s, buf + 11) < 0)
 			reply_len = -1;

wpa_supplicant/p2p_supplicant.c

@@ -2868,6 +2868,35 @@ void wpas_p2p_service_flush(struct wpa_supplicant *wpa_s)
 }
 
 
+int wpas_p2p_service_p2ps_id_exists(struct wpa_supplicant *wpa_s, u32 adv_id)
+{
+	if (adv_id == 0)
+		return 1;
+
+	if (p2p_service_p2ps_id(wpa_s->global->p2p, adv_id))
+		return 1;
+
+	return 0;
+}
+
+
+int wpas_p2p_service_del_asp(struct wpa_supplicant *wpa_s, u32 adv_id)
+{
+	return p2p_service_del_asp(wpa_s->global->p2p, adv_id);
+}
+
+
+int wpas_p2p_service_add_asp(struct wpa_supplicant *wpa_s,
+			     int auto_accept, u32 adv_id,
+			     const char *adv_str, u8 svc_state,
+			     u16 config_methods, const char *svc_info)
+{
+	return p2p_service_add_asp(wpa_s->global->p2p, auto_accept, adv_id,
+				   adv_str, svc_state, config_methods,
+				   svc_info);
+}
+
+
 int wpas_p2p_service_add_bonjour(struct wpa_supplicant *wpa_s,
 				 struct wpabuf *query, struct wpabuf *resp)
 {

wpa_supplicant/p2p_supplicant.h

@@ -84,6 +84,11 @@ int wpas_p2p_service_add_upnp(struct wpa_supplicant *wpa_s, u8 version,
 			      const char *service);
 int wpas_p2p_service_del_upnp(struct wpa_supplicant *wpa_s, u8 version,
 			      const char *service);
+int wpas_p2p_service_add_asp(struct wpa_supplicant *wpa_s, int auto_accept,
+			     u32 adv_id, const char *adv_str, u8 svc_state,
+			     u16 config_methods, const char *svc_info);
+int wpas_p2p_service_del_asp(struct wpa_supplicant *wpa_s, u32 adv_id);
+int wpas_p2p_service_p2ps_id_exists(struct wpa_supplicant *wpa_s, u32 adv_id);
 int wpas_p2p_reject(struct wpa_supplicant *wpa_s, const u8 *addr);
 int wpas_p2p_invite(struct wpa_supplicant *wpa_s, const u8 *peer_addr,
 		    struct wpa_ssid *ssid, const u8 *go_dev_addr, int freq,

wpa_supplicant/wpa_cli.c

@@ -1975,27 +1975,25 @@ static int wpa_cli_cmd_p2p_service_flush(struct wpa_ctrl *ctrl, int argc,
 static int wpa_cli_cmd_p2p_service_add(struct wpa_ctrl *ctrl, int argc,
 				       char *argv[])
 {
-	char cmd[4096];
-	int res;
+	if (argc < 3) {
+		printf("Invalid P2P_SERVICE_ADD command: needs 3-6 arguments\n");
+		return -1;
+	}
 
-	if (argc != 3 && argc != 4) {
-		printf("Invalid P2P_SERVICE_ADD command: needs three or four "
+	return wpa_cli_cmd(ctrl, "P2P_SERVICE_ADD", 3, argc, argv);
+}
+
+
+static int wpa_cli_cmd_p2p_service_rep(struct wpa_ctrl *ctrl, int argc,
+				       char *argv[])
+{
+	if (argc < 5 || argc > 6) {
+		printf("Invalid P2P_SERVICE_REP command: needs 5-6 "
 		       "arguments\n");
 		return -1;
 	}
 
-	if (argc == 4)
-		res = os_snprintf(cmd, sizeof(cmd),
-				  "P2P_SERVICE_ADD %s %s %s %s",
-				  argv[0], argv[1], argv[2], argv[3]);
-	else
-		res = os_snprintf(cmd, sizeof(cmd),
-				  "P2P_SERVICE_ADD %s %s %s",
-				  argv[0], argv[1], argv[2]);
-	if (os_snprintf_error(sizeof(cmd), res))
-		return -1;
-	cmd[sizeof(cmd) - 1] = '\0';
-	return wpa_ctrl_command(ctrl, cmd);
+	return wpa_cli_cmd(ctrl, "P2P_SERVICE_REP", 5, argc, argv);
 }
 
 
@@ -2918,8 +2916,12 @@ static struct wpa_cli_cmd wpa_cli_commands[] = {
 	  "= remove all stored service entries" },
 	{ "p2p_service_add", wpa_cli_cmd_p2p_service_add, NULL,
 	  cli_cmd_flag_none,
-	  "<bonjour|upnp> <query|version> <response|service> = add a local "
+	  "<bonjour|upnp|asp> <query|version> <response|service> = add a local "
 	  "service" },
+	{ "p2p_service_rep", wpa_cli_cmd_p2p_service_rep, NULL,
+	  cli_cmd_flag_none,
+	  "asp <auto> <adv_id> <svc_state> <svc_string> [<svc_info>] = replace "
+	  "local ASP service" },
 	{ "p2p_service_del", wpa_cli_cmd_p2p_service_del, NULL,
 	  cli_cmd_flag_none,
 	  "<bonjour|upnp> <query|version> [|service] = remove a local "