diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c index 0fb41f88e..59ab71af2 100644 --- a/src/p2p/p2p.c +++ b/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); } diff --git a/src/p2p/p2p.h b/src/p2p/p2p.h index 9d76032de..1ea13dae6 100644 --- a/src/p2p/p2p.h +++ b/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 */ diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h index f686c88c6..dcaae07aa 100644 --- a/src/p2p/p2p_i.h +++ b/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; diff --git a/wpa_supplicant/ctrl_iface.c b/wpa_supplicant/ctrl_iface.c index 80481938c..461de7e0e 100644 --- a/wpa_supplicant/ctrl_iface.c +++ b/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; diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c index 4ae4956bc..4325a12ae 100644 --- a/wpa_supplicant/p2p_supplicant.c +++ b/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) { diff --git a/wpa_supplicant/p2p_supplicant.h b/wpa_supplicant/p2p_supplicant.h index c8acda135..eea92d218 100644 --- a/wpa_supplicant/p2p_supplicant.h +++ b/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, diff --git a/wpa_supplicant/wpa_cli.c b/wpa_supplicant/wpa_cli.c index 911effe4a..6fa1489a8 100644 --- a/wpa_supplicant/wpa_cli.c +++ b/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, - " = add a local " + " = add a local " "service" }, + { "p2p_service_rep", wpa_cli_cmd_p2p_service_rep, NULL, + cli_cmd_flag_none, + "asp [] = replace " + "local ASP service" }, { "p2p_service_del", wpa_cli_cmd_p2p_service_del, NULL, cli_cmd_flag_none, " [|service] = remove a local "