18d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt/* 28d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * Wi-Fi Direct - P2P service discovery 38d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * Copyright (c) 2009, Atheros Communications 48d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * 5c5ec7f57ead87efa365800228aa0b09a12d9e6c4Dmitry Shmidt * This software may be distributed under the terms of the BSD license. 6c5ec7f57ead87efa365800228aa0b09a12d9e6c4Dmitry Shmidt * See README for more details. 78d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt */ 88d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 98d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt#include "includes.h" 108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt#include "common.h" 128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt#include "common/ieee802_11_defs.h" 131f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt#include "common/gas.h" 148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt#include "p2p_i.h" 158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt#include "p2p.h" 168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1861d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#ifdef CONFIG_WIFI_DISPLAY 1961d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidtstatic int wfd_wsd_supported(struct wpabuf *wfd) 2061d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt{ 2161d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt const u8 *pos, *end; 2261d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt u8 subelem; 2361d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt u16 len; 2461d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 2561d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt if (wfd == NULL) 2661d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt return 0; 2761d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 2861d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt pos = wpabuf_head(wfd); 2961d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt end = pos + wpabuf_len(wfd); 3061d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 31d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt while (end - pos >= 3) { 3261d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt subelem = *pos++; 3361d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt len = WPA_GET_BE16(pos); 3461d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt pos += 2; 35d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (len > end - pos) 3661d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt break; 3761d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 3861d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt if (subelem == WFD_SUBELEM_DEVICE_INFO && len >= 6) { 3961d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt u16 info = WPA_GET_BE16(pos); 4061d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt return !!(info & 0x0040); 4161d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt } 4261d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 4361d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt pos += len; 4461d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt } 4561d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 4661d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt return 0; 4761d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt} 4861d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#endif /* CONFIG_WIFI_DISPLAY */ 4961d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstruct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p, 518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_device *dev) 528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q; 5461d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt int wsd = 0; 5513ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt int count = 0; 568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY)) 581f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt return NULL; /* peer does not support SD */ 5961d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#ifdef CONFIG_WIFI_DISPLAY 6061d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt if (wfd_wsd_supported(dev->info.wfd_subelems)) 6161d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt wsd = 1; 6261d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#endif /* CONFIG_WIFI_DISPLAY */ 638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt for (q = p2p->sd_queries; q; q = q->next) { 6561d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt /* Use WSD only if the peer indicates support or it */ 6661d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt if (q->wsd && !wsd) 6761d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt continue; 6813ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt /* if the query is a broadcast query */ 6913ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (q->for_all_peers) { 7013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt /* 7113ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt * check if there are any broadcast queries pending for 7213ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt * this device 7313ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt */ 7413ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (dev->sd_pending_bcast_queries <= 0) 7513ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt return NULL; 7613ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt /* query number that needs to be send to the device */ 7713ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (count == dev->sd_pending_bcast_queries - 1) 78fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidt goto found; 7913ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt count++; 8013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt } 818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (!q->for_all_peers && 828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcmp(q->peer, dev->info.p2p_device_addr, ETH_ALEN) == 838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 0) 84fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidt goto found; 858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 88fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidt 89fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidtfound: 90fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidt if (dev->sd_reqs > 100) { 91fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidt p2p_dbg(p2p, "Too many SD request attempts to " MACSTR 92fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidt " - skip remaining queries", 93fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidt MAC2STR(dev->info.p2p_device_addr)); 94fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidt return NULL; 95fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidt } 96fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidt return q; 978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 10013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidtstatic void p2p_decrease_sd_bc_queries(struct p2p_data *p2p, int query_number) 10113ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt{ 10213ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt struct p2p_device *dev; 10313ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 10413ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt p2p->num_p2p_sd_queries--; 10513ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) { 10613ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (query_number <= dev->sd_pending_bcast_queries - 1) { 10713ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt /* 10813ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt * Query not yet sent to the device and it is to be 10913ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt * removed, so update the pending count. 11013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt */ 11113ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dev->sd_pending_bcast_queries--; 11213ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt } 11313ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt } 11413ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt} 11513ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 11613ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 1178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic int p2p_unlink_sd_query(struct p2p_data *p2p, 1188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *query) 1198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q, *prev; 12113ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt int query_number = 0; 12213ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 1238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_queries; 1248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev = NULL; 1258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt while (q) { 1268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q == query) { 12713ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt /* If the query is a broadcast query, decrease one from 12813ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt * all the devices */ 12913ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (query->for_all_peers) 13013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt p2p_decrease_sd_bc_queries(p2p, query_number); 1318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (prev) 1328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev->next = q->next; 1338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt else 1348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_queries = q->next; 1358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_query == query) 1368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = NULL; 1378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return 1; 1388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 13913ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (q->for_all_peers) 14013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt query_number++; 1418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev = q; 1428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = q->next; 1438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 1448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return 0; 1458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic void p2p_free_sd_query(struct p2p_sd_query *q) 1498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q == NULL) 1518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 1528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(q->tlvs); 1538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_free(q); 1548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_free_sd_queries(struct p2p_data *p2p) 1588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q, *prev; 1608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_queries; 1618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_queries = NULL; 1628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt while (q) { 1638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev = q; 1648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = q->next; 1658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(prev); 1668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 16713ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt p2p->num_p2p_sd_queries = 0; 1688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic struct wpabuf * p2p_build_sd_query(u16 update_indic, 1728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *tlvs) 1738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *buf; 1751f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt u8 *len_pos; 1768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1771f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs)); 1788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (buf == NULL) 1798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 1808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1811f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Request Frame */ 1821f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); 183a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); 1848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */ 1858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_buf(buf, tlvs); 1861f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_element_len(buf, len_pos); 1878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1881f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_len(buf); 1898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return buf; 1918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst, 1958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token, int freq) 1968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *req; 1988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1991f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt req = gas_build_comeback_req(dialog_token); 2008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (req == NULL) 2018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 2028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_NO_PENDING_ACTION; 2048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst, 2058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(req), wpabuf_len(req), 200) < 0) 206b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Failed to send Action frame"); 2078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(req); 2098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 2108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code, 2138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 comeback_delay, 2148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic, 2158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const struct wpabuf *tlvs) 2168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 2178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *buf; 2181f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt u8 *len_pos; 2198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2201f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt buf = gas_anqp_build_initial_resp(dialog_token, status_code, 2211f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt comeback_delay, 2221f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt 100 + (tlvs ? wpabuf_len(tlvs) : 0)); 2238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (buf == NULL) 2248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 2258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (tlvs) { 2271f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response Frame */ 2281f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); 229a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); 2308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Service Update Indicator */ 2318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, update_indic); 2328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_buf(buf, tlvs); 2331f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_element_len(buf, len_pos); 2348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 2358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2361f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_len(buf); 2378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return buf; 2398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 2408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token, 2438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 status_code, 2448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic, 2458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, 2468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 frag_id, u8 more, 2478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 total_len) 2488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 2498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *buf; 2508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2511f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id, 2521f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt more, 0, 100 + len); 2538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (buf == NULL) 2548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 2558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (frag_id == 0) { 2571f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response Frame */ 2581f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */ 2598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, 3 + 1 + 2 + total_len); 260a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); 2618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Service Update Indicator */ 2628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, update_indic); 2638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 2648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_data(buf, data, len); 2661f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_len(buf); 2678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return buf; 2698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 2708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtint p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev) 2738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 2748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *req; 2758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int ret = 0; 2768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *query; 2778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int freq; 278623d63a3a443027e50efdaaec027befcc3882527Dmitry Shmidt unsigned int wait_time; 2798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq; 2818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (freq <= 0) { 282b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "No Listen/Operating frequency known for the peer " 283b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt MACSTR " to send SD Request", 2848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(dev->info.p2p_device_addr)); 2858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 2868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 2878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt query = p2p_pending_sd_req(p2p, dev); 2898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (query == NULL) 2908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 291de47be75037bccd4a11b62eedb3d4aed1b36fa67Dmitry Shmidt if (p2p->state == P2P_SEARCH && 292de47be75037bccd4a11b62eedb3d4aed1b36fa67Dmitry Shmidt os_memcmp(p2p->sd_query_no_ack, dev->info.p2p_device_addr, 293de47be75037bccd4a11b62eedb3d4aed1b36fa67Dmitry Shmidt ETH_ALEN) == 0) { 294de47be75037bccd4a11b62eedb3d4aed1b36fa67Dmitry Shmidt p2p_dbg(p2p, "Do not start Service Discovery with " MACSTR 295de47be75037bccd4a11b62eedb3d4aed1b36fa67Dmitry Shmidt " due to it being the first no-ACK peer in this search iteration", 296de47be75037bccd4a11b62eedb3d4aed1b36fa67Dmitry Shmidt MAC2STR(dev->info.p2p_device_addr)); 297de47be75037bccd4a11b62eedb3d4aed1b36fa67Dmitry Shmidt return -2; 298de47be75037bccd4a11b62eedb3d4aed1b36fa67Dmitry Shmidt } 2998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 300b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Start Service Discovery with " MACSTR, 3018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(dev->info.p2p_device_addr)); 3028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs); 3048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (req == NULL) 3058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 3068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 307fb45fd5cfed8bdccd0859c7fc05449fc187e2d06Dmitry Shmidt dev->sd_reqs++; 3088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer = dev; 3098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = query; 3108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_PENDING_SD; 3118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 312623d63a3a443027e50efdaaec027befcc3882527Dmitry Shmidt wait_time = 5000; 313623d63a3a443027e50efdaaec027befcc3882527Dmitry Shmidt if (p2p->cfg->max_listen && wait_time > p2p->cfg->max_listen) 314623d63a3a443027e50efdaaec027befcc3882527Dmitry Shmidt wait_time = p2p->cfg->max_listen; 3158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr, 3168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->dev_addr, dev->info.p2p_device_addr, 317623d63a3a443027e50efdaaec027befcc3882527Dmitry Shmidt wpabuf_head(req), wpabuf_len(req), wait_time) < 0) { 318b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Failed to send Action frame"); 3198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt ret = -1; 3208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(req); 3238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return ret; 3258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 3268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa, 3298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 3308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 3318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *pos = data; 3328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *end = data + len; 3338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *next; 3348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 3358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 slen; 3368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int freq; 3378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic; 3388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->cfg->sd_request == NULL) 3418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (rx_freq > 0) 3448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt freq = rx_freq; 3458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt else 3464b06059785b935dd1f4f09314e4e12c417d2c6a4Dmitry Shmidt freq = p2p_channel_to_freq(p2p->cfg->reg_class, 3478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->channel); 3488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (freq < 0) 3498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 1 + 2) 3528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *pos++; 355b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "GAS Initial Request from " MACSTR 356b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt " (dialog token %u, freq %d)", 3578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(sa), dialog_token, rx_freq); 3588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != WLAN_EID_ADV_PROTO) { 360b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unexpected IE in GAS Initial Request: %u", *pos); 3618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 3648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = *pos++; 366d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (slen > end - pos || slen < 2) { 367b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid IE in GAS Initial Request"); 3688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 370d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt next = pos + slen; 3718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; /* skip QueryRespLenLimit and PAME-BI */ 3728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3731f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 374b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u", 3758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 3768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos = next; 3808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Query Request */ 381d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (end - pos < 2) 3828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 3848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 385d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (slen > end - pos) 3868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt end = pos + slen; 3888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3891f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Request */ 390d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (end - pos < 4) 3918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3921f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 393b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 3948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 3978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 3998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 400d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (slen > end - pos || slen < 3 + 1) { 401b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid ANQP Query Request length"); 4028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 4048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 405a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) { 406a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x", 407a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt WPA_GET_BE32(pos)); 4088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 410a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt pos += 4; 4118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 412d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (end - pos < 2) 4138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt update_indic = WPA_GET_LE16(pos); 415b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Service Update Indicator: %u", update_indic); 4168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 4178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token, 4198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt update_indic, pos, end - pos); 4208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* the response will be indicated with a call to p2p_sd_response() */ 4218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 4228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst, 4258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token, const struct wpabuf *resp_tlvs) 4268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 4278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *resp; 42831a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt size_t max_len; 429293335998d38c497293b1c41f7ad8342b507d458Dmitry Shmidt unsigned int wait_time = 200; 43031a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt 43131a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt /* 43231a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt * In the 60 GHz, we have a smaller maximum frame length for management 43331a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt * frames. 43431a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt */ 43531a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt max_len = (freq > 56160) ? 928 : 1400; 4368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: fix the length limit to match with the maximum frame length */ 43831a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt if (wpabuf_len(resp_tlvs) > max_len) { 439b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "SD response long enough to require fragmentation"); 4408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_resp) { 4418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* 4428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * TODO: Could consider storing the fragmented response 4438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * separately for each peer to avoid having to drop old 4448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * one if there is more than one pending SD query. 4458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * Though, that would eat more memory, so there are 4468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * also benefits to just using a single buffer. 4478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt */ 448b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Drop previous SD response"); 4498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_resp); 4508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 45104949598a23f501be6eec21697465fd46a28840aDmitry Shmidt p2p->sd_resp = wpabuf_dup(resp_tlvs); 45204949598a23f501be6eec21697465fd46a28840aDmitry Shmidt if (p2p->sd_resp == NULL) { 453b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_err(p2p, "Failed to allocate SD response fragmentation area"); 45404949598a23f501be6eec21697465fd46a28840aDmitry Shmidt return; 45504949598a23f501be6eec21697465fd46a28840aDmitry Shmidt } 4568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN); 4578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_dialog_token = dialog_token; 4588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_pos = 0; 4598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id = 0; 4608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS, 4618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1, p2p->srv_update_indic, NULL); 4628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } else { 463b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "SD response fits in initial response"); 464293335998d38c497293b1c41f7ad8342b507d458Dmitry Shmidt wait_time = 0; /* no more SD frames in the sequence */ 4658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt resp = p2p_build_sd_response(dialog_token, 4668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt WLAN_STATUS_SUCCESS, 0, 4678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->srv_update_indic, resp_tlvs); 4688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 4698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (resp == NULL) 4708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_NO_PENDING_ACTION; 4738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, 4748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->dev_addr, 475293335998d38c497293b1c41f7ad8342b507d458Dmitry Shmidt wpabuf_head(resp), wpabuf_len(resp), wait_time) < 0) 476b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Failed to send Action frame"); 4778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(resp); 4798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 4808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa, 4838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 4848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 4858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *pos = data; 4868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *end = data + len; 4878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *next; 4888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 4898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 status_code; 4908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 comeback_delay; 4918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 slen; 4928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic; 4938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || 4958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { 496b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from " 4978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MACSTR, MAC2STR(sa)); 4988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->send_action_done(p2p->cfg->cb_ctx); 5018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_clear_timeout(p2p); 5028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 503b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)", 5048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(sa), (int) len); 5058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 5 + 2) { 507b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Too short GAS Initial Response frame"); 5088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *pos++; 5128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: check dialog_token match */ 5138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code = WPA_GET_LE16(pos); 5148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 5158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt comeback_delay = WPA_GET_LE16(pos); 5168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 517b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u", 5188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token, status_code, comeback_delay); 5198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (status_code) { 520b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Service Discovery failed: status code %u", 5218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code); 5228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != WLAN_EID_ADV_PROTO) { 526b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos); 5278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 5308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = *pos++; 532d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (slen > end - pos || slen < 2) { 533b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid IE in GAS Initial Response"); 5348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 536d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt next = pos + slen; 5378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; /* skip QueryRespLenLimit and PAME-BI */ 5388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5391f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 540b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u", 5418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 5428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos = next; 5468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Query Response */ 547d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (end - pos < 2) { 548b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Too short Query Response"); 5498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 5528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 553b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Query Response Length: %d", slen); 554d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (slen > end - pos) { 555b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Not enough Query Response data"); 5568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt end = pos + slen; 5598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (comeback_delay) { 561b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Fragmented response - request fragments"); 5628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_rx_resp) { 563b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Drop old SD reassembly buffer"); 5648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_rx_resp); 5658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_resp = NULL; 5668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq); 5688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5711f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response */ 572d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (end - pos < 4) 5738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5741f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 575b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 5768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 5798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 5818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 582d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (slen > end - pos || slen < 3 + 1) { 583b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid ANQP Query Response length"); 5848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 587a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) { 588a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x", 589a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt WPA_GET_BE32(pos)); 5908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 592a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt pos += 4; 5938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 594d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (end - pos < 2) 5958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt update_indic = WPA_GET_LE16(pos); 597b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Service Update Indicator: %u", update_indic); 5988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 5998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer = NULL; 6018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_query) { 6038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (!p2p->sd_query->for_all_peers) { 6048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q; 605b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Remove completed SD query %p", 6068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query); 6078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_query; 6088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_unlink_sd_query(p2p, p2p->sd_query); 6098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(q); 6108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = NULL; 6128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->cfg->sd_response) 6158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic, 6168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos, end - pos); 6178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_continue_find(p2p); 6188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 6198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa, 6228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 6238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 6248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *resp; 6258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 62631a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt size_t frag_len, max_len; 6278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int more = 0; 6288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len); 6308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 1) 6318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *data; 633b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Dialog Token: %u", dialog_token); 6348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (dialog_token != p2p->sd_resp_dialog_token) { 635b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "No pending SD response fragment for dialog token %u", 636b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt dialog_token); 6378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_resp == NULL) { 641b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "No pending SD response fragment available"); 6428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) { 645b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "No pending SD response fragment for " MACSTR, 646b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt MAC2STR(sa)); 6478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 65031a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt /* 65131a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt * In the 60 GHz, we have a smaller maximum frame length for management 65231a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt * frames. 65331a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt */ 65431a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt max_len = (rx_freq > 56160) ? 928 : 1400; 6558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos; 65631a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt if (frag_len > max_len) { 65731a29cc7ed87b62465c7e01f03484f4643d12309Dmitry Shmidt frag_len = max_len; 6588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt more = 1; 6598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS, 6618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->srv_update_indic, 6628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head_u8(p2p->sd_resp) + 6638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_pos, frag_len, 6648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id, more, 6658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_len(p2p->sd_resp)); 6668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (resp == NULL) 6678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 668b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Send GAS Comeback Response (frag_id %d more=%d frag_len=%d)", 6698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id, more, (int) frag_len); 6708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id++; 6718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_pos += frag_len; 6728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (more) { 674b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "%d more bytes remain to be sent", 6758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos)); 6768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } else { 677b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "All fragments of SD response sent"); 6788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_resp); 6798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp = NULL; 6808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_NO_PENDING_ACTION; 6838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr, 6848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->dev_addr, 6858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(resp), wpabuf_len(resp), 200) < 0) 686b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Failed to send Action frame"); 6878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(resp); 6898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 6908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa, 6938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 6948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 6958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *pos = data; 6968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *end = data + len; 6978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *next; 6988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 6998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 status_code; 7008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 frag_id; 7018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 more_frags; 7028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 comeback_delay; 7038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 slen; 7048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len); 7068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || 7088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { 709b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from " 7108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MACSTR, MAC2STR(sa)); 7118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->send_action_done(p2p->cfg->cb_ctx); 7148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_clear_timeout(p2p); 7158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 716b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)", 7178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(sa), (int) len); 7188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 6 + 2) { 720b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Too short GAS Comeback Response frame"); 7218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *pos++; 7258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: check dialog_token match */ 7268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code = WPA_GET_LE16(pos); 7278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 7288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt frag_id = *pos & 0x7f; 7298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt more_frags = (*pos & 0x80) >> 7; 7308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 7318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt comeback_delay = WPA_GET_LE16(pos); 7328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 733b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d " 7348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "comeback_delay=%u", 7358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token, status_code, frag_id, more_frags, 7368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt comeback_delay); 7378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: check frag_id match */ 7388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (status_code) { 739b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Service Discovery failed: status code %u", 7408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code); 7418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != WLAN_EID_ADV_PROTO) { 745b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u", 7468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 7478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 7508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = *pos++; 752d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (slen > end - pos || slen < 2) { 753b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid IE in GAS Comeback Response"); 7548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 756d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt next = pos + slen; 7578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; /* skip QueryRespLenLimit and PAME-BI */ 7588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7591f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 760b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u", 7618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 7628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos = next; 7668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Query Response */ 767d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (end - pos < 2) { 768b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Too short Query Response"); 7698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 7728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 773b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Query Response Length: %d", slen); 774d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (slen > end - pos) { 775b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Not enough Query Response data"); 7768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (slen == 0) { 779b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "No Query Response data"); 7808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt end = pos + slen; 7838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_rx_resp) { 7858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* 7861f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt * ANQP header is only included in the first fragment; rest of 7878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * the fragments start with continue TLVs. 7888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt */ 7898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt goto skip_nqp_header; 7908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7921f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response */ 793d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (end - pos < 4) 7948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7951f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 796b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 7978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 8008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 8028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 803b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "ANQP Query Response length: %u", slen); 8048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (slen < 3 + 1) { 805b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid ANQP Query Response length"); 8068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 808d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (end - pos < 4) 8098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 811a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) { 812a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x", 813a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt WPA_GET_BE32(pos)); 8148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 816a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt pos += 4; 8178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 818d80a401aed31d06f261efd19223cf55d1a2a8228Dmitry Shmidt if (end - pos < 2) 8198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_update_indic = WPA_GET_LE16(pos); 821b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic); 8228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 8238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtskip_nqp_header: 8258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0) 8268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos); 828b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Current SD reassembly buffer length: %u", 8298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt (unsigned int) wpabuf_len(p2p->sd_rx_resp)); 8308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (more_frags) { 832b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "More fragments remains"); 8338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: what would be a good size limit? */ 8348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (wpabuf_len(p2p->sd_rx_resp) > 64000) { 8358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_rx_resp); 8368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_resp = NULL; 837b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Too long SD response - drop it"); 8388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq); 8418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer = NULL; 8458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_query) { 8478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (!p2p->sd_query->for_all_peers) { 8488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q; 849b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Remove completed SD query %p", 8508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query); 8518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_query; 8528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_unlink_sd_query(p2p, p2p->sd_query); 8538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(q); 8548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = NULL; 8568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->cfg->sd_response) 8598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, 8608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_update_indic, 8618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(p2p->sd_rx_resp), 8628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_len(p2p->sd_rx_resp)); 8638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_rx_resp); 8648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_resp = NULL; 8658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_continue_find(p2p); 8678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 8688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid * p2p_sd_request(struct p2p_data *p2p, const u8 *dst, 8718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const struct wpabuf *tlvs) 8728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 8738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q; 8748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = os_zalloc(sizeof(*q)); 8768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q == NULL) 8778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 8788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (dst) 8808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcpy(q->peer, dst, ETH_ALEN); 8818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt else 8828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q->for_all_peers = 1; 8838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q->tlvs = wpabuf_dup(tlvs); 8858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q->tlvs == NULL) { 8868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(q); 8878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 8888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q->next = p2p->sd_queries; 8918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_queries = q; 892b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Added SD Query %p", q); 8938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 89496161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff if (dst == NULL) { 89596161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff struct p2p_device *dev; 89613ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 89713ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt p2p->num_p2p_sd_queries++; 89813ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 89913ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt /* Update all the devices for the newly added broadcast query */ 90013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) { 90113ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (dev->sd_pending_bcast_queries <= 0) 90213ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dev->sd_pending_bcast_queries = 1; 90313ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt else 90413ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dev->sd_pending_bcast_queries++; 90513ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt } 90696161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff } 90796161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff 9088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return q; 9098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 9108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 91261d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#ifdef CONFIG_WIFI_DISPLAY 91361d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidtvoid * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst, 91461d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt const struct wpabuf *tlvs) 91561d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt{ 91661d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt struct p2p_sd_query *q; 91761d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt q = p2p_sd_request(p2p, dst, tlvs); 91861d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt if (q) 91961d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt q->wsd = 1; 92061d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt return q; 92161d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt} 92261d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#endif /* CONFIG_WIFI_DISPLAY */ 92361d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 92461d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 9258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_sd_service_update(struct p2p_data *p2p) 9268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 9278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->srv_update_indic++; 9288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 9298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtint p2p_sd_cancel_request(struct p2p_data *p2p, void *req) 9328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 9338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_unlink_sd_query(p2p, req)) { 934b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Cancel pending SD query %p", req); 9358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(req); 9368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return 0; 9378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 9388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 9398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 940