diff --git a/wpa_supplicant/Android.mk b/wpa_supplicant/Android.mk index 579582b63..6d3961311 100644 --- a/wpa_supplicant/Android.mk +++ b/wpa_supplicant/Android.mk @@ -271,6 +271,7 @@ endif ifdef CONFIG_P2P OBJS += p2p_supplicant.c +OBJS += p2p_supplicant_sd.c OBJS += src/p2p/p2p.c OBJS += src/p2p/p2p_utils.c OBJS += src/p2p/p2p_parse.c diff --git a/wpa_supplicant/Makefile b/wpa_supplicant/Makefile index 0f82af9d7..976b984bd 100644 --- a/wpa_supplicant/Makefile +++ b/wpa_supplicant/Makefile @@ -276,6 +276,7 @@ endif ifdef CONFIG_P2P OBJS += p2p_supplicant.o +OBJS += p2p_supplicant_sd.o OBJS += ../src/p2p/p2p.o OBJS += ../src/p2p/p2p_utils.o OBJS += ../src/p2p/p2p_parse.o diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c index b200ca010..bb6180820 100644 --- a/wpa_supplicant/p2p_supplicant.c +++ b/wpa_supplicant/p2p_supplicant.c @@ -2388,1227 +2388,6 @@ static int wpas_send_probe_resp(void *ctx, const struct wpabuf *buf) } -/* - * DNS Header section is used only to calculate compression pointers, so the - * contents of this data does not matter, but the length needs to be reserved - * in the virtual packet. - */ -#define DNS_HEADER_LEN 12 - -/* - * 27-octet in-memory packet from P2P specification containing two implied - * queries for _tcp.lcoal. PTR IN and _udp.local. PTR IN - */ -#define P2P_SD_IN_MEMORY_LEN 27 - -static int p2p_sd_dns_uncompress_label(char **upos, char *uend, u8 *start, - u8 **spos, const u8 *end) -{ - while (*spos < end) { - u8 val = ((*spos)[0] & 0xc0) >> 6; - int len; - - if (val == 1 || val == 2) { - /* These are reserved values in RFC 1035 */ - wpa_printf(MSG_DEBUG, "P2P: Invalid domain name " - "sequence starting with 0x%x", val); - return -1; - } - - if (val == 3) { - u16 offset; - u8 *spos_tmp; - - /* Offset */ - if (*spos + 2 > end) { - wpa_printf(MSG_DEBUG, "P2P: No room for full " - "DNS offset field"); - return -1; - } - - offset = (((*spos)[0] & 0x3f) << 8) | (*spos)[1]; - if (offset >= *spos - start) { - wpa_printf(MSG_DEBUG, "P2P: Invalid DNS " - "pointer offset %u", offset); - return -1; - } - - (*spos) += 2; - spos_tmp = start + offset; - return p2p_sd_dns_uncompress_label(upos, uend, start, - &spos_tmp, - *spos - 2); - } - - /* Label */ - len = (*spos)[0] & 0x3f; - if (len == 0) - return 0; - - (*spos)++; - if (*spos + len > end) { - wpa_printf(MSG_DEBUG, "P2P: Invalid domain name " - "sequence - no room for label with length " - "%u", len); - return -1; - } - - if (*upos + len + 2 > uend) - return -2; - - os_memcpy(*upos, *spos, len); - *spos += len; - *upos += len; - (*upos)[0] = '.'; - (*upos)++; - (*upos)[0] = '\0'; - } - - return 0; -} - - -/* Uncompress domain names per RFC 1035 using the P2P SD in-memory packet. - * Returns -1 on parsing error (invalid input sequence), -2 if output buffer is - * not large enough */ -static int p2p_sd_dns_uncompress(char *buf, size_t buf_len, const u8 *msg, - size_t msg_len, size_t offset) -{ - /* 27-octet in-memory packet from P2P specification */ - const char *prefix = "\x04_tcp\x05local\x00\x00\x0C\x00\x01" - "\x04_udp\xC0\x11\x00\x0C\x00\x01"; - u8 *tmp, *end, *spos; - char *upos, *uend; - int ret = 0; - - if (buf_len < 2) - return -1; - if (offset > msg_len) - return -1; - - tmp = os_malloc(DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN + msg_len); - if (tmp == NULL) - return -1; - spos = tmp + DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN; - end = spos + msg_len; - spos += offset; - - os_memset(tmp, 0, DNS_HEADER_LEN); - os_memcpy(tmp + DNS_HEADER_LEN, prefix, P2P_SD_IN_MEMORY_LEN); - os_memcpy(tmp + DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN, msg, msg_len); - - upos = buf; - uend = buf + buf_len; - - ret = p2p_sd_dns_uncompress_label(&upos, uend, tmp, &spos, end); - if (ret) { - os_free(tmp); - return ret; - } - - if (upos == buf) { - upos[0] = '.'; - upos[1] = '\0'; - } else if (upos[-1] == '.') - upos[-1] = '\0'; - - os_free(tmp); - return 0; -} - - -static struct p2p_srv_bonjour * -wpas_p2p_service_get_bonjour(struct wpa_supplicant *wpa_s, - const struct wpabuf *query) -{ - struct p2p_srv_bonjour *bsrv; - size_t len; - - len = wpabuf_len(query); - dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, - struct p2p_srv_bonjour, list) { - if (len == wpabuf_len(bsrv->query) && - os_memcmp(wpabuf_head(query), wpabuf_head(bsrv->query), - len) == 0) - return bsrv; - } - return NULL; -} - - -static struct p2p_srv_upnp * -wpas_p2p_service_get_upnp(struct wpa_supplicant *wpa_s, u8 version, - const char *service) -{ - struct p2p_srv_upnp *usrv; - - dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, - struct p2p_srv_upnp, list) { - if (version == usrv->version && - os_strcmp(service, usrv->service) == 0) - return usrv; - } - return NULL; -} - - -static void wpas_sd_add_empty(struct wpabuf *resp, u8 srv_proto, - u8 srv_trans_id, u8 status) -{ - u8 *len_pos; - - if (wpabuf_tailroom(resp) < 5) - return; - - /* Length (to be filled) */ - len_pos = wpabuf_put(resp, 2); - wpabuf_put_u8(resp, srv_proto); - wpabuf_put_u8(resp, srv_trans_id); - /* Status Code */ - wpabuf_put_u8(resp, status); - /* Response Data: empty */ - WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); -} - - -static void wpas_sd_add_proto_not_avail(struct wpabuf *resp, u8 srv_proto, - u8 srv_trans_id) -{ - wpas_sd_add_empty(resp, srv_proto, srv_trans_id, - P2P_SD_PROTO_NOT_AVAILABLE); -} - - -static void wpas_sd_add_bad_request(struct wpabuf *resp, u8 srv_proto, - u8 srv_trans_id) -{ - wpas_sd_add_empty(resp, srv_proto, srv_trans_id, P2P_SD_BAD_REQUEST); -} - - -static void wpas_sd_add_not_found(struct wpabuf *resp, u8 srv_proto, - u8 srv_trans_id) -{ - wpas_sd_add_empty(resp, srv_proto, srv_trans_id, - P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); -} - - -static void wpas_sd_all_bonjour(struct wpa_supplicant *wpa_s, - struct wpabuf *resp, u8 srv_trans_id) -{ - struct p2p_srv_bonjour *bsrv; - u8 *len_pos; - - wpa_printf(MSG_DEBUG, "P2P: SD Request for all Bonjour services"); - - if (dl_list_empty(&wpa_s->global->p2p_srv_bonjour)) { - wpa_printf(MSG_DEBUG, "P2P: Bonjour protocol not available"); - return; - } - - dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, - struct p2p_srv_bonjour, list) { - if (wpabuf_tailroom(resp) < - 5 + wpabuf_len(bsrv->query) + wpabuf_len(bsrv->resp)) - return; - /* Length (to be filled) */ - len_pos = wpabuf_put(resp, 2); - wpabuf_put_u8(resp, P2P_SERV_BONJOUR); - wpabuf_put_u8(resp, srv_trans_id); - /* Status Code */ - wpabuf_put_u8(resp, P2P_SD_SUCCESS); - wpa_hexdump_ascii(MSG_DEBUG, "P2P: Matching Bonjour service", - wpabuf_head(bsrv->resp), - wpabuf_len(bsrv->resp)); - /* Response Data */ - wpabuf_put_buf(resp, bsrv->query); /* Key */ - wpabuf_put_buf(resp, bsrv->resp); /* Value */ - WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - - 2); - } -} - - -static int match_bonjour_query(struct p2p_srv_bonjour *bsrv, const u8 *query, - size_t query_len) -{ - char str_rx[256], str_srv[256]; - - if (query_len < 3 || wpabuf_len(bsrv->query) < 3) - return 0; /* Too short to include DNS Type and Version */ - if (os_memcmp(query + query_len - 3, - wpabuf_head_u8(bsrv->query) + wpabuf_len(bsrv->query) - 3, - 3) != 0) - return 0; /* Mismatch in DNS Type or Version */ - if (query_len == wpabuf_len(bsrv->query) && - os_memcmp(query, wpabuf_head(bsrv->query), query_len - 3) == 0) - return 1; /* Binary match */ - - if (p2p_sd_dns_uncompress(str_rx, sizeof(str_rx), query, query_len - 3, - 0)) - return 0; /* Failed to uncompress query */ - if (p2p_sd_dns_uncompress(str_srv, sizeof(str_srv), - wpabuf_head(bsrv->query), - wpabuf_len(bsrv->query) - 3, 0)) - return 0; /* Failed to uncompress service */ - - return os_strcmp(str_rx, str_srv) == 0; -} - - -static void wpas_sd_req_bonjour(struct wpa_supplicant *wpa_s, - struct wpabuf *resp, u8 srv_trans_id, - const u8 *query, size_t query_len) -{ - struct p2p_srv_bonjour *bsrv; - u8 *len_pos; - int matches = 0; - - wpa_hexdump_ascii(MSG_DEBUG, "P2P: SD Request for Bonjour", - query, query_len); - if (dl_list_empty(&wpa_s->global->p2p_srv_bonjour)) { - wpa_printf(MSG_DEBUG, "P2P: Bonjour protocol not available"); - wpas_sd_add_proto_not_avail(resp, P2P_SERV_BONJOUR, - srv_trans_id); - return; - } - - if (query_len == 0) { - wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); - return; - } - - dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, - struct p2p_srv_bonjour, list) { - if (!match_bonjour_query(bsrv, query, query_len)) - continue; - - if (wpabuf_tailroom(resp) < - 5 + query_len + wpabuf_len(bsrv->resp)) - return; - - matches++; - - /* Length (to be filled) */ - len_pos = wpabuf_put(resp, 2); - wpabuf_put_u8(resp, P2P_SERV_BONJOUR); - wpabuf_put_u8(resp, srv_trans_id); - - /* Status Code */ - wpabuf_put_u8(resp, P2P_SD_SUCCESS); - wpa_hexdump_ascii(MSG_DEBUG, "P2P: Matching Bonjour service", - wpabuf_head(bsrv->resp), - wpabuf_len(bsrv->resp)); - - /* Response Data */ - wpabuf_put_data(resp, query, query_len); /* Key */ - wpabuf_put_buf(resp, bsrv->resp); /* Value */ - - WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); - } - - if (matches == 0) { - wpa_printf(MSG_DEBUG, "P2P: Requested Bonjour service not " - "available"); - if (wpabuf_tailroom(resp) < 5) - return; - - /* Length (to be filled) */ - len_pos = wpabuf_put(resp, 2); - wpabuf_put_u8(resp, P2P_SERV_BONJOUR); - wpabuf_put_u8(resp, srv_trans_id); - - /* Status Code */ - wpabuf_put_u8(resp, P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); - /* Response Data: empty */ - WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - - 2); - } -} - - -static void wpas_sd_all_upnp(struct wpa_supplicant *wpa_s, - struct wpabuf *resp, u8 srv_trans_id) -{ - struct p2p_srv_upnp *usrv; - u8 *len_pos; - - wpa_printf(MSG_DEBUG, "P2P: SD Request for all UPnP services"); - - if (dl_list_empty(&wpa_s->global->p2p_srv_upnp)) { - wpa_printf(MSG_DEBUG, "P2P: UPnP protocol not available"); - return; - } - - dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, - struct p2p_srv_upnp, list) { - if (wpabuf_tailroom(resp) < 5 + 1 + os_strlen(usrv->service)) - return; - - /* Length (to be filled) */ - len_pos = wpabuf_put(resp, 2); - wpabuf_put_u8(resp, P2P_SERV_UPNP); - wpabuf_put_u8(resp, srv_trans_id); - - /* Status Code */ - wpabuf_put_u8(resp, P2P_SD_SUCCESS); - /* Response Data */ - wpabuf_put_u8(resp, usrv->version); - wpa_printf(MSG_DEBUG, "P2P: Matching UPnP Service: %s", - usrv->service); - wpabuf_put_str(resp, usrv->service); - WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - - 2); - } -} - - -static void wpas_sd_req_upnp(struct wpa_supplicant *wpa_s, - struct wpabuf *resp, u8 srv_trans_id, - const u8 *query, size_t query_len) -{ - struct p2p_srv_upnp *usrv; - u8 *len_pos; - u8 version; - char *str; - int count = 0; - - wpa_hexdump_ascii(MSG_DEBUG, "P2P: SD Request for UPnP", - query, query_len); - - if (dl_list_empty(&wpa_s->global->p2p_srv_upnp)) { - wpa_printf(MSG_DEBUG, "P2P: UPnP protocol not available"); - wpas_sd_add_proto_not_avail(resp, P2P_SERV_UPNP, - srv_trans_id); - return; - } - - if (query_len == 0) { - wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); - return; - } - - if (wpabuf_tailroom(resp) < 5) - return; - - /* Length (to be filled) */ - len_pos = wpabuf_put(resp, 2); - wpabuf_put_u8(resp, P2P_SERV_UPNP); - wpabuf_put_u8(resp, srv_trans_id); - - version = query[0]; - str = os_malloc(query_len); - if (str == NULL) - return; - os_memcpy(str, query + 1, query_len - 1); - str[query_len - 1] = '\0'; - - dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, - struct p2p_srv_upnp, list) { - if (version != usrv->version) - continue; - - if (os_strcmp(str, "ssdp:all") != 0 && - os_strstr(usrv->service, str) == NULL) - continue; - - if (wpabuf_tailroom(resp) < 2) - break; - if (count == 0) { - /* Status Code */ - wpabuf_put_u8(resp, P2P_SD_SUCCESS); - /* Response Data */ - wpabuf_put_u8(resp, version); - } else - wpabuf_put_u8(resp, ','); - - count++; - - wpa_printf(MSG_DEBUG, "P2P: Matching UPnP Service: %s", - usrv->service); - if (wpabuf_tailroom(resp) < os_strlen(usrv->service)) - break; - wpabuf_put_str(resp, usrv->service); - } - os_free(str); - - if (count == 0) { - wpa_printf(MSG_DEBUG, "P2P: Requested UPnP service not " - "available"); - /* Status Code */ - wpabuf_put_u8(resp, P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); - /* Response Data: empty */ - } - - WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); -} - - -#ifdef CONFIG_WIFI_DISPLAY -static void wpas_sd_req_wfd(struct wpa_supplicant *wpa_s, - struct wpabuf *resp, u8 srv_trans_id, - const u8 *query, size_t query_len) -{ - const u8 *pos; - u8 role; - u8 *len_pos; - - wpa_hexdump(MSG_DEBUG, "P2P: SD Request for WFD", query, query_len); - - if (!wpa_s->global->wifi_display) { - wpa_printf(MSG_DEBUG, "P2P: WFD protocol not available"); - wpas_sd_add_proto_not_avail(resp, P2P_SERV_WIFI_DISPLAY, - srv_trans_id); - return; - } - - if (query_len < 1) { - wpa_printf(MSG_DEBUG, "P2P: Missing WFD Requested Device " - "Role"); - return; - } - - if (wpabuf_tailroom(resp) < 5) - return; - - pos = query; - role = *pos++; - wpa_printf(MSG_DEBUG, "P2P: WSD for device role 0x%x", role); - - /* TODO: role specific handling */ - - /* Length (to be filled) */ - len_pos = wpabuf_put(resp, 2); - wpabuf_put_u8(resp, P2P_SERV_WIFI_DISPLAY); - wpabuf_put_u8(resp, srv_trans_id); - wpabuf_put_u8(resp, P2P_SD_SUCCESS); /* Status Code */ - - while (pos < query + query_len) { - if (*pos < MAX_WFD_SUBELEMS && - wpa_s->global->wfd_subelem[*pos] && - wpabuf_tailroom(resp) >= - wpabuf_len(wpa_s->global->wfd_subelem[*pos])) { - wpa_printf(MSG_DEBUG, "P2P: Add WSD response " - "subelement %u", *pos); - wpabuf_put_buf(resp, wpa_s->global->wfd_subelem[*pos]); - } - pos++; - } - - WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); -} -#endif /* CONFIG_WIFI_DISPLAY */ - - -static int find_p2ps_substr(struct p2ps_advertisement *adv_data, - const u8 *needle, size_t needle_len) -{ - const u8 *haystack = (const u8 *) adv_data->svc_info; - size_t haystack_len, i; - - /* Allow search term to be empty */ - if (!needle || !needle_len) - return 1; - - if (!haystack) - return 0; - - haystack_len = os_strlen(adv_data->svc_info); - for (i = 0; i < haystack_len; i++) { - if (haystack_len - i < needle_len) - break; - if (os_memcmp(haystack + i, needle, needle_len) == 0) - return 1; - } - - return 0; -} - - -static void wpas_sd_req_asp(struct wpa_supplicant *wpa_s, - struct wpabuf *resp, u8 srv_trans_id, - const u8 *query, size_t query_len) -{ - struct p2ps_advertisement *adv_data; - const u8 *svc = &query[1]; - const u8 *info = NULL; - size_t svc_len = query[0]; - size_t info_len = 0; - int prefix = 0; - u8 *count_pos = NULL; - u8 *len_pos = NULL; - - wpa_hexdump(MSG_DEBUG, "P2P: SD Request for ASP", query, query_len); - - if (!wpa_s->global->p2p) { - wpa_printf(MSG_DEBUG, "P2P: ASP protocol not available"); - wpas_sd_add_proto_not_avail(resp, P2P_SERV_P2PS, srv_trans_id); - return; - } - - /* Info block is optional */ - if (svc_len + 1 < query_len) { - info = &svc[svc_len]; - info_len = *info++; - } - - /* Range check length of svc string and info block */ - if (svc_len + (info_len ? info_len + 2 : 1) > query_len) { - wpa_printf(MSG_DEBUG, "P2P: ASP bad request"); - wpas_sd_add_bad_request(resp, P2P_SERV_P2PS, srv_trans_id); - return; - } - - /* Detect and correct for prefix search */ - if (svc_len && svc[svc_len - 1] == '*') { - prefix = 1; - svc_len--; - } - - for (adv_data = p2p_get_p2ps_adv_list(wpa_s->global->p2p); - adv_data; adv_data = adv_data->next) { - /* If not a prefix match, reject length mismatches */ - if (!prefix && svc_len != os_strlen(adv_data->svc_name)) - continue; - - /* Search each service for request */ - if (os_memcmp(adv_data->svc_name, svc, svc_len) == 0 && - find_p2ps_substr(adv_data, info, info_len)) { - size_t len = os_strlen(adv_data->svc_name); - size_t svc_info_len = 0; - - if (adv_data->svc_info) - svc_info_len = os_strlen(adv_data->svc_info); - - if (len > 0xff || svc_info_len > 0xffff) - return; - - /* Length & Count to be filled as we go */ - if (!len_pos && !count_pos) { - if (wpabuf_tailroom(resp) < - len + svc_info_len + 16) - return; - - len_pos = wpabuf_put(resp, 2); - wpabuf_put_u8(resp, P2P_SERV_P2PS); - wpabuf_put_u8(resp, srv_trans_id); - /* Status Code */ - wpabuf_put_u8(resp, P2P_SD_SUCCESS); - count_pos = wpabuf_put(resp, 1); - *count_pos = 0; - } else if (wpabuf_tailroom(resp) < - len + svc_info_len + 10) - return; - - if (svc_info_len) { - wpa_printf(MSG_DEBUG, - "P2P: Add Svc: %s info: %s", - adv_data->svc_name, - adv_data->svc_info); - } else { - wpa_printf(MSG_DEBUG, "P2P: Add Svc: %s", - adv_data->svc_name); - } - - /* Advertisement ID */ - wpabuf_put_le32(resp, adv_data->id); - - /* Config Methods */ - wpabuf_put_be16(resp, adv_data->config_methods); - - /* Service Name */ - wpabuf_put_u8(resp, (u8) len); - wpabuf_put_data(resp, adv_data->svc_name, len); - - /* Service State */ - wpabuf_put_u8(resp, adv_data->state); - - /* Service Information */ - wpabuf_put_le16(resp, (u16) svc_info_len); - wpabuf_put_data(resp, adv_data->svc_info, svc_info_len); - - /* Update length and count */ - (*count_pos)++; - WPA_PUT_LE16(len_pos, - (u8 *) wpabuf_put(resp, 0) - len_pos - 2); - } - } - - /* Return error if no matching svc found */ - if (count_pos == NULL) { - wpa_printf(MSG_DEBUG, "P2P: ASP service not found"); - wpas_sd_add_not_found(resp, P2P_SERV_P2PS, srv_trans_id); - } -} - - -static void wpas_sd_request(void *ctx, int freq, const u8 *sa, u8 dialog_token, - u16 update_indic, const u8 *tlvs, size_t tlvs_len) -{ - struct wpa_supplicant *wpa_s = ctx; - const u8 *pos = tlvs; - const u8 *end = tlvs + tlvs_len; - const u8 *tlv_end; - u16 slen; - struct wpabuf *resp; - u8 srv_proto, srv_trans_id; - size_t buf_len; - char *buf; - - wpa_hexdump(MSG_MSGDUMP, "P2P: Service Discovery Request TLVs", - tlvs, tlvs_len); - buf_len = 2 * tlvs_len + 1; - buf = os_malloc(buf_len); - if (buf) { - wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len); - wpa_msg_ctrl(wpa_s, MSG_INFO, P2P_EVENT_SERV_DISC_REQ "%d " - MACSTR " %u %u %s", - freq, MAC2STR(sa), dialog_token, update_indic, - buf); - os_free(buf); - } - - if (wpa_s->p2p_sd_over_ctrl_iface) { - wpas_notify_p2p_sd_request(wpa_s, freq, sa, dialog_token, - update_indic, tlvs, tlvs_len); - return; /* to be processed by an external program */ - } - - resp = wpabuf_alloc(10000); - if (resp == NULL) - return; - - while (pos + 1 < end) { - wpa_printf(MSG_DEBUG, "P2P: Service Request TLV"); - slen = WPA_GET_LE16(pos); - pos += 2; - if (pos + slen > end || slen < 2) { - wpa_printf(MSG_DEBUG, "P2P: Unexpected Query Data " - "length"); - wpabuf_free(resp); - return; - } - tlv_end = pos + slen; - - srv_proto = *pos++; - wpa_printf(MSG_DEBUG, "P2P: Service Protocol Type %u", - srv_proto); - srv_trans_id = *pos++; - wpa_printf(MSG_DEBUG, "P2P: Service Transaction ID %u", - srv_trans_id); - - wpa_hexdump(MSG_MSGDUMP, "P2P: Query Data", - pos, tlv_end - pos); - - - if (wpa_s->force_long_sd) { - wpa_printf(MSG_DEBUG, "P2P: SD test - force long " - "response"); - wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); - wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); - goto done; - } - - switch (srv_proto) { - case P2P_SERV_ALL_SERVICES: - wpa_printf(MSG_DEBUG, "P2P: Service Discovery Request " - "for all services"); - if (dl_list_empty(&wpa_s->global->p2p_srv_upnp) && - dl_list_empty(&wpa_s->global->p2p_srv_bonjour)) { - wpa_printf(MSG_DEBUG, "P2P: No service " - "discovery protocols available"); - wpas_sd_add_proto_not_avail( - resp, P2P_SERV_ALL_SERVICES, - srv_trans_id); - break; - } - wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); - wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); - break; - case P2P_SERV_BONJOUR: - wpas_sd_req_bonjour(wpa_s, resp, srv_trans_id, - pos, tlv_end - pos); - break; - case P2P_SERV_UPNP: - wpas_sd_req_upnp(wpa_s, resp, srv_trans_id, - pos, tlv_end - pos); - break; -#ifdef CONFIG_WIFI_DISPLAY - case P2P_SERV_WIFI_DISPLAY: - wpas_sd_req_wfd(wpa_s, resp, srv_trans_id, - pos, tlv_end - pos); - break; -#endif /* CONFIG_WIFI_DISPLAY */ - case P2P_SERV_P2PS: - wpas_sd_req_asp(wpa_s, resp, srv_trans_id, - pos, tlv_end - pos); - break; - default: - wpa_printf(MSG_DEBUG, "P2P: Unavailable service " - "protocol %u", srv_proto); - wpas_sd_add_proto_not_avail(resp, srv_proto, - srv_trans_id); - break; - } - - pos = tlv_end; - } - -done: - wpas_notify_p2p_sd_request(wpa_s, freq, sa, dialog_token, - update_indic, tlvs, tlvs_len); - - wpas_p2p_sd_response(wpa_s, freq, sa, dialog_token, resp); - - wpabuf_free(resp); -} - - -static void wpas_sd_p2ps_serv_response(struct wpa_supplicant *wpa_s, - const u8 *sa, u8 srv_trans_id, - const u8 *pos, const u8 *tlv_end) -{ - u8 left = *pos++; - u32 adv_id; - u8 svc_status; - u16 config_methods; - char svc_str[256]; - - while (left-- && pos < tlv_end) { - char *buf = NULL; - size_t buf_len; - u8 svc_len; - - /* Sanity check fixed length+svc_str */ - if (pos + 6 >= tlv_end) - break; - svc_len = pos[6]; - if (pos + svc_len + 10 > tlv_end) - break; - - /* Advertisement ID */ - adv_id = WPA_GET_LE32(pos); - pos += sizeof(u32); - - /* Config Methods */ - config_methods = WPA_GET_BE16(pos); - pos += sizeof(u16); - - /* Service Name */ - pos++; /* svc_len */ - os_memcpy(svc_str, pos, svc_len); - svc_str[svc_len] = '\0'; - pos += svc_len; - - /* Service Status */ - svc_status = *pos++; - - /* Service Information Length */ - buf_len = WPA_GET_LE16(pos); - pos += sizeof(u16); - - /* Sanity check buffer length */ - if (buf_len > (unsigned int) (tlv_end - pos)) - break; - - if (buf_len) { - buf = os_zalloc(2 * buf_len + 1); - if (buf) { - utf8_escape((const char *) pos, buf_len, buf, - 2 * buf_len + 1); - } - } - - pos += buf_len; - - if (buf) { - wpa_msg_global(wpa_s, MSG_INFO, P2P_EVENT_SERV_ASP_RESP - MACSTR " %x %x %x %x %s '%s'", - MAC2STR(sa), srv_trans_id, adv_id, - svc_status, config_methods, svc_str, - buf); - os_free(buf); - } else { - wpa_msg_global(wpa_s, MSG_INFO, P2P_EVENT_SERV_ASP_RESP - MACSTR " %x %x %x %x %s", - MAC2STR(sa), srv_trans_id, adv_id, - svc_status, config_methods, svc_str); - } - } -} - - -static void wpas_sd_response(void *ctx, const u8 *sa, u16 update_indic, - const u8 *tlvs, size_t tlvs_len) -{ - struct wpa_supplicant *wpa_s = ctx; - const u8 *pos = tlvs; - const u8 *end = tlvs + tlvs_len; - const u8 *tlv_end; - u16 slen; - size_t buf_len; - char *buf; - - wpa_hexdump(MSG_MSGDUMP, "P2P: Service Discovery Response TLVs", - tlvs, tlvs_len); - if (tlvs_len > 1500) { - /* TODO: better way for handling this */ - wpa_msg_ctrl(wpa_s, MSG_INFO, - P2P_EVENT_SERV_DISC_RESP MACSTR - " %u ", - MAC2STR(sa), update_indic, - (unsigned int) tlvs_len); - } else { - buf_len = 2 * tlvs_len + 1; - buf = os_malloc(buf_len); - if (buf) { - wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len); - wpa_msg_ctrl(wpa_s, MSG_INFO, - P2P_EVENT_SERV_DISC_RESP MACSTR " %u %s", - MAC2STR(sa), update_indic, buf); - os_free(buf); - } - } - - while (pos < end) { - u8 srv_proto, srv_trans_id, status; - - wpa_printf(MSG_DEBUG, "P2P: Service Response TLV"); - slen = WPA_GET_LE16(pos); - pos += 2; - if (pos + slen > end || slen < 3) { - wpa_printf(MSG_DEBUG, "P2P: Unexpected Response Data " - "length"); - return; - } - tlv_end = pos + slen; - - srv_proto = *pos++; - wpa_printf(MSG_DEBUG, "P2P: Service Protocol Type %u", - srv_proto); - srv_trans_id = *pos++; - wpa_printf(MSG_DEBUG, "P2P: Service Transaction ID %u", - srv_trans_id); - status = *pos++; - wpa_printf(MSG_DEBUG, "P2P: Status Code ID %u", - status); - - wpa_hexdump(MSG_MSGDUMP, "P2P: Response Data", - pos, tlv_end - pos); - - if (srv_proto == P2P_SERV_P2PS && pos < tlv_end) { - wpas_sd_p2ps_serv_response(wpa_s, sa, srv_trans_id, - pos, tlv_end); - } - - pos = tlv_end; - } - - wpas_notify_p2p_sd_response(wpa_s, sa, update_indic, tlvs, tlvs_len); -} - - -u64 wpas_p2p_sd_request(struct wpa_supplicant *wpa_s, const u8 *dst, - const struct wpabuf *tlvs) -{ - if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) - return 0; - return (uintptr_t) p2p_sd_request(wpa_s->global->p2p, dst, tlvs); -} - - -u64 wpas_p2p_sd_request_upnp(struct wpa_supplicant *wpa_s, const u8 *dst, - u8 version, const char *query) -{ - struct wpabuf *tlvs; - u64 ret; - - tlvs = wpabuf_alloc(2 + 1 + 1 + 1 + os_strlen(query)); - if (tlvs == NULL) - return 0; - wpabuf_put_le16(tlvs, 1 + 1 + 1 + os_strlen(query)); - wpabuf_put_u8(tlvs, P2P_SERV_UPNP); /* Service Protocol Type */ - wpabuf_put_u8(tlvs, 1); /* Service Transaction ID */ - wpabuf_put_u8(tlvs, version); - wpabuf_put_str(tlvs, query); - ret = wpas_p2p_sd_request(wpa_s, dst, tlvs); - wpabuf_free(tlvs); - return ret; -} - - -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, - const struct wpabuf *tlvs) -{ - 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->global->p2p_disabled || wpa_s->global->p2p == NULL) - return -1; - return p2p_sd_cancel_request(wpa_s->global->p2p, - (void *) (uintptr_t) req); -} - - -void wpas_p2p_sd_response(struct wpa_supplicant *wpa_s, int freq, - const u8 *dst, u8 dialog_token, - const struct wpabuf *resp_tlvs) -{ - if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) - return; - p2p_sd_response(wpa_s->global->p2p, freq, dst, dialog_token, - resp_tlvs); -} - - -void wpas_p2p_sd_service_update(struct wpa_supplicant *wpa_s) -{ - if (wpa_s->global->p2p) - p2p_sd_service_update(wpa_s->global->p2p); -} - - -static void wpas_p2p_srv_bonjour_free(struct p2p_srv_bonjour *bsrv) -{ - dl_list_del(&bsrv->list); - wpabuf_free(bsrv->query); - wpabuf_free(bsrv->resp); - os_free(bsrv); -} - - -static void wpas_p2p_srv_upnp_free(struct p2p_srv_upnp *usrv) -{ - dl_list_del(&usrv->list); - os_free(usrv->service); - os_free(usrv); -} - - -void wpas_p2p_service_flush(struct wpa_supplicant *wpa_s) -{ - struct p2p_srv_bonjour *bsrv, *bn; - struct p2p_srv_upnp *usrv, *un; - - dl_list_for_each_safe(bsrv, bn, &wpa_s->global->p2p_srv_bonjour, - struct p2p_srv_bonjour, list) - wpas_p2p_srv_bonjour_free(bsrv); - - dl_list_for_each_safe(usrv, un, &wpa_s->global->p2p_srv_upnp, - struct p2p_srv_upnp, list) - wpas_p2p_srv_upnp_free(usrv); - - wpas_p2p_sd_service_update(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) -{ - struct p2p_srv_bonjour *bsrv; - - bsrv = os_zalloc(sizeof(*bsrv)); - if (bsrv == NULL) - return -1; - bsrv->query = query; - bsrv->resp = resp; - dl_list_add(&wpa_s->global->p2p_srv_bonjour, &bsrv->list); - - wpas_p2p_sd_service_update(wpa_s); - return 0; -} - - -int wpas_p2p_service_del_bonjour(struct wpa_supplicant *wpa_s, - const struct wpabuf *query) -{ - struct p2p_srv_bonjour *bsrv; - - bsrv = wpas_p2p_service_get_bonjour(wpa_s, query); - if (bsrv == NULL) - return -1; - wpas_p2p_srv_bonjour_free(bsrv); - wpas_p2p_sd_service_update(wpa_s); - return 0; -} - - -int wpas_p2p_service_add_upnp(struct wpa_supplicant *wpa_s, u8 version, - const char *service) -{ - struct p2p_srv_upnp *usrv; - - if (wpas_p2p_service_get_upnp(wpa_s, version, service)) - return 0; /* Already listed */ - usrv = os_zalloc(sizeof(*usrv)); - if (usrv == NULL) - return -1; - usrv->version = version; - usrv->service = os_strdup(service); - if (usrv->service == NULL) { - os_free(usrv); - return -1; - } - dl_list_add(&wpa_s->global->p2p_srv_upnp, &usrv->list); - - wpas_p2p_sd_service_update(wpa_s); - return 0; -} - - -int wpas_p2p_service_del_upnp(struct wpa_supplicant *wpa_s, u8 version, - const char *service) -{ - struct p2p_srv_upnp *usrv; - - usrv = wpas_p2p_service_get_upnp(wpa_s, version, service); - if (usrv == NULL) - return -1; - wpas_p2p_srv_upnp_free(usrv); - wpas_p2p_sd_service_update(wpa_s); - return 0; -} - - static void wpas_prov_disc_local_display(struct wpa_supplicant *wpa_s, const u8 *peer, const char *params, unsigned int generated_pin) diff --git a/wpa_supplicant/p2p_supplicant.h b/wpa_supplicant/p2p_supplicant.h index b7861786c..1d3c67b7e 100644 --- a/wpa_supplicant/p2p_supplicant.h +++ b/wpa_supplicant/p2p_supplicant.h @@ -94,6 +94,10 @@ int wpas_p2p_service_add_asp(struct wpa_supplicant *wpa_s, int auto_accept, 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); +void wpas_sd_request(void *ctx, int freq, const u8 *sa, u8 dialog_token, + u16 update_indic, const u8 *tlvs, size_t tlvs_len); +void wpas_sd_response(void *ctx, const u8 *sa, u16 update_indic, + const u8 *tlvs, size_t tlvs_len); 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/p2p_supplicant_sd.c b/wpa_supplicant/p2p_supplicant_sd.c new file mode 100644 index 000000000..cb68c03bb --- /dev/null +++ b/wpa_supplicant/p2p_supplicant_sd.c @@ -0,0 +1,1237 @@ +/* + * wpa_supplicant - P2P service discovery + * Copyright (c) 2009-2010, Atheros Communications + * Copyright (c) 2010-2014, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "utils/includes.h" + +#include "utils/common.h" +#include "p2p/p2p.h" +#include "wpa_supplicant_i.h" +#include "notify.h" +#include "p2p_supplicant.h" + + +/* + * DNS Header section is used only to calculate compression pointers, so the + * contents of this data does not matter, but the length needs to be reserved + * in the virtual packet. + */ +#define DNS_HEADER_LEN 12 + +/* + * 27-octet in-memory packet from P2P specification containing two implied + * queries for _tcp.lcoal. PTR IN and _udp.local. PTR IN + */ +#define P2P_SD_IN_MEMORY_LEN 27 + +static int p2p_sd_dns_uncompress_label(char **upos, char *uend, u8 *start, + u8 **spos, const u8 *end) +{ + while (*spos < end) { + u8 val = ((*spos)[0] & 0xc0) >> 6; + int len; + + if (val == 1 || val == 2) { + /* These are reserved values in RFC 1035 */ + wpa_printf(MSG_DEBUG, "P2P: Invalid domain name " + "sequence starting with 0x%x", val); + return -1; + } + + if (val == 3) { + u16 offset; + u8 *spos_tmp; + + /* Offset */ + if (*spos + 2 > end) { + wpa_printf(MSG_DEBUG, "P2P: No room for full " + "DNS offset field"); + return -1; + } + + offset = (((*spos)[0] & 0x3f) << 8) | (*spos)[1]; + if (offset >= *spos - start) { + wpa_printf(MSG_DEBUG, "P2P: Invalid DNS " + "pointer offset %u", offset); + return -1; + } + + (*spos) += 2; + spos_tmp = start + offset; + return p2p_sd_dns_uncompress_label(upos, uend, start, + &spos_tmp, + *spos - 2); + } + + /* Label */ + len = (*spos)[0] & 0x3f; + if (len == 0) + return 0; + + (*spos)++; + if (*spos + len > end) { + wpa_printf(MSG_DEBUG, "P2P: Invalid domain name " + "sequence - no room for label with length " + "%u", len); + return -1; + } + + if (*upos + len + 2 > uend) + return -2; + + os_memcpy(*upos, *spos, len); + *spos += len; + *upos += len; + (*upos)[0] = '.'; + (*upos)++; + (*upos)[0] = '\0'; + } + + return 0; +} + + +/* Uncompress domain names per RFC 1035 using the P2P SD in-memory packet. + * Returns -1 on parsing error (invalid input sequence), -2 if output buffer is + * not large enough */ +static int p2p_sd_dns_uncompress(char *buf, size_t buf_len, const u8 *msg, + size_t msg_len, size_t offset) +{ + /* 27-octet in-memory packet from P2P specification */ + const char *prefix = "\x04_tcp\x05local\x00\x00\x0C\x00\x01" + "\x04_udp\xC0\x11\x00\x0C\x00\x01"; + u8 *tmp, *end, *spos; + char *upos, *uend; + int ret = 0; + + if (buf_len < 2) + return -1; + if (offset > msg_len) + return -1; + + tmp = os_malloc(DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN + msg_len); + if (tmp == NULL) + return -1; + spos = tmp + DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN; + end = spos + msg_len; + spos += offset; + + os_memset(tmp, 0, DNS_HEADER_LEN); + os_memcpy(tmp + DNS_HEADER_LEN, prefix, P2P_SD_IN_MEMORY_LEN); + os_memcpy(tmp + DNS_HEADER_LEN + P2P_SD_IN_MEMORY_LEN, msg, msg_len); + + upos = buf; + uend = buf + buf_len; + + ret = p2p_sd_dns_uncompress_label(&upos, uend, tmp, &spos, end); + if (ret) { + os_free(tmp); + return ret; + } + + if (upos == buf) { + upos[0] = '.'; + upos[1] = '\0'; + } else if (upos[-1] == '.') + upos[-1] = '\0'; + + os_free(tmp); + return 0; +} + + +static struct p2p_srv_bonjour * +wpas_p2p_service_get_bonjour(struct wpa_supplicant *wpa_s, + const struct wpabuf *query) +{ + struct p2p_srv_bonjour *bsrv; + size_t len; + + len = wpabuf_len(query); + dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, + struct p2p_srv_bonjour, list) { + if (len == wpabuf_len(bsrv->query) && + os_memcmp(wpabuf_head(query), wpabuf_head(bsrv->query), + len) == 0) + return bsrv; + } + return NULL; +} + + +static struct p2p_srv_upnp * +wpas_p2p_service_get_upnp(struct wpa_supplicant *wpa_s, u8 version, + const char *service) +{ + struct p2p_srv_upnp *usrv; + + dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, + struct p2p_srv_upnp, list) { + if (version == usrv->version && + os_strcmp(service, usrv->service) == 0) + return usrv; + } + return NULL; +} + + +static void wpas_sd_add_empty(struct wpabuf *resp, u8 srv_proto, + u8 srv_trans_id, u8 status) +{ + u8 *len_pos; + + if (wpabuf_tailroom(resp) < 5) + return; + + /* Length (to be filled) */ + len_pos = wpabuf_put(resp, 2); + wpabuf_put_u8(resp, srv_proto); + wpabuf_put_u8(resp, srv_trans_id); + /* Status Code */ + wpabuf_put_u8(resp, status); + /* Response Data: empty */ + WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); +} + + +static void wpas_sd_add_proto_not_avail(struct wpabuf *resp, u8 srv_proto, + u8 srv_trans_id) +{ + wpas_sd_add_empty(resp, srv_proto, srv_trans_id, + P2P_SD_PROTO_NOT_AVAILABLE); +} + + +static void wpas_sd_add_bad_request(struct wpabuf *resp, u8 srv_proto, + u8 srv_trans_id) +{ + wpas_sd_add_empty(resp, srv_proto, srv_trans_id, P2P_SD_BAD_REQUEST); +} + + +static void wpas_sd_add_not_found(struct wpabuf *resp, u8 srv_proto, + u8 srv_trans_id) +{ + wpas_sd_add_empty(resp, srv_proto, srv_trans_id, + P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); +} + + +static void wpas_sd_all_bonjour(struct wpa_supplicant *wpa_s, + struct wpabuf *resp, u8 srv_trans_id) +{ + struct p2p_srv_bonjour *bsrv; + u8 *len_pos; + + wpa_printf(MSG_DEBUG, "P2P: SD Request for all Bonjour services"); + + if (dl_list_empty(&wpa_s->global->p2p_srv_bonjour)) { + wpa_printf(MSG_DEBUG, "P2P: Bonjour protocol not available"); + return; + } + + dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, + struct p2p_srv_bonjour, list) { + if (wpabuf_tailroom(resp) < + 5 + wpabuf_len(bsrv->query) + wpabuf_len(bsrv->resp)) + return; + /* Length (to be filled) */ + len_pos = wpabuf_put(resp, 2); + wpabuf_put_u8(resp, P2P_SERV_BONJOUR); + wpabuf_put_u8(resp, srv_trans_id); + /* Status Code */ + wpabuf_put_u8(resp, P2P_SD_SUCCESS); + wpa_hexdump_ascii(MSG_DEBUG, "P2P: Matching Bonjour service", + wpabuf_head(bsrv->resp), + wpabuf_len(bsrv->resp)); + /* Response Data */ + wpabuf_put_buf(resp, bsrv->query); /* Key */ + wpabuf_put_buf(resp, bsrv->resp); /* Value */ + WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - + 2); + } +} + + +static int match_bonjour_query(struct p2p_srv_bonjour *bsrv, const u8 *query, + size_t query_len) +{ + char str_rx[256], str_srv[256]; + + if (query_len < 3 || wpabuf_len(bsrv->query) < 3) + return 0; /* Too short to include DNS Type and Version */ + if (os_memcmp(query + query_len - 3, + wpabuf_head_u8(bsrv->query) + wpabuf_len(bsrv->query) - 3, + 3) != 0) + return 0; /* Mismatch in DNS Type or Version */ + if (query_len == wpabuf_len(bsrv->query) && + os_memcmp(query, wpabuf_head(bsrv->query), query_len - 3) == 0) + return 1; /* Binary match */ + + if (p2p_sd_dns_uncompress(str_rx, sizeof(str_rx), query, query_len - 3, + 0)) + return 0; /* Failed to uncompress query */ + if (p2p_sd_dns_uncompress(str_srv, sizeof(str_srv), + wpabuf_head(bsrv->query), + wpabuf_len(bsrv->query) - 3, 0)) + return 0; /* Failed to uncompress service */ + + return os_strcmp(str_rx, str_srv) == 0; +} + + +static void wpas_sd_req_bonjour(struct wpa_supplicant *wpa_s, + struct wpabuf *resp, u8 srv_trans_id, + const u8 *query, size_t query_len) +{ + struct p2p_srv_bonjour *bsrv; + u8 *len_pos; + int matches = 0; + + wpa_hexdump_ascii(MSG_DEBUG, "P2P: SD Request for Bonjour", + query, query_len); + if (dl_list_empty(&wpa_s->global->p2p_srv_bonjour)) { + wpa_printf(MSG_DEBUG, "P2P: Bonjour protocol not available"); + wpas_sd_add_proto_not_avail(resp, P2P_SERV_BONJOUR, + srv_trans_id); + return; + } + + if (query_len == 0) { + wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); + return; + } + + dl_list_for_each(bsrv, &wpa_s->global->p2p_srv_bonjour, + struct p2p_srv_bonjour, list) { + if (!match_bonjour_query(bsrv, query, query_len)) + continue; + + if (wpabuf_tailroom(resp) < + 5 + query_len + wpabuf_len(bsrv->resp)) + return; + + matches++; + + /* Length (to be filled) */ + len_pos = wpabuf_put(resp, 2); + wpabuf_put_u8(resp, P2P_SERV_BONJOUR); + wpabuf_put_u8(resp, srv_trans_id); + + /* Status Code */ + wpabuf_put_u8(resp, P2P_SD_SUCCESS); + wpa_hexdump_ascii(MSG_DEBUG, "P2P: Matching Bonjour service", + wpabuf_head(bsrv->resp), + wpabuf_len(bsrv->resp)); + + /* Response Data */ + wpabuf_put_data(resp, query, query_len); /* Key */ + wpabuf_put_buf(resp, bsrv->resp); /* Value */ + + WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); + } + + if (matches == 0) { + wpa_printf(MSG_DEBUG, "P2P: Requested Bonjour service not " + "available"); + if (wpabuf_tailroom(resp) < 5) + return; + + /* Length (to be filled) */ + len_pos = wpabuf_put(resp, 2); + wpabuf_put_u8(resp, P2P_SERV_BONJOUR); + wpabuf_put_u8(resp, srv_trans_id); + + /* Status Code */ + wpabuf_put_u8(resp, P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); + /* Response Data: empty */ + WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - + 2); + } +} + + +static void wpas_sd_all_upnp(struct wpa_supplicant *wpa_s, + struct wpabuf *resp, u8 srv_trans_id) +{ + struct p2p_srv_upnp *usrv; + u8 *len_pos; + + wpa_printf(MSG_DEBUG, "P2P: SD Request for all UPnP services"); + + if (dl_list_empty(&wpa_s->global->p2p_srv_upnp)) { + wpa_printf(MSG_DEBUG, "P2P: UPnP protocol not available"); + return; + } + + dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, + struct p2p_srv_upnp, list) { + if (wpabuf_tailroom(resp) < 5 + 1 + os_strlen(usrv->service)) + return; + + /* Length (to be filled) */ + len_pos = wpabuf_put(resp, 2); + wpabuf_put_u8(resp, P2P_SERV_UPNP); + wpabuf_put_u8(resp, srv_trans_id); + + /* Status Code */ + wpabuf_put_u8(resp, P2P_SD_SUCCESS); + /* Response Data */ + wpabuf_put_u8(resp, usrv->version); + wpa_printf(MSG_DEBUG, "P2P: Matching UPnP Service: %s", + usrv->service); + wpabuf_put_str(resp, usrv->service); + WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - + 2); + } +} + + +static void wpas_sd_req_upnp(struct wpa_supplicant *wpa_s, + struct wpabuf *resp, u8 srv_trans_id, + const u8 *query, size_t query_len) +{ + struct p2p_srv_upnp *usrv; + u8 *len_pos; + u8 version; + char *str; + int count = 0; + + wpa_hexdump_ascii(MSG_DEBUG, "P2P: SD Request for UPnP", + query, query_len); + + if (dl_list_empty(&wpa_s->global->p2p_srv_upnp)) { + wpa_printf(MSG_DEBUG, "P2P: UPnP protocol not available"); + wpas_sd_add_proto_not_avail(resp, P2P_SERV_UPNP, + srv_trans_id); + return; + } + + if (query_len == 0) { + wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); + return; + } + + if (wpabuf_tailroom(resp) < 5) + return; + + /* Length (to be filled) */ + len_pos = wpabuf_put(resp, 2); + wpabuf_put_u8(resp, P2P_SERV_UPNP); + wpabuf_put_u8(resp, srv_trans_id); + + version = query[0]; + str = os_malloc(query_len); + if (str == NULL) + return; + os_memcpy(str, query + 1, query_len - 1); + str[query_len - 1] = '\0'; + + dl_list_for_each(usrv, &wpa_s->global->p2p_srv_upnp, + struct p2p_srv_upnp, list) { + if (version != usrv->version) + continue; + + if (os_strcmp(str, "ssdp:all") != 0 && + os_strstr(usrv->service, str) == NULL) + continue; + + if (wpabuf_tailroom(resp) < 2) + break; + if (count == 0) { + /* Status Code */ + wpabuf_put_u8(resp, P2P_SD_SUCCESS); + /* Response Data */ + wpabuf_put_u8(resp, version); + } else + wpabuf_put_u8(resp, ','); + + count++; + + wpa_printf(MSG_DEBUG, "P2P: Matching UPnP Service: %s", + usrv->service); + if (wpabuf_tailroom(resp) < os_strlen(usrv->service)) + break; + wpabuf_put_str(resp, usrv->service); + } + os_free(str); + + if (count == 0) { + wpa_printf(MSG_DEBUG, "P2P: Requested UPnP service not " + "available"); + /* Status Code */ + wpabuf_put_u8(resp, P2P_SD_REQUESTED_INFO_NOT_AVAILABLE); + /* Response Data: empty */ + } + + WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); +} + + +#ifdef CONFIG_WIFI_DISPLAY +static void wpas_sd_req_wfd(struct wpa_supplicant *wpa_s, + struct wpabuf *resp, u8 srv_trans_id, + const u8 *query, size_t query_len) +{ + const u8 *pos; + u8 role; + u8 *len_pos; + + wpa_hexdump(MSG_DEBUG, "P2P: SD Request for WFD", query, query_len); + + if (!wpa_s->global->wifi_display) { + wpa_printf(MSG_DEBUG, "P2P: WFD protocol not available"); + wpas_sd_add_proto_not_avail(resp, P2P_SERV_WIFI_DISPLAY, + srv_trans_id); + return; + } + + if (query_len < 1) { + wpa_printf(MSG_DEBUG, "P2P: Missing WFD Requested Device " + "Role"); + return; + } + + if (wpabuf_tailroom(resp) < 5) + return; + + pos = query; + role = *pos++; + wpa_printf(MSG_DEBUG, "P2P: WSD for device role 0x%x", role); + + /* TODO: role specific handling */ + + /* Length (to be filled) */ + len_pos = wpabuf_put(resp, 2); + wpabuf_put_u8(resp, P2P_SERV_WIFI_DISPLAY); + wpabuf_put_u8(resp, srv_trans_id); + wpabuf_put_u8(resp, P2P_SD_SUCCESS); /* Status Code */ + + while (pos < query + query_len) { + if (*pos < MAX_WFD_SUBELEMS && + wpa_s->global->wfd_subelem[*pos] && + wpabuf_tailroom(resp) >= + wpabuf_len(wpa_s->global->wfd_subelem[*pos])) { + wpa_printf(MSG_DEBUG, "P2P: Add WSD response " + "subelement %u", *pos); + wpabuf_put_buf(resp, wpa_s->global->wfd_subelem[*pos]); + } + pos++; + } + + WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2); +} +#endif /* CONFIG_WIFI_DISPLAY */ + + +static int find_p2ps_substr(struct p2ps_advertisement *adv_data, + const u8 *needle, size_t needle_len) +{ + const u8 *haystack = (const u8 *) adv_data->svc_info; + size_t haystack_len, i; + + /* Allow search term to be empty */ + if (!needle || !needle_len) + return 1; + + if (!haystack) + return 0; + + haystack_len = os_strlen(adv_data->svc_info); + for (i = 0; i < haystack_len; i++) { + if (haystack_len - i < needle_len) + break; + if (os_memcmp(haystack + i, needle, needle_len) == 0) + return 1; + } + + return 0; +} + + +static void wpas_sd_req_asp(struct wpa_supplicant *wpa_s, + struct wpabuf *resp, u8 srv_trans_id, + const u8 *query, size_t query_len) +{ + struct p2ps_advertisement *adv_data; + const u8 *svc = &query[1]; + const u8 *info = NULL; + size_t svc_len = query[0]; + size_t info_len = 0; + int prefix = 0; + u8 *count_pos = NULL; + u8 *len_pos = NULL; + + wpa_hexdump(MSG_DEBUG, "P2P: SD Request for ASP", query, query_len); + + if (!wpa_s->global->p2p) { + wpa_printf(MSG_DEBUG, "P2P: ASP protocol not available"); + wpas_sd_add_proto_not_avail(resp, P2P_SERV_P2PS, srv_trans_id); + return; + } + + /* Info block is optional */ + if (svc_len + 1 < query_len) { + info = &svc[svc_len]; + info_len = *info++; + } + + /* Range check length of svc string and info block */ + if (svc_len + (info_len ? info_len + 2 : 1) > query_len) { + wpa_printf(MSG_DEBUG, "P2P: ASP bad request"); + wpas_sd_add_bad_request(resp, P2P_SERV_P2PS, srv_trans_id); + return; + } + + /* Detect and correct for prefix search */ + if (svc_len && svc[svc_len - 1] == '*') { + prefix = 1; + svc_len--; + } + + for (adv_data = p2p_get_p2ps_adv_list(wpa_s->global->p2p); + adv_data; adv_data = adv_data->next) { + /* If not a prefix match, reject length mismatches */ + if (!prefix && svc_len != os_strlen(adv_data->svc_name)) + continue; + + /* Search each service for request */ + if (os_memcmp(adv_data->svc_name, svc, svc_len) == 0 && + find_p2ps_substr(adv_data, info, info_len)) { + size_t len = os_strlen(adv_data->svc_name); + size_t svc_info_len = 0; + + if (adv_data->svc_info) + svc_info_len = os_strlen(adv_data->svc_info); + + if (len > 0xff || svc_info_len > 0xffff) + return; + + /* Length & Count to be filled as we go */ + if (!len_pos && !count_pos) { + if (wpabuf_tailroom(resp) < + len + svc_info_len + 16) + return; + + len_pos = wpabuf_put(resp, 2); + wpabuf_put_u8(resp, P2P_SERV_P2PS); + wpabuf_put_u8(resp, srv_trans_id); + /* Status Code */ + wpabuf_put_u8(resp, P2P_SD_SUCCESS); + count_pos = wpabuf_put(resp, 1); + *count_pos = 0; + } else if (wpabuf_tailroom(resp) < + len + svc_info_len + 10) + return; + + if (svc_info_len) { + wpa_printf(MSG_DEBUG, + "P2P: Add Svc: %s info: %s", + adv_data->svc_name, + adv_data->svc_info); + } else { + wpa_printf(MSG_DEBUG, "P2P: Add Svc: %s", + adv_data->svc_name); + } + + /* Advertisement ID */ + wpabuf_put_le32(resp, adv_data->id); + + /* Config Methods */ + wpabuf_put_be16(resp, adv_data->config_methods); + + /* Service Name */ + wpabuf_put_u8(resp, (u8) len); + wpabuf_put_data(resp, adv_data->svc_name, len); + + /* Service State */ + wpabuf_put_u8(resp, adv_data->state); + + /* Service Information */ + wpabuf_put_le16(resp, (u16) svc_info_len); + wpabuf_put_data(resp, adv_data->svc_info, svc_info_len); + + /* Update length and count */ + (*count_pos)++; + WPA_PUT_LE16(len_pos, + (u8 *) wpabuf_put(resp, 0) - len_pos - 2); + } + } + + /* Return error if no matching svc found */ + if (count_pos == NULL) { + wpa_printf(MSG_DEBUG, "P2P: ASP service not found"); + wpas_sd_add_not_found(resp, P2P_SERV_P2PS, srv_trans_id); + } +} + + +void wpas_sd_request(void *ctx, int freq, const u8 *sa, u8 dialog_token, + u16 update_indic, const u8 *tlvs, size_t tlvs_len) +{ + struct wpa_supplicant *wpa_s = ctx; + const u8 *pos = tlvs; + const u8 *end = tlvs + tlvs_len; + const u8 *tlv_end; + u16 slen; + struct wpabuf *resp; + u8 srv_proto, srv_trans_id; + size_t buf_len; + char *buf; + + wpa_hexdump(MSG_MSGDUMP, "P2P: Service Discovery Request TLVs", + tlvs, tlvs_len); + buf_len = 2 * tlvs_len + 1; + buf = os_malloc(buf_len); + if (buf) { + wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len); + wpa_msg_ctrl(wpa_s, MSG_INFO, P2P_EVENT_SERV_DISC_REQ "%d " + MACSTR " %u %u %s", + freq, MAC2STR(sa), dialog_token, update_indic, + buf); + os_free(buf); + } + + if (wpa_s->p2p_sd_over_ctrl_iface) { + wpas_notify_p2p_sd_request(wpa_s, freq, sa, dialog_token, + update_indic, tlvs, tlvs_len); + return; /* to be processed by an external program */ + } + + resp = wpabuf_alloc(10000); + if (resp == NULL) + return; + + while (pos + 1 < end) { + wpa_printf(MSG_DEBUG, "P2P: Service Request TLV"); + slen = WPA_GET_LE16(pos); + pos += 2; + if (pos + slen > end || slen < 2) { + wpa_printf(MSG_DEBUG, "P2P: Unexpected Query Data " + "length"); + wpabuf_free(resp); + return; + } + tlv_end = pos + slen; + + srv_proto = *pos++; + wpa_printf(MSG_DEBUG, "P2P: Service Protocol Type %u", + srv_proto); + srv_trans_id = *pos++; + wpa_printf(MSG_DEBUG, "P2P: Service Transaction ID %u", + srv_trans_id); + + wpa_hexdump(MSG_MSGDUMP, "P2P: Query Data", + pos, tlv_end - pos); + + + if (wpa_s->force_long_sd) { + wpa_printf(MSG_DEBUG, "P2P: SD test - force long " + "response"); + wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); + wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); + goto done; + } + + switch (srv_proto) { + case P2P_SERV_ALL_SERVICES: + wpa_printf(MSG_DEBUG, "P2P: Service Discovery Request " + "for all services"); + if (dl_list_empty(&wpa_s->global->p2p_srv_upnp) && + dl_list_empty(&wpa_s->global->p2p_srv_bonjour)) { + wpa_printf(MSG_DEBUG, "P2P: No service " + "discovery protocols available"); + wpas_sd_add_proto_not_avail( + resp, P2P_SERV_ALL_SERVICES, + srv_trans_id); + break; + } + wpas_sd_all_bonjour(wpa_s, resp, srv_trans_id); + wpas_sd_all_upnp(wpa_s, resp, srv_trans_id); + break; + case P2P_SERV_BONJOUR: + wpas_sd_req_bonjour(wpa_s, resp, srv_trans_id, + pos, tlv_end - pos); + break; + case P2P_SERV_UPNP: + wpas_sd_req_upnp(wpa_s, resp, srv_trans_id, + pos, tlv_end - pos); + break; +#ifdef CONFIG_WIFI_DISPLAY + case P2P_SERV_WIFI_DISPLAY: + wpas_sd_req_wfd(wpa_s, resp, srv_trans_id, + pos, tlv_end - pos); + break; +#endif /* CONFIG_WIFI_DISPLAY */ + case P2P_SERV_P2PS: + wpas_sd_req_asp(wpa_s, resp, srv_trans_id, + pos, tlv_end - pos); + break; + default: + wpa_printf(MSG_DEBUG, "P2P: Unavailable service " + "protocol %u", srv_proto); + wpas_sd_add_proto_not_avail(resp, srv_proto, + srv_trans_id); + break; + } + + pos = tlv_end; + } + +done: + wpas_notify_p2p_sd_request(wpa_s, freq, sa, dialog_token, + update_indic, tlvs, tlvs_len); + + wpas_p2p_sd_response(wpa_s, freq, sa, dialog_token, resp); + + wpabuf_free(resp); +} + + +static void wpas_sd_p2ps_serv_response(struct wpa_supplicant *wpa_s, + const u8 *sa, u8 srv_trans_id, + const u8 *pos, const u8 *tlv_end) +{ + u8 left = *pos++; + u32 adv_id; + u8 svc_status; + u16 config_methods; + char svc_str[256]; + + while (left-- && pos < tlv_end) { + char *buf = NULL; + size_t buf_len; + u8 svc_len; + + /* Sanity check fixed length+svc_str */ + if (pos + 6 >= tlv_end) + break; + svc_len = pos[6]; + if (pos + svc_len + 10 > tlv_end) + break; + + /* Advertisement ID */ + adv_id = WPA_GET_LE32(pos); + pos += sizeof(u32); + + /* Config Methods */ + config_methods = WPA_GET_BE16(pos); + pos += sizeof(u16); + + /* Service Name */ + pos++; /* svc_len */ + os_memcpy(svc_str, pos, svc_len); + svc_str[svc_len] = '\0'; + pos += svc_len; + + /* Service Status */ + svc_status = *pos++; + + /* Service Information Length */ + buf_len = WPA_GET_LE16(pos); + pos += sizeof(u16); + + /* Sanity check buffer length */ + if (buf_len > (unsigned int) (tlv_end - pos)) + break; + + if (buf_len) { + buf = os_zalloc(2 * buf_len + 1); + if (buf) { + utf8_escape((const char *) pos, buf_len, buf, + 2 * buf_len + 1); + } + } + + pos += buf_len; + + if (buf) { + wpa_msg_global(wpa_s, MSG_INFO, P2P_EVENT_SERV_ASP_RESP + MACSTR " %x %x %x %x %s '%s'", + MAC2STR(sa), srv_trans_id, adv_id, + svc_status, config_methods, svc_str, + buf); + os_free(buf); + } else { + wpa_msg_global(wpa_s, MSG_INFO, P2P_EVENT_SERV_ASP_RESP + MACSTR " %x %x %x %x %s", + MAC2STR(sa), srv_trans_id, adv_id, + svc_status, config_methods, svc_str); + } + } +} + + +void wpas_sd_response(void *ctx, const u8 *sa, u16 update_indic, + const u8 *tlvs, size_t tlvs_len) +{ + struct wpa_supplicant *wpa_s = ctx; + const u8 *pos = tlvs; + const u8 *end = tlvs + tlvs_len; + const u8 *tlv_end; + u16 slen; + size_t buf_len; + char *buf; + + wpa_hexdump(MSG_MSGDUMP, "P2P: Service Discovery Response TLVs", + tlvs, tlvs_len); + if (tlvs_len > 1500) { + /* TODO: better way for handling this */ + wpa_msg_ctrl(wpa_s, MSG_INFO, + P2P_EVENT_SERV_DISC_RESP MACSTR + " %u ", + MAC2STR(sa), update_indic, + (unsigned int) tlvs_len); + } else { + buf_len = 2 * tlvs_len + 1; + buf = os_malloc(buf_len); + if (buf) { + wpa_snprintf_hex(buf, buf_len, tlvs, tlvs_len); + wpa_msg_ctrl(wpa_s, MSG_INFO, + P2P_EVENT_SERV_DISC_RESP MACSTR " %u %s", + MAC2STR(sa), update_indic, buf); + os_free(buf); + } + } + + while (pos < end) { + u8 srv_proto, srv_trans_id, status; + + wpa_printf(MSG_DEBUG, "P2P: Service Response TLV"); + slen = WPA_GET_LE16(pos); + pos += 2; + if (pos + slen > end || slen < 3) { + wpa_printf(MSG_DEBUG, "P2P: Unexpected Response Data " + "length"); + return; + } + tlv_end = pos + slen; + + srv_proto = *pos++; + wpa_printf(MSG_DEBUG, "P2P: Service Protocol Type %u", + srv_proto); + srv_trans_id = *pos++; + wpa_printf(MSG_DEBUG, "P2P: Service Transaction ID %u", + srv_trans_id); + status = *pos++; + wpa_printf(MSG_DEBUG, "P2P: Status Code ID %u", + status); + + wpa_hexdump(MSG_MSGDUMP, "P2P: Response Data", + pos, tlv_end - pos); + + if (srv_proto == P2P_SERV_P2PS && pos < tlv_end) { + wpas_sd_p2ps_serv_response(wpa_s, sa, srv_trans_id, + pos, tlv_end); + } + + pos = tlv_end; + } + + wpas_notify_p2p_sd_response(wpa_s, sa, update_indic, tlvs, tlvs_len); +} + + +u64 wpas_p2p_sd_request(struct wpa_supplicant *wpa_s, const u8 *dst, + const struct wpabuf *tlvs) +{ + if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) + return 0; + return (uintptr_t) p2p_sd_request(wpa_s->global->p2p, dst, tlvs); +} + + +u64 wpas_p2p_sd_request_upnp(struct wpa_supplicant *wpa_s, const u8 *dst, + u8 version, const char *query) +{ + struct wpabuf *tlvs; + u64 ret; + + tlvs = wpabuf_alloc(2 + 1 + 1 + 1 + os_strlen(query)); + if (tlvs == NULL) + return 0; + wpabuf_put_le16(tlvs, 1 + 1 + 1 + os_strlen(query)); + wpabuf_put_u8(tlvs, P2P_SERV_UPNP); /* Service Protocol Type */ + wpabuf_put_u8(tlvs, 1); /* Service Transaction ID */ + wpabuf_put_u8(tlvs, version); + wpabuf_put_str(tlvs, query); + ret = wpas_p2p_sd_request(wpa_s, dst, tlvs); + wpabuf_free(tlvs); + return ret; +} + + +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, + const struct wpabuf *tlvs) +{ + 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->global->p2p_disabled || wpa_s->global->p2p == NULL) + return -1; + return p2p_sd_cancel_request(wpa_s->global->p2p, + (void *) (uintptr_t) req); +} + + +void wpas_p2p_sd_response(struct wpa_supplicant *wpa_s, int freq, + const u8 *dst, u8 dialog_token, + const struct wpabuf *resp_tlvs) +{ + if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) + return; + p2p_sd_response(wpa_s->global->p2p, freq, dst, dialog_token, + resp_tlvs); +} + + +void wpas_p2p_sd_service_update(struct wpa_supplicant *wpa_s) +{ + if (wpa_s->global->p2p) + p2p_sd_service_update(wpa_s->global->p2p); +} + + +static void wpas_p2p_srv_bonjour_free(struct p2p_srv_bonjour *bsrv) +{ + dl_list_del(&bsrv->list); + wpabuf_free(bsrv->query); + wpabuf_free(bsrv->resp); + os_free(bsrv); +} + + +static void wpas_p2p_srv_upnp_free(struct p2p_srv_upnp *usrv) +{ + dl_list_del(&usrv->list); + os_free(usrv->service); + os_free(usrv); +} + + +void wpas_p2p_service_flush(struct wpa_supplicant *wpa_s) +{ + struct p2p_srv_bonjour *bsrv, *bn; + struct p2p_srv_upnp *usrv, *un; + + dl_list_for_each_safe(bsrv, bn, &wpa_s->global->p2p_srv_bonjour, + struct p2p_srv_bonjour, list) + wpas_p2p_srv_bonjour_free(bsrv); + + dl_list_for_each_safe(usrv, un, &wpa_s->global->p2p_srv_upnp, + struct p2p_srv_upnp, list) + wpas_p2p_srv_upnp_free(usrv); + + wpas_p2p_sd_service_update(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) +{ + struct p2p_srv_bonjour *bsrv; + + bsrv = os_zalloc(sizeof(*bsrv)); + if (bsrv == NULL) + return -1; + bsrv->query = query; + bsrv->resp = resp; + dl_list_add(&wpa_s->global->p2p_srv_bonjour, &bsrv->list); + + wpas_p2p_sd_service_update(wpa_s); + return 0; +} + + +int wpas_p2p_service_del_bonjour(struct wpa_supplicant *wpa_s, + const struct wpabuf *query) +{ + struct p2p_srv_bonjour *bsrv; + + bsrv = wpas_p2p_service_get_bonjour(wpa_s, query); + if (bsrv == NULL) + return -1; + wpas_p2p_srv_bonjour_free(bsrv); + wpas_p2p_sd_service_update(wpa_s); + return 0; +} + + +int wpas_p2p_service_add_upnp(struct wpa_supplicant *wpa_s, u8 version, + const char *service) +{ + struct p2p_srv_upnp *usrv; + + if (wpas_p2p_service_get_upnp(wpa_s, version, service)) + return 0; /* Already listed */ + usrv = os_zalloc(sizeof(*usrv)); + if (usrv == NULL) + return -1; + usrv->version = version; + usrv->service = os_strdup(service); + if (usrv->service == NULL) { + os_free(usrv); + return -1; + } + dl_list_add(&wpa_s->global->p2p_srv_upnp, &usrv->list); + + wpas_p2p_sd_service_update(wpa_s); + return 0; +} + + +int wpas_p2p_service_del_upnp(struct wpa_supplicant *wpa_s, u8 version, + const char *service) +{ + struct p2p_srv_upnp *usrv; + + usrv = wpas_p2p_service_get_upnp(wpa_s, version, service); + if (usrv == NULL) + return -1; + wpas_p2p_srv_upnp_free(usrv); + wpas_p2p_sd_service_update(wpa_s); + return 0; +}