diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c index 1e9a16e8a..f442c4881 100644 --- a/src/p2p/p2p.c +++ b/src/p2p/p2p.c @@ -13,6 +13,8 @@ #include "common/ieee802_11_defs.h" #include "common/ieee802_11_common.h" #include "common/wpa_ctrl.h" +#include "crypto/sha256.h" +#include "crypto/crypto.h" #include "wps/wps_i.h" #include "p2p_i.h" #include "p2p.h" @@ -1083,10 +1085,44 @@ static void p2p_free_req_dev_types(struct p2p_data *p2p) } +static int p2ps_gen_hash(struct p2p_data *p2p, const char *str, u8 *hash) +{ + u8 buf[SHA256_MAC_LEN]; + char str_buf[256]; + const u8 *adv_array; + size_t i, adv_len; + + if (!str || !hash) + return 0; + + if (!str[0]) { + os_memcpy(hash, p2p->wild_card_hash, P2PS_HASH_LEN); + return 1; + } + + adv_array = (u8 *) str_buf; + adv_len = os_strlen(str); + + for (i = 0; str[i] && i < adv_len; i++) { + if (str[i] >= 'A' && str[i] <= 'Z') + str_buf[i] = str[i] - 'A' + 'a'; + else + str_buf[i] = str[i]; + } + + if (sha256_vector(1, &adv_array, &adv_len, buf)) + return 0; + + os_memcpy(hash, buf, P2PS_HASH_LEN); + return 1; +} + + int p2p_find(struct p2p_data *p2p, unsigned int timeout, enum p2p_discovery_type type, unsigned int num_req_dev_types, const u8 *req_dev_types, - const u8 *dev_id, unsigned int search_delay) + const u8 *dev_id, unsigned int search_delay, + u8 seek_count, const char **seek) { int res; @@ -1113,6 +1149,47 @@ int p2p_find(struct p2p_data *p2p, unsigned int timeout, } else p2p->find_dev_id = NULL; + if (seek_count == 0 || !seek) { + /* Not an ASP search */ + p2p->p2ps_seek = 0; + } else if (seek_count == 1 && seek && (!seek[0] || !seek[0][0])) { + /* + * An empty seek string means no hash values, but still an ASP + * search. + */ + p2p->p2ps_seek_count = 0; + p2p->p2ps_seek = 1; + } else if (seek && seek_count <= P2P_MAX_QUERY_HASH) { + u8 buf[P2PS_HASH_LEN]; + int i; + + p2p->p2ps_seek_count = seek_count; + for (i = 0; i < seek_count; i++) { + if (!p2ps_gen_hash(p2p, seek[i], buf)) + continue; + + /* If asking for wildcard, don't do others */ + if (os_memcmp(buf, p2p->wild_card_hash, + P2PS_HASH_LEN) == 0) { + p2p->p2ps_seek_count = 0; + break; + } + + os_memcpy(&p2p->query_hash[i * P2PS_HASH_LEN], buf, + P2PS_HASH_LEN); + } + p2p->p2ps_seek = 1; + } else { + p2p->p2ps_seek_count = 0; + p2p->p2ps_seek = 1; + } + + /* Special case to perform wildcard search */ + if (p2p->p2ps_seek_count == 0 && p2p->p2ps_seek) { + p2p->p2ps_seek_count = 1; + os_memcpy(&p2p->query_hash, p2p->wild_card_hash, P2PS_HASH_LEN); + } + p2p->start_after_scan = P2P_AFTER_SCAN_NOTHING; p2p_clear_timeout(p2p); p2p->cfg->stop_listen(p2p->cfg->cb_ctx); @@ -1165,6 +1242,9 @@ void p2p_stop_find_for_freq(struct p2p_data *p2p, int freq) p2p_clear_timeout(p2p); if (p2p->state == P2P_SEARCH) p2p->cfg->find_stopped(p2p->cfg->cb_ctx); + + p2p->p2ps_seek_count = 0; + p2p_set_state(p2p, P2P_IDLE); p2p_free_req_dev_types(p2p); p2p->start_after_scan = P2P_AFTER_SCAN_NOTHING; @@ -2508,6 +2588,8 @@ struct p2p_data * p2p_init(const struct p2p_config *cfg) p2p->cfg->num_pref_chan = 0; } + p2ps_gen_hash(p2p, P2PS_WILD_HASH_STR, p2p->wild_card_hash); + p2p->min_disc_int = 1; p2p->max_disc_int = 3; p2p->max_disc_tu = -1; diff --git a/src/p2p/p2p.h b/src/p2p/p2p.h index 36e3fe73c..047d3fa6a 100644 --- a/src/p2p/p2p.h +++ b/src/p2p/p2p.h @@ -11,7 +11,9 @@ #include "wps/wps_defs.h" +#define P2PS_WILD_HASH_STR "org.wi-fi.wfds" #define P2PS_HASH_LEN 6 +#define P2P_MAX_QUERY_HASH 6 /** * P2P_MAX_REG_CLASSES - Maximum number of regulatory classes @@ -943,12 +945,15 @@ enum p2p_discovery_type { * requested device types. * @dev_id: Device ID to search for or %NULL to find all devices * @search_delay: Extra delay in milliseconds between search iterations + * @seek_count: Number of ASP Service Strings in the seek_string array + * @seek_string: ASP Service Strings to query for in Probe Requests * Returns: 0 on success, -1 on failure */ int p2p_find(struct p2p_data *p2p, unsigned int timeout, enum p2p_discovery_type type, unsigned int num_req_dev_types, const u8 *req_dev_types, - const u8 *dev_id, unsigned int search_delay); + const u8 *dev_id, unsigned int search_delay, + u8 seek_count, const char **seek_string); /** * p2p_notify_scan_trigger_status - Indicate scan trigger status diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h index 5ed0e7ce8..e7bcf0045 100644 --- a/src/p2p/p2p_i.h +++ b/src/p2p/p2p_i.h @@ -492,6 +492,12 @@ struct p2p_data { u8 pending_channel; u8 pending_channel_forced; + /* ASP Support */ + u8 wild_card_hash[P2PS_HASH_LEN]; + u8 query_hash[P2P_MAX_QUERY_HASH * P2PS_HASH_LEN]; + u8 p2ps_seek; + u8 p2ps_seek_count; + #ifdef CONFIG_WIFI_DISPLAY struct wpabuf *wfd_ie_beacon; struct wpabuf *wfd_ie_probe_req; diff --git a/wpa_supplicant/ctrl_iface.c b/wpa_supplicant/ctrl_iface.c index 843ddb735..ec6d690a4 100644 --- a/wpa_supplicant/ctrl_iface.c +++ b/wpa_supplicant/ctrl_iface.c @@ -4483,6 +4483,8 @@ static int p2p_ctrl_find(struct wpa_supplicant *wpa_s, char *cmd) u8 dev_type[WPS_DEV_TYPE_LEN], *_dev_type = NULL; char *pos; unsigned int search_delay; + const char *seek[P2P_MAX_QUERY_HASH + 1]; + u8 seek_count = 0; if (wpa_s->wpa_state == WPA_INTERFACE_DISABLED) { wpa_dbg(wpa_s, MSG_INFO, @@ -4517,8 +4519,33 @@ static int p2p_ctrl_find(struct wpa_supplicant *wpa_s, char *cmd) } else search_delay = wpas_p2p_search_delay(wpa_s); + /* Must be searched for last, because it adds nul termination */ + pos = os_strstr(cmd, " seek="); + while (pos && seek_count < P2P_MAX_QUERY_HASH + 1) { + char *term; + + term = os_strchr(pos + 1, ' '); + seek[seek_count++] = pos + 6; + pos = os_strstr(pos + 6, " seek="); + + if (term) + *term = '\0'; + } + + if (!seek_count) + return wpas_p2p_find(wpa_s, timeout, type, _dev_type != NULL, + _dev_type, _dev_id, + search_delay, 0, NULL); + + if (seek_count > P2P_MAX_QUERY_HASH) { + seek[0] = NULL; + return wpas_p2p_find(wpa_s, timeout, type, _dev_type != NULL, + _dev_type, _dev_id, + search_delay, 1, seek); + } + return wpas_p2p_find(wpa_s, timeout, type, _dev_type != NULL, _dev_type, - _dev_id, search_delay); + _dev_id, search_delay, seek_count, seek); } @@ -7654,7 +7681,7 @@ char * wpa_supplicant_ctrl_iface_process(struct wpa_supplicant *wpa_s, #endif /* CONFIG_MESH */ #ifdef CONFIG_P2P } else if (os_strncmp(buf, "P2P_FIND ", 9) == 0) { - if (p2p_ctrl_find(wpa_s, buf + 9)) + if (p2p_ctrl_find(wpa_s, buf + 8)) reply_len = -1; } else if (os_strcmp(buf, "P2P_FIND") == 0) { if (p2p_ctrl_find(wpa_s, "")) diff --git a/wpa_supplicant/dbus/dbus_new_handlers_p2p.c b/wpa_supplicant/dbus/dbus_new_handlers_p2p.c index 9c880a230..a14d35ddc 100644 --- a/wpa_supplicant/dbus/dbus_new_handlers_p2p.c +++ b/wpa_supplicant/dbus/dbus_new_handlers_p2p.c @@ -131,7 +131,7 @@ DBusMessage * wpas_dbus_handler_p2p_find(DBusMessage *message, wpa_s = wpa_s->p2p_dev; wpas_p2p_find(wpa_s, timeout, type, num_req_dev_types, req_dev_types, - NULL, 0); + NULL, 0, 0, NULL); os_free(req_dev_types); return reply; diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c index dfc3c9c0e..5fc401ba9 100644 --- a/wpa_supplicant/p2p_supplicant.c +++ b/wpa_supplicant/p2p_supplicant.c @@ -5912,7 +5912,8 @@ static void wpas_p2p_clear_pending_action_tx(struct wpa_supplicant *wpa_s) int wpas_p2p_find(struct wpa_supplicant *wpa_s, unsigned int timeout, enum p2p_discovery_type type, unsigned int num_req_dev_types, const u8 *req_dev_types, - const u8 *dev_id, unsigned int search_delay) + const u8 *dev_id, unsigned int search_delay, + u8 seek_cnt, const char **seek_string) { wpas_p2p_clear_pending_action_tx(wpa_s); wpa_s->p2p_long_listen = 0; @@ -5925,7 +5926,7 @@ int wpas_p2p_find(struct wpa_supplicant *wpa_s, unsigned int timeout, return p2p_find(wpa_s->global->p2p, timeout, type, num_req_dev_types, req_dev_types, dev_id, - search_delay); + search_delay, seek_cnt, seek_string); } diff --git a/wpa_supplicant/p2p_supplicant.h b/wpa_supplicant/p2p_supplicant.h index 9f5a83bd7..c8acda135 100644 --- a/wpa_supplicant/p2p_supplicant.h +++ b/wpa_supplicant/p2p_supplicant.h @@ -55,7 +55,8 @@ enum p2p_discovery_type; int wpas_p2p_find(struct wpa_supplicant *wpa_s, unsigned int timeout, enum p2p_discovery_type type, unsigned int num_req_dev_types, const u8 *req_dev_types, - const u8 *dev_id, unsigned int search_delay); + const u8 *dev_id, unsigned int search_delay, + u8 seek_cnt, const char **seek_string); void wpas_p2p_stop_find(struct wpa_supplicant *wpa_s); int wpas_p2p_listen(struct wpa_supplicant *wpa_s, unsigned int timeout); int wpas_p2p_listen_start(struct wpa_supplicant *wpa_s, unsigned int timeout);