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 3161d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt while (pos + 3 <= end) { 3261d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt subelem = *pos++; 3361d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt len = WPA_GET_BE16(pos); 3461d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt pos += 2; 3561d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt if (pos + len > end) 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) 7813ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt return q; 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) 848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return q; 858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9113ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidtstatic void p2p_decrease_sd_bc_queries(struct p2p_data *p2p, int query_number) 9213ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt{ 9313ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt struct p2p_device *dev; 9413ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 9513ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt p2p->num_p2p_sd_queries--; 9613ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) { 9713ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (query_number <= dev->sd_pending_bcast_queries - 1) { 9813ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt /* 9913ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt * Query not yet sent to the device and it is to be 10013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt * removed, so update the pending count. 10113ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt */ 10213ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dev->sd_pending_bcast_queries--; 10313ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt } 10413ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt } 10513ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt} 10613ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 10713ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 1088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic int p2p_unlink_sd_query(struct p2p_data *p2p, 1098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *query) 1108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q, *prev; 11213ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt int query_number = 0; 11313ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 1148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_queries; 1158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev = NULL; 1168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt while (q) { 1178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q == query) { 11813ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt /* If the query is a broadcast query, decrease one from 11913ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt * all the devices */ 12013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (query->for_all_peers) 12113ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt p2p_decrease_sd_bc_queries(p2p, query_number); 1228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (prev) 1238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev->next = q->next; 1248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt else 1258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_queries = q->next; 1268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_query == query) 1278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = NULL; 1288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return 1; 1298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 13013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (q->for_all_peers) 13113ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt query_number++; 1328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev = q; 1338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = q->next; 1348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 1358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return 0; 1368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic void p2p_free_sd_query(struct p2p_sd_query *q) 1408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q == NULL) 1428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 1438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(q->tlvs); 1448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_free(q); 1458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_free_sd_queries(struct p2p_data *p2p) 1498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q, *prev; 1518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_queries; 1528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_queries = NULL; 1538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt while (q) { 1548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev = q; 1558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = q->next; 1568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(prev); 1578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 15813ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt p2p->num_p2p_sd_queries = 0; 1598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic struct wpabuf * p2p_build_sd_query(u16 update_indic, 1638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *tlvs) 1648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *buf; 1661f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt u8 *len_pos; 1678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1681f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs)); 1698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (buf == NULL) 1708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 1718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1721f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Request Frame */ 1731f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); 174a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); 1758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */ 1768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_buf(buf, tlvs); 1771f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_element_len(buf, len_pos); 1788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1791f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_len(buf); 1808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return buf; 1828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst, 1868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token, int freq) 1878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *req; 1898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1901f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt req = gas_build_comeback_req(dialog_token); 1918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (req == NULL) 1928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 1938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_NO_PENDING_ACTION; 1958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst, 1968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(req), wpabuf_len(req), 200) < 0) 197b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Failed to send Action frame"); 1988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(req); 2008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 2018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code, 2048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 comeback_delay, 2058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic, 2068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const struct wpabuf *tlvs) 2078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 2088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *buf; 2091f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt u8 *len_pos; 2108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2111f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt buf = gas_anqp_build_initial_resp(dialog_token, status_code, 2121f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt comeback_delay, 2131f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt 100 + (tlvs ? wpabuf_len(tlvs) : 0)); 2148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (buf == NULL) 2158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 2168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (tlvs) { 2181f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response Frame */ 2191f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); 220a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); 2218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Service Update Indicator */ 2228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, update_indic); 2238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_buf(buf, tlvs); 2241f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_element_len(buf, len_pos); 2258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 2268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2271f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_len(buf); 2288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return buf; 2308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 2318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token, 2348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 status_code, 2358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic, 2368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, 2378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 frag_id, u8 more, 2388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 total_len) 2398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 2408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *buf; 2418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2421f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id, 2431f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt more, 0, 100 + len); 2448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (buf == NULL) 2458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 2468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (frag_id == 0) { 2481f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response Frame */ 2491f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */ 2508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, 3 + 1 + 2 + total_len); 251a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); 2528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Service Update Indicator */ 2538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, update_indic); 2548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 2558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_data(buf, data, len); 2571f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_len(buf); 2588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return buf; 2608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 2618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtint p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev) 2648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 2658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *req; 2668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int ret = 0; 2678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *query; 2688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int freq; 269623d63a3a443027e50efdaaec027befcc3882527Dmitry Shmidt unsigned int wait_time; 2708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq; 2728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (freq <= 0) { 273b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "No Listen/Operating frequency known for the peer " 274b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt MACSTR " to send SD Request", 2758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(dev->info.p2p_device_addr)); 2768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 2778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 2788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt query = p2p_pending_sd_req(p2p, dev); 2808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (query == NULL) 2818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 2828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 283b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Start Service Discovery with " MACSTR, 2848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(dev->info.p2p_device_addr)); 2858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs); 2878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (req == NULL) 2888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 2898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer = dev; 2918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = query; 2928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_PENDING_SD; 2938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 294623d63a3a443027e50efdaaec027befcc3882527Dmitry Shmidt wait_time = 5000; 295623d63a3a443027e50efdaaec027befcc3882527Dmitry Shmidt if (p2p->cfg->max_listen && wait_time > p2p->cfg->max_listen) 296623d63a3a443027e50efdaaec027befcc3882527Dmitry Shmidt wait_time = p2p->cfg->max_listen; 2978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr, 2988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->dev_addr, dev->info.p2p_device_addr, 299623d63a3a443027e50efdaaec027befcc3882527Dmitry Shmidt wpabuf_head(req), wpabuf_len(req), wait_time) < 0) { 300b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Failed to send Action frame"); 3018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt ret = -1; 3028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 30413ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt /* Update the pending broadcast SD query count for this device */ 30513ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dev->sd_pending_bcast_queries--; 30613ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 30713ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt /* 30813ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt * If there are no pending broadcast queries for this device, mark it as 30913ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt * done (-1). 31013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt */ 31113ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (dev->sd_pending_bcast_queries == 0) 31213ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dev->sd_pending_bcast_queries = -1; 31313ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 3148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(req); 3158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return ret; 3178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 3188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa, 3218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 3228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 3238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *pos = data; 3248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *end = data + len; 3258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *next; 3268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 3278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 slen; 3288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int freq; 3298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic; 3308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->cfg->sd_request == NULL) 3338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (rx_freq > 0) 3368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt freq = rx_freq; 3378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt else 3384b06059785b935dd1f4f09314e4e12c417d2c6a4Dmitry Shmidt freq = p2p_channel_to_freq(p2p->cfg->reg_class, 3398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->channel); 3408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (freq < 0) 3418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 1 + 2) 3448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *pos++; 347b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "GAS Initial Request from " MACSTR 348b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt " (dialog token %u, freq %d)", 3498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(sa), dialog_token, rx_freq); 3508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != WLAN_EID_ADV_PROTO) { 352b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unexpected IE in GAS Initial Request: %u", *pos); 3538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 3568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = *pos++; 3588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt next = pos + slen; 3598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (next > end || slen < 2) { 360b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid IE in GAS Initial Request"); 3618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; /* skip QueryRespLenLimit and PAME-BI */ 3648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3651f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 366b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u", 3678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 3688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos = next; 3728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Query Request */ 3738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) 3748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 3768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 3778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + slen > end) 3788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt end = pos + slen; 3808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3811f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Request */ 3828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 4 > end) 3838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3841f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 385b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 3868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 3898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 3918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 3928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + slen > end || slen < 3 + 1) { 393b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid ANQP Query Request length"); 3948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 397a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) { 398a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x", 399a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt WPA_GET_BE32(pos)); 4008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 402a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt pos += 4; 4038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) 4058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt update_indic = WPA_GET_LE16(pos); 407b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Service Update Indicator: %u", update_indic); 4088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 4098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token, 4118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt update_indic, pos, end - pos); 4128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* the response will be indicated with a call to p2p_sd_response() */ 4138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 4148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst, 4178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token, const struct wpabuf *resp_tlvs) 4188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 4198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *resp; 4208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: fix the length limit to match with the maximum frame length */ 4228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (wpabuf_len(resp_tlvs) > 1400) { 423b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "SD response long enough to require fragmentation"); 4248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_resp) { 4258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* 4268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * TODO: Could consider storing the fragmented response 4278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * separately for each peer to avoid having to drop old 4288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * one if there is more than one pending SD query. 4298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * Though, that would eat more memory, so there are 4308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * also benefits to just using a single buffer. 4318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt */ 432b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Drop previous SD response"); 4338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_resp); 4348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 43504949598a23f501be6eec21697465fd46a28840aDmitry Shmidt p2p->sd_resp = wpabuf_dup(resp_tlvs); 43604949598a23f501be6eec21697465fd46a28840aDmitry Shmidt if (p2p->sd_resp == NULL) { 437b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_err(p2p, "Failed to allocate SD response fragmentation area"); 43804949598a23f501be6eec21697465fd46a28840aDmitry Shmidt return; 43904949598a23f501be6eec21697465fd46a28840aDmitry Shmidt } 4408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN); 4418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_dialog_token = dialog_token; 4428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_pos = 0; 4438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id = 0; 4448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS, 4458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1, p2p->srv_update_indic, NULL); 4468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } else { 447b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "SD response fits in initial response"); 4488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt resp = p2p_build_sd_response(dialog_token, 4498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt WLAN_STATUS_SUCCESS, 0, 4508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->srv_update_indic, resp_tlvs); 4518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 4528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (resp == NULL) 4538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_NO_PENDING_ACTION; 4568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, 4578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->dev_addr, 4588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(resp), wpabuf_len(resp), 200) < 0) 459b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Failed to send Action frame"); 4608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(resp); 4628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 4638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa, 4668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 4678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 4688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *pos = data; 4698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *end = data + len; 4708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *next; 4718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 4728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 status_code; 4738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 comeback_delay; 4748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 slen; 4758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic; 4768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || 4788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { 479b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from " 4808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MACSTR, MAC2STR(sa)); 4818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 4838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->send_action_done(p2p->cfg->cb_ctx); 4848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_clear_timeout(p2p); 4858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 486b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)", 4878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(sa), (int) len); 4888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 5 + 2) { 490b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Too short GAS Initial Response frame"); 4918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 4938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *pos++; 4958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: check dialog_token match */ 4968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code = WPA_GET_LE16(pos); 4978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 4988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt comeback_delay = WPA_GET_LE16(pos); 4998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 500b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u", 5018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token, status_code, comeback_delay); 5028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (status_code) { 503b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Service Discovery failed: status code %u", 5048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code); 5058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != WLAN_EID_ADV_PROTO) { 509b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos); 5108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 5138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = *pos++; 5158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt next = pos + slen; 5168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (next > end || slen < 2) { 517b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid IE in GAS Initial Response"); 5188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; /* skip QueryRespLenLimit and PAME-BI */ 5218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5221f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 523b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u", 5248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 5258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos = next; 5298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Query Response */ 5308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) { 531b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Too short Query Response"); 5328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 5358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 536b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Query Response Length: %d", slen); 5378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + slen > end) { 538b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Not enough Query Response data"); 5398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt end = pos + slen; 5428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (comeback_delay) { 544b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Fragmented response - request fragments"); 5458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_rx_resp) { 546b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Drop old SD reassembly buffer"); 5478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_rx_resp); 5488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_resp = NULL; 5498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq); 5518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5541f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response */ 5558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 4 > end) 5568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5571f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 558b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 5598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 5628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 5648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 5658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + slen > end || slen < 3 + 1) { 566b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid ANQP Query Response length"); 5678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 570a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) { 571a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x", 572a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt WPA_GET_BE32(pos)); 5738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 575a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt pos += 4; 5768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) 5788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt update_indic = WPA_GET_LE16(pos); 580b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Service Update Indicator: %u", update_indic); 5818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 5828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer = NULL; 5848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_query) { 5868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (!p2p->sd_query->for_all_peers) { 5878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q; 588b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Remove completed SD query %p", 5898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query); 5908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_query; 5918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_unlink_sd_query(p2p, p2p->sd_query); 5928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(q); 5938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = NULL; 5958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->cfg->sd_response) 5988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic, 5998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos, end - pos); 6008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_continue_find(p2p); 6018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 6028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa, 6058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 6068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 6078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *resp; 6088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 6098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt size_t frag_len; 6108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int more = 0; 6118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len); 6138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 1) 6148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *data; 616b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Dialog Token: %u", dialog_token); 6178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (dialog_token != p2p->sd_resp_dialog_token) { 618b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "No pending SD response fragment for dialog token %u", 619b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt dialog_token); 6208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_resp == NULL) { 624b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "No pending SD response fragment available"); 6258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) { 628b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "No pending SD response fragment for " MACSTR, 629b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt MAC2STR(sa)); 6308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos; 6348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (frag_len > 1400) { 6358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt frag_len = 1400; 6368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt more = 1; 6378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS, 6398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->srv_update_indic, 6408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head_u8(p2p->sd_resp) + 6418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_pos, frag_len, 6428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id, more, 6438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_len(p2p->sd_resp)); 6448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (resp == NULL) 6458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 646b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Send GAS Comeback Response (frag_id %d more=%d frag_len=%d)", 6478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id, more, (int) frag_len); 6488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id++; 6498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_pos += frag_len; 6508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (more) { 652b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "%d more bytes remain to be sent", 6538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos)); 6548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } else { 655b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "All fragments of SD response sent"); 6568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_resp); 6578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp = NULL; 6588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_NO_PENDING_ACTION; 6618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr, 6628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->dev_addr, 6638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(resp), wpabuf_len(resp), 200) < 0) 664b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Failed to send Action frame"); 6658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(resp); 6678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 6688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa, 6718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 6728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 6738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *pos = data; 6748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *end = data + len; 6758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *next; 6768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 6778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 status_code; 6788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 frag_id; 6798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 more_frags; 6808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 comeback_delay; 6818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 slen; 6828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len); 6848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || 6868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { 687b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from " 6888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MACSTR, MAC2STR(sa)); 6898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->send_action_done(p2p->cfg->cb_ctx); 6928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_clear_timeout(p2p); 6938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 694b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)", 6958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(sa), (int) len); 6968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 6 + 2) { 698b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Too short GAS Comeback Response frame"); 6998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *pos++; 7038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: check dialog_token match */ 7048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code = WPA_GET_LE16(pos); 7058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 7068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt frag_id = *pos & 0x7f; 7078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt more_frags = (*pos & 0x80) >> 7; 7088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 7098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt comeback_delay = WPA_GET_LE16(pos); 7108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 711b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d " 7128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "comeback_delay=%u", 7138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token, status_code, frag_id, more_frags, 7148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt comeback_delay); 7158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: check frag_id match */ 7168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (status_code) { 717b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Service Discovery failed: status code %u", 7188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code); 7198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != WLAN_EID_ADV_PROTO) { 723b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u", 7248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 7258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 7288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = *pos++; 7308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt next = pos + slen; 7318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (next > end || slen < 2) { 732b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid IE in GAS Comeback Response"); 7338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; /* skip QueryRespLenLimit and PAME-BI */ 7368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7371f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 738b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u", 7398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 7408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos = next; 7448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Query Response */ 7458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) { 746b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Too short Query Response"); 7478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 7508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 751b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Query Response Length: %d", slen); 7528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + slen > end) { 753b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Not enough Query Response data"); 7548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (slen == 0) { 757b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "No Query Response data"); 7588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt end = pos + slen; 7618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_rx_resp) { 7638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* 7641f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt * ANQP header is only included in the first fragment; rest of 7658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * the fragments start with continue TLVs. 7668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt */ 7678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt goto skip_nqp_header; 7688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7701f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response */ 7718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 4 > end) 7728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7731f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 774b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 7758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 7788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 7808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 781b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "ANQP Query Response length: %u", slen); 7828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (slen < 3 + 1) { 783b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Invalid ANQP Query Response length"); 7848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 4 > end) 7878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 789a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) { 790a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x", 791a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt WPA_GET_BE32(pos)); 7928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 794a38abf9af7bec7e89dbfb39ac7bb77223fe47c72Dmitry Shmidt pos += 4; 7958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) 7978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_update_indic = WPA_GET_LE16(pos); 799b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic); 8008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 8018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtskip_nqp_header: 8038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0) 8048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos); 806b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Current SD reassembly buffer length: %u", 8078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt (unsigned int) wpabuf_len(p2p->sd_rx_resp)); 8088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (more_frags) { 810b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "More fragments remains"); 8118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: what would be a good size limit? */ 8128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (wpabuf_len(p2p->sd_rx_resp) > 64000) { 8138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_rx_resp); 8148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_resp = NULL; 815b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Too long SD response - drop it"); 8168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq); 8198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer = NULL; 8238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_query) { 8258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (!p2p->sd_query->for_all_peers) { 8268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q; 827b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Remove completed SD query %p", 8288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query); 8298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_query; 8308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_unlink_sd_query(p2p, p2p->sd_query); 8318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(q); 8328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = NULL; 8348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->cfg->sd_response) 8378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, 8388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_update_indic, 8398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(p2p->sd_rx_resp), 8408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_len(p2p->sd_rx_resp)); 8418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_rx_resp); 8428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_resp = NULL; 8438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_continue_find(p2p); 8458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 8468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid * p2p_sd_request(struct p2p_data *p2p, const u8 *dst, 8498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const struct wpabuf *tlvs) 8508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 8518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q; 8528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = os_zalloc(sizeof(*q)); 8548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q == NULL) 8558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 8568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (dst) 8588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcpy(q->peer, dst, ETH_ALEN); 8598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt else 8608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q->for_all_peers = 1; 8618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q->tlvs = wpabuf_dup(tlvs); 8638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q->tlvs == NULL) { 8648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(q); 8658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 8668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q->next = p2p->sd_queries; 8698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_queries = q; 870b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Added SD Query %p", q); 8718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 87296161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff if (dst == NULL) { 87396161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff struct p2p_device *dev; 87413ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 87513ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt p2p->num_p2p_sd_queries++; 87613ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt 87713ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt /* Update all the devices for the newly added broadcast query */ 87813ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) { 87913ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt if (dev->sd_pending_bcast_queries <= 0) 88013ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dev->sd_pending_bcast_queries = 1; 88113ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt else 88213ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt dev->sd_pending_bcast_queries++; 88313ca8d8ea51a1aa5e24c6c956473a11b0c7daed4Dmitry Shmidt } 88496161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff } 88596161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff 8868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return q; 8878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 8888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 89061d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#ifdef CONFIG_WIFI_DISPLAY 89161d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidtvoid * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst, 89261d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt const struct wpabuf *tlvs) 89361d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt{ 89461d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt struct p2p_sd_query *q; 89561d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt q = p2p_sd_request(p2p, dst, tlvs); 89661d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt if (q) 89761d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt q->wsd = 1; 89861d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt return q; 89961d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt} 90061d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#endif /* CONFIG_WIFI_DISPLAY */ 90161d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 90261d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 9038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_sd_service_update(struct p2p_data *p2p) 9048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 9058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->srv_update_indic++; 9068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 9078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtint p2p_sd_cancel_request(struct p2p_data *p2p, void *req) 9108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 9118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_unlink_sd_query(p2p, req)) { 912b6e9aaf735990dc64cdb6efccc03d076768eabf3Dmitry Shmidt p2p_dbg(p2p, "Cancel pending SD query %p", req); 9138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(req); 9148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return 0; 9158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 9168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 9178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 918