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; 558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY)) 571f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt return NULL; /* peer does not support SD */ 5861d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#ifdef CONFIG_WIFI_DISPLAY 5961d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt if (wfd_wsd_supported(dev->info.wfd_subelems)) 6061d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt wsd = 1; 6161d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#endif /* CONFIG_WIFI_DISPLAY */ 628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt for (q = p2p->sd_queries; q; q = q->next) { 6461d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt /* Use WSD only if the peer indicates support or it */ 6561d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt if (q->wsd && !wsd) 6661d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt continue; 678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO)) 688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return q; 698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (!q->for_all_peers && 708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcmp(q->peer, dev->info.p2p_device_addr, ETH_ALEN) == 718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 0) 728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return q; 738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic int p2p_unlink_sd_query(struct p2p_data *p2p, 808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *query) 818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q, *prev; 838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_queries; 848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev = NULL; 858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt while (q) { 868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q == query) { 878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (prev) 888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev->next = q->next; 898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt else 908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_queries = q->next; 918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_query == query) 928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = NULL; 938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return 1; 948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev = q; 968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = q->next; 978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return 0; 998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic void p2p_free_sd_query(struct p2p_sd_query *q) 1038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q == NULL) 1058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 1068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(q->tlvs); 1078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_free(q); 1088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_free_sd_queries(struct p2p_data *p2p) 1128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q, *prev; 1148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_queries; 1158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_queries = NULL; 1168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt while (q) { 1178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt prev = q; 1188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = q->next; 1198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(prev); 1208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 1218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic struct wpabuf * p2p_build_sd_query(u16 update_indic, 1258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *tlvs) 1268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *buf; 1281f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt u8 *len_pos; 1298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1301f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs)); 1318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (buf == NULL) 1328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 1338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1341f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Request Frame */ 1351f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); 1368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_be24(buf, OUI_WFA); 1378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_u8(buf, P2P_OUI_TYPE); 1388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */ 1398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_buf(buf, tlvs); 1401f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_element_len(buf, len_pos); 1418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1421f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_len(buf); 1438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return buf; 1458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst, 1498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token, int freq) 1508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *req; 1528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1531f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt req = gas_build_comeback_req(dialog_token); 1548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (req == NULL) 1558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 1568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_NO_PENDING_ACTION; 1588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst, 1598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(req), wpabuf_len(req), 200) < 0) 1608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 1618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Failed to send Action frame"); 1628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(req); 1648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code, 1688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 comeback_delay, 1698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic, 1708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const struct wpabuf *tlvs) 1718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 1728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *buf; 1731f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt u8 *len_pos; 1748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1751f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt buf = gas_anqp_build_initial_resp(dialog_token, status_code, 1761f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt comeback_delay, 1771f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt 100 + (tlvs ? wpabuf_len(tlvs) : 0)); 1788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (buf == NULL) 1798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 1808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (tlvs) { 1821f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response Frame */ 1831f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); 1848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_be24(buf, OUI_WFA); 1858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_u8(buf, P2P_OUI_TYPE); 1868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Service Update Indicator */ 1878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, update_indic); 1888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_buf(buf, tlvs); 1891f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_element_len(buf, len_pos); 1908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 1918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1921f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_len(buf); 1938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return buf; 1958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtstatic struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token, 1998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 status_code, 2008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic, 2018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, 2028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 frag_id, u8 more, 2038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 total_len) 2048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 2058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *buf; 2068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2071f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id, 2081f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt more, 0, 100 + len); 2098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (buf == NULL) 2108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 2118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (frag_id == 0) { 2131f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response Frame */ 2141f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */ 2158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, 3 + 1 + 2 + total_len); 2168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_be24(buf, OUI_WFA); 2178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_u8(buf, P2P_OUI_TYPE); 2188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Service Update Indicator */ 2198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_le16(buf, update_indic); 2208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 2218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_data(buf, data, len); 2231f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt gas_anqp_set_len(buf); 2248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return buf; 2268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 2278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtint p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev) 2308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 2318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *req; 2328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int ret = 0; 2338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *query; 2348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int freq; 2358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq; 2378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (freq <= 0) { 2388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 2398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: No Listen/Operating frequency known for the " 2408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "peer " MACSTR " to send SD Request", 2418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(dev->info.p2p_device_addr)); 2428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 2438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 2448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt query = p2p_pending_sd_req(p2p, dev); 2468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (query == NULL) 2478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 2488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 2508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Start Service Discovery with " MACSTR, 2518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(dev->info.p2p_device_addr)); 2528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs); 2548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (req == NULL) 2558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 2568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer = dev; 2588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = query; 2598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_PENDING_SD; 2608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr, 2628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->dev_addr, dev->info.p2p_device_addr, 2638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(req), wpabuf_len(req), 5000) < 0) { 2648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 2658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Failed to send Action frame"); 2668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt ret = -1; 2678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 2688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(req); 2708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return ret; 2728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 2738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa, 2768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 2778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 2788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *pos = data; 2798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *end = data + len; 2808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *next; 2818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 2828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 slen; 2838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int freq; 2848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic; 2858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->cfg->sd_request == NULL) 2888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 2898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (rx_freq > 0) 2918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt freq = rx_freq; 2928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt else 2938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt freq = p2p_channel_to_freq(p2p->cfg->country, 2948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->reg_class, 2958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->channel); 2968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (freq < 0) 2978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 2988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 2998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 1 + 2) 3008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *pos++; 3038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 3048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: GAS Initial Request from " MACSTR " (dialog token %u, " 3058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "freq %d)", 3068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(sa), dialog_token, rx_freq); 3078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != WLAN_EID_ADV_PROTO) { 3098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 3108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Unexpected IE in GAS Initial Request: %u", *pos); 3118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 3148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = *pos++; 3168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt next = pos + slen; 3178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (next > end || slen < 2) { 3188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 3198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Invalid IE in GAS Initial Request"); 3208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; /* skip QueryRespLenLimit and PAME-BI */ 3238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3241f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 3258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 3268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Unsupported GAS advertisement protocol id %u", 3278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 3288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos = next; 3328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Query Request */ 3338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) 3348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 3368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 3378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + slen > end) 3388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt end = pos + slen; 3408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3411f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Request */ 3428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 4 > end) 3438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3441f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 3458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 3461f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 3478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 3508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 3528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 3538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + slen > end || slen < 3 + 1) { 3548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 3551f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Invalid ANQP Query Request length"); 3568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (WPA_GET_BE24(pos) != OUI_WFA) { 3608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 3611f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos)); 3628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 3; 3658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != P2P_OUI_TYPE) { 3678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 3681f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Unsupported ANQP vendor type %u", *pos); 3698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 3718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 3728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) 3748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 3758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt update_indic = WPA_GET_LE16(pos); 3768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 3778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Service Update Indicator: %u", update_indic); 3788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 3798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token, 3818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt update_indic, pos, end - pos); 3828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* the response will be indicated with a call to p2p_sd_response() */ 3838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 3848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst, 3878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token, const struct wpabuf *resp_tlvs) 3888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 3898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *resp; 3908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 3918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: fix the length limit to match with the maximum frame length */ 3928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (wpabuf_len(resp_tlvs) > 1400) { 3938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response long " 3948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "enough to require fragmentation"); 3958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_resp) { 3968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* 3978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * TODO: Could consider storing the fragmented response 3988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * separately for each peer to avoid having to drop old 3998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * one if there is more than one pending SD query. 4008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * Though, that would eat more memory, so there are 4018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * also benefits to just using a single buffer. 4028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt */ 4038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop " 4048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "previous SD response"); 4058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_resp); 4068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 40704949598a23f501be6eec21697465fd46a28840aDmitry Shmidt p2p->sd_resp = wpabuf_dup(resp_tlvs); 40804949598a23f501be6eec21697465fd46a28840aDmitry Shmidt if (p2p->sd_resp == NULL) { 40904949598a23f501be6eec21697465fd46a28840aDmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_ERROR, "P2P: Failed to " 41004949598a23f501be6eec21697465fd46a28840aDmitry Shmidt "allocate SD response fragmentation area"); 41104949598a23f501be6eec21697465fd46a28840aDmitry Shmidt return; 41204949598a23f501be6eec21697465fd46a28840aDmitry Shmidt } 4138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN); 4148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_dialog_token = dialog_token; 4158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_pos = 0; 4168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id = 0; 4178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS, 4188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 1, p2p->srv_update_indic, NULL); 4198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } else { 4208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response fits " 4218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "in initial response"); 4228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt resp = p2p_build_sd_response(dialog_token, 4238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt WLAN_STATUS_SUCCESS, 0, 4248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->srv_update_indic, resp_tlvs); 4258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 4268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (resp == NULL) 4278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_NO_PENDING_ACTION; 4308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, 4318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->dev_addr, 4328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(resp), wpabuf_len(resp), 200) < 0) 4338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 4348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Failed to send Action frame"); 4358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(resp); 4378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 4388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa, 4418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 4428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 4438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *pos = data; 4448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *end = data + len; 4458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *next; 4468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 4478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 status_code; 4488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 comeback_delay; 4498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 slen; 4508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 update_indic; 4518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 45204949598a23f501be6eec21697465fd46a28840aDmitry Shmidt#ifdef ANDROID_P2P 45304949598a23f501be6eec21697465fd46a28840aDmitry Shmidt if (p2p->state != P2P_SD_DURING_FIND) { 45404949598a23f501be6eec21697465fd46a28840aDmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 45504949598a23f501be6eec21697465fd46a28840aDmitry Shmidt "P2P: #### Not ignoring unexpected GAS Initial Response from " 45604949598a23f501be6eec21697465fd46a28840aDmitry Shmidt MACSTR " state %d", MAC2STR(sa), p2p->state); 45704949598a23f501be6eec21697465fd46a28840aDmitry Shmidt } 45804949598a23f501be6eec21697465fd46a28840aDmitry Shmidt if (p2p->sd_peer == NULL || 45904949598a23f501be6eec21697465fd46a28840aDmitry Shmidt#else 4608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || 46104949598a23f501be6eec21697465fd46a28840aDmitry Shmidt#endif 4628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { 46361d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 4648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Ignore unexpected GAS Initial Response from " 4658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MACSTR, MAC2STR(sa)); 4668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 4688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->send_action_done(p2p->cfg->cb_ctx); 4698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_clear_timeout(p2p); 4708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 4728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Received GAS Initial Response from " MACSTR " (len=%d)", 4738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(sa), (int) len); 4748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 5 + 2) { 4768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 4778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Too short GAS Initial Response frame"); 4788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 4808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *pos++; 4828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: check dialog_token match */ 4838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code = WPA_GET_LE16(pos); 4848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 4858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt comeback_delay = WPA_GET_LE16(pos); 4868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 4878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 4888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: dialog_token=%u status_code=%u comeback_delay=%u", 4898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token, status_code, comeback_delay); 4908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (status_code) { 4918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 4928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Service Discovery failed: status code %u", 4938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code); 4948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 4958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 4968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 4978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != WLAN_EID_ADV_PROTO) { 4988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 4998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Unexpected IE in GAS Initial Response: %u", 5008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 5018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 5048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = *pos++; 5068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt next = pos + slen; 5078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (next > end || slen < 2) { 5088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 5098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Invalid IE in GAS Initial Response"); 5108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; /* skip QueryRespLenLimit and PAME-BI */ 5138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5141f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 5158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 5168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Unsupported GAS advertisement protocol id %u", 5178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 5188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos = next; 5228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Query Response */ 5238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) { 5248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query " 5258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "Response"); 5268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 5298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 5308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d", 5318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen); 5328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + slen > end) { 5338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query " 5348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "Response data"); 5358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt end = pos + slen; 5388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (comeback_delay) { 5408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Fragmented " 5418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "response - request fragments"); 5428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_rx_resp) { 5438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop " 5448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "old SD reassembly buffer"); 5458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_rx_resp); 5468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_resp = NULL; 5478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq); 5498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5521f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response */ 5538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 4 > end) 5548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5551f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 5568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 5571f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 5588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 5618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 5638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 5648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + slen > end || slen < 3 + 1) { 5658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 5661f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Invalid ANQP Query Response length"); 5678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (WPA_GET_BE24(pos) != OUI_WFA) { 5718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 5721f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos)); 5738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 3; 5768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != P2P_OUI_TYPE) { 5788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 5791f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Unsupported ANQP vendor type %u", *pos); 5808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 5828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 5838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) 5858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 5868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt update_indic = WPA_GET_LE16(pos); 5878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 5888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Service Update Indicator: %u", update_indic); 5898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 5908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer->flags |= P2P_DEV_SD_INFO; 5928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE; 5938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer = NULL; 5948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 5958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_query) { 5968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (!p2p->sd_query->for_all_peers) { 5978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q; 5988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 5998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Remove completed SD query %p", 6008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query); 6018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_query; 6028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_unlink_sd_query(p2p, p2p->sd_query); 6038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(q); 6048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = NULL; 6068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6088d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->cfg->sd_response) 6098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic, 6108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos, end - pos); 6118d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_continue_find(p2p); 6128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 6138d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa, 6168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 6178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 6188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct wpabuf *resp; 6198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 6208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt size_t frag_len; 6218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt int more = 0; 6228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len); 6248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 1) 6258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *data; 6278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Dialog Token: %u", 6288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token); 6298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (dialog_token != p2p->sd_resp_dialog_token) { 6308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD " 6318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "response fragment for dialog token %u", dialog_token); 6328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_resp == NULL) { 6368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD " 6378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "response fragment available"); 6388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) { 6418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD " 6428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "response fragment for " MACSTR, MAC2STR(sa)); 6438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos; 6478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (frag_len > 1400) { 6488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt frag_len = 1400; 6498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt more = 1; 6508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS, 6528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->srv_update_indic, 6538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head_u8(p2p->sd_resp) + 6548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_pos, frag_len, 6558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id, more, 6568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_len(p2p->sd_resp)); 6578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (resp == NULL) 6588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 6598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Send GAS Comeback " 6608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "Response (frag_id %d more=%d frag_len=%d)", 6618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id, more, (int) frag_len); 6628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_frag_id++; 6638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp_pos += frag_len; 6648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (more) { 6668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: %d more bytes " 6678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "remain to be sent", 6688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos)); 6698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } else { 6708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: All fragments of " 6718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "SD response sent"); 6728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_resp); 6738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_resp = NULL; 6748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 6758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->pending_action_state = P2P_NO_PENDING_ACTION; 6778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr, 6788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->dev_addr, 6798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(resp), wpabuf_len(resp), 200) < 0) 6808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 6818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Failed to send Action frame"); 6828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(resp); 6848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 6858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 6878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa, 6888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *data, size_t len, int rx_freq) 6898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 6908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *pos = data; 6918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *end = data + len; 6928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const u8 *next; 6938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 dialog_token; 6948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 status_code; 6958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 frag_id; 6968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u8 more_frags; 6978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 comeback_delay; 6988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt u16 slen; 6998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len); 7018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 70204949598a23f501be6eec21697465fd46a28840aDmitry Shmidt#ifdef ANDROID_P2P 70304949598a23f501be6eec21697465fd46a28840aDmitry Shmidt if (p2p->state != P2P_SD_DURING_FIND) { 70404949598a23f501be6eec21697465fd46a28840aDmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 70504949598a23f501be6eec21697465fd46a28840aDmitry Shmidt "P2P: #### Not ignoring unexpected GAS Comeback Response from " 70604949598a23f501be6eec21697465fd46a28840aDmitry Shmidt MACSTR " state %d", MAC2STR(sa), p2p->state); 70704949598a23f501be6eec21697465fd46a28840aDmitry Shmidt } 70804949598a23f501be6eec21697465fd46a28840aDmitry Shmidt if (p2p->sd_peer == NULL || 70904949598a23f501be6eec21697465fd46a28840aDmitry Shmidt#else 7108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || 71104949598a23f501be6eec21697465fd46a28840aDmitry Shmidt#endif 7128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { 71361d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 7148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Ignore unexpected GAS Comeback Response from " 7158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MACSTR, MAC2STR(sa)); 7168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->send_action_done(p2p->cfg->cb_ctx); 7198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_clear_timeout(p2p); 7208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 7228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Received GAS Comeback Response from " MACSTR " (len=%d)", 7238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt MAC2STR(sa), (int) len); 7248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (len < 6 + 2) { 7268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 7278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Too short GAS Comeback Response frame"); 7288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token = *pos++; 7328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: check dialog_token match */ 7338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code = WPA_GET_LE16(pos); 7348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 7358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt frag_id = *pos & 0x7f; 7368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt more_frags = (*pos & 0x80) >> 7; 7378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 7388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt comeback_delay = WPA_GET_LE16(pos); 7398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 7408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 7418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: dialog_token=%u status_code=%u frag_id=%d more_frags=%d " 7428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "comeback_delay=%u", 7438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt dialog_token, status_code, frag_id, more_frags, 7448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt comeback_delay); 7458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: check frag_id match */ 7468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (status_code) { 7478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 7488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Service Discovery failed: status code %u", 7498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt status_code); 7508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != WLAN_EID_ADV_PROTO) { 7548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 7558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Unexpected IE in GAS Comeback Response: %u", 7568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 7578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 7608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = *pos++; 7628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt next = pos + slen; 7638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (next > end || slen < 2) { 7648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 7658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Invalid IE in GAS Comeback Response"); 7668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; /* skip QueryRespLenLimit and PAME-BI */ 7698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7701f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 7718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 7728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Unsupported GAS advertisement protocol id %u", 7738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt *pos); 7748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 7778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos = next; 7788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* Query Response */ 7798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) { 7808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query " 7818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "Response"); 7828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 7858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 7868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d", 7878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen); 7888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + slen > end) { 7898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query " 7908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "Response data"); 7918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (slen == 0) { 7948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No Query Response " 7958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "data"); 7968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 7978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 7988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt end = pos + slen; 7998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_rx_resp) { 8018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* 8021f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt * ANQP header is only included in the first fragment; rest of 8038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt * the fragments start with continue TLVs. 8048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt */ 8058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt goto skip_nqp_header; 8068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8078d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8081f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt /* ANQP Query Response */ 8098d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 4 > end) 8108d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8111f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 8128d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 8131f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 8148d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8158d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8168d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 8178d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8188d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt slen = WPA_GET_LE16(pos); 8198d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 8201f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: ANQP Query Response " 8218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "length: %u", slen); 8228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (slen < 3 + 1) { 8238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 8241f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Invalid ANQP Query Response length"); 8258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 4 > end) 8288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (WPA_GET_BE24(pos) != OUI_WFA) { 8318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 8321f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos)); 8338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 3; 8368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (*pos != P2P_OUI_TYPE) { 8388d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 8391f69aa52ea2e0a73ac502565df8c666ee49cab6aDmitry Shmidt "P2P: Unsupported ANQP vendor type %u", *pos); 8408d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8418d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8428d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos++; 8438d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8448d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (pos + 2 > end) 8458d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_update_indic = WPA_GET_LE16(pos); 8478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 8488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Service Update Indicator: %u", p2p->sd_rx_update_indic); 8498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt pos += 2; 8508d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8518d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtskip_nqp_header: 8528d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0) 8538d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8548d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos); 8558d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Current SD reassembly " 8568d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "buffer length: %u", 8578d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt (unsigned int) wpabuf_len(p2p->sd_rx_resp)); 8588d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8598d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (more_frags) { 8608d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: More fragments " 8618d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "remains"); 8628d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt /* TODO: what would be a good size limit? */ 8638d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (wpabuf_len(p2p->sd_rx_resp) > 64000) { 8648d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_rx_resp); 8658d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_resp = NULL; 8668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too long " 8678d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "SD response - drop it"); 8688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8708d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq); 8718d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return; 8728d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8738d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8748d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer->flags |= P2P_DEV_SD_INFO; 8758d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE; 8768d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_peer = NULL; 8778d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8788d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->sd_query) { 8798d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (!p2p->sd_query->for_all_peers) { 8808d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q; 8818d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 8828d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Remove completed SD query %p", 8838d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query); 8848d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = p2p->sd_query; 8858d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_unlink_sd_query(p2p, p2p->sd_query); 8868d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(q); 8878d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8888d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_query = NULL; 8898d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 8908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p->cfg->sd_response) 8928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, 8938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_update_indic, 8948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_head(p2p->sd_rx_resp), 8958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_len(p2p->sd_rx_resp)); 8968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpabuf_free(p2p->sd_rx_resp); 8978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_rx_resp = NULL; 8988d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 8998d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_continue_find(p2p); 9008d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 9018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid * p2p_sd_request(struct p2p_data *p2p, const u8 *dst, 9048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt const struct wpabuf *tlvs) 9058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 9068d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt struct p2p_sd_query *q; 907f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff#ifdef ANDROID_P2P 90804949598a23f501be6eec21697465fd46a28840aDmitry Shmidt /* Currently, supplicant doesn't support more than one pending broadcast SD request. 909f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff * So reject if application is registering another one before cancelling the existing one. 910f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff */ 911f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff for (q = p2p->sd_queries; q; q = q->next) { 912f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff if( (q->for_all_peers == 1) && (!dst)) { 913f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff wpa_printf(MSG_ERROR, "P2P: Already one pending" 914f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff " Broadcast request. Please cancel the current one" 915f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff " before adding a new one"); 916f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff return NULL; 917f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff } 918f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff } 919f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff#endif 9208d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9218d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q = os_zalloc(sizeof(*q)); 9228d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q == NULL) 9238d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 9248d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9258d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (dst) 9268d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt os_memcpy(q->peer, dst, ETH_ALEN); 9278d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt else 9288d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q->for_all_peers = 1; 9298d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9308d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q->tlvs = wpabuf_dup(tlvs); 9318d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (q->tlvs == NULL) { 9328d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(q); 9338d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return NULL; 9348d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 9358d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9368d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt q->next = p2p->sd_queries; 9378d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->sd_queries = q; 93804949598a23f501be6eec21697465fd46a28840aDmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p", q); 9398d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 94096161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff if (dst == NULL) { 94196161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff struct p2p_device *dev; 94296161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) 94396161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff dev->flags &= ~P2P_DEV_SD_INFO; 94496161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff } 94596161876d932a9e995465e9186471cb1b7f80979Irfan Sheriff 9468d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return q; 9478d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 9488d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9498d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 95061d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#ifdef CONFIG_WIFI_DISPLAY 95161d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidtvoid * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst, 95261d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt const struct wpabuf *tlvs) 95361d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt{ 95461d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt struct p2p_sd_query *q; 95561d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt q = p2p_sd_request(p2p, dst, tlvs); 95661d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt if (q) 95761d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt q->wsd = 1; 95861d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt return q; 95961d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt} 96061d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt#endif /* CONFIG_WIFI_DISPLAY */ 96161d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 96261d9df3e62aaa0e87ad05452fcb95142159a17b6Dmitry Shmidt 963b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt#ifdef ANDROID_P2P 964b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidtvoid p2p_sd_service_update(struct p2p_data *p2p, int action) 965b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt#else 9668d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtvoid p2p_sd_service_update(struct p2p_data *p2p) 967b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt#endif 9688d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 9698d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p->srv_update_indic++; 970b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt#ifdef ANDROID_P2P 971b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt if(action == SRV_FLUSH) 972b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt p2p->srv_count = 0; 973b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt else if (action == SRV_DEL) 974b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt p2p->srv_count--; 975b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt else if (action == SRV_ADD) 976b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt p2p->srv_count++; 977b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt 978b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt if(p2p->cfg->sd_request) { 979b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt if (p2p->srv_count == 1) { 980b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt /* First Service Registered. Enable SD capability */ 981b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt p2p->dev_capab |= P2P_DEV_CAPAB_SERVICE_DISCOVERY; 982b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt } else if (p2p->srv_count == 0 && !p2p->sd_queries) { 983b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt /* No services remaining + No queries registered . 984b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt * Remove the SD Capability 985b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt */ 986b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt p2p->dev_capab &= ~P2P_DEV_CAPAB_SERVICE_DISCOVERY; 987b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt } 988b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt } 989b5e8f06e18446918f6d801566e5709a8c87f1780Dmitry Shmidt#endif 9908d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 9918d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9928d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt 9938d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidtint p2p_sd_cancel_request(struct p2p_data *p2p, void *req) 9948d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt{ 9958d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt if (p2p_unlink_sd_query(p2p, req)) { 9968d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 9978d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt "P2P: Cancel pending SD query %p", req); 998f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff#ifdef ANDROID_P2P 99904949598a23f501be6eec21697465fd46a28840aDmitry Shmidt p2p->sd_dev_list = NULL; 1000f44b9c4a18d17fbd39901f76a014c32006570fb8Irfan Sheriff#endif 10018d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt p2p_free_sd_query(req); 10028d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return 0; 10038d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt } 10048d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt return -1; 10058d520ff1dc2da35cdca849e982051b86468016d8Dmitry Shmidt} 1006