p2p_sd.c revision 252190
1252190Srpaulo/* 2252190Srpaulo * Wi-Fi Direct - P2P service discovery 3252190Srpaulo * Copyright (c) 2009, Atheros Communications 4252190Srpaulo * 5252190Srpaulo * This software may be distributed under the terms of the BSD license. 6252190Srpaulo * See README for more details. 7252190Srpaulo */ 8252190Srpaulo 9252190Srpaulo#include "includes.h" 10252190Srpaulo 11252190Srpaulo#include "common.h" 12252190Srpaulo#include "common/ieee802_11_defs.h" 13252190Srpaulo#include "common/gas.h" 14252190Srpaulo#include "p2p_i.h" 15252190Srpaulo#include "p2p.h" 16252190Srpaulo 17252190Srpaulo 18252190Srpaulo#ifdef CONFIG_WIFI_DISPLAY 19252190Srpaulostatic int wfd_wsd_supported(struct wpabuf *wfd) 20252190Srpaulo{ 21252190Srpaulo const u8 *pos, *end; 22252190Srpaulo u8 subelem; 23252190Srpaulo u16 len; 24252190Srpaulo 25252190Srpaulo if (wfd == NULL) 26252190Srpaulo return 0; 27252190Srpaulo 28252190Srpaulo pos = wpabuf_head(wfd); 29252190Srpaulo end = pos + wpabuf_len(wfd); 30252190Srpaulo 31252190Srpaulo while (pos + 3 <= end) { 32252190Srpaulo subelem = *pos++; 33252190Srpaulo len = WPA_GET_BE16(pos); 34252190Srpaulo pos += 2; 35252190Srpaulo if (pos + len > end) 36252190Srpaulo break; 37252190Srpaulo 38252190Srpaulo if (subelem == WFD_SUBELEM_DEVICE_INFO && len >= 6) { 39252190Srpaulo u16 info = WPA_GET_BE16(pos); 40252190Srpaulo return !!(info & 0x0040); 41252190Srpaulo } 42252190Srpaulo 43252190Srpaulo pos += len; 44252190Srpaulo } 45252190Srpaulo 46252190Srpaulo return 0; 47252190Srpaulo} 48252190Srpaulo#endif /* CONFIG_WIFI_DISPLAY */ 49252190Srpaulo 50252190Srpaulostruct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p, 51252190Srpaulo struct p2p_device *dev) 52252190Srpaulo{ 53252190Srpaulo struct p2p_sd_query *q; 54252190Srpaulo int wsd = 0; 55252190Srpaulo 56252190Srpaulo if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY)) 57252190Srpaulo return NULL; /* peer does not support SD */ 58252190Srpaulo#ifdef CONFIG_WIFI_DISPLAY 59252190Srpaulo if (wfd_wsd_supported(dev->info.wfd_subelems)) 60252190Srpaulo wsd = 1; 61252190Srpaulo#endif /* CONFIG_WIFI_DISPLAY */ 62252190Srpaulo 63252190Srpaulo for (q = p2p->sd_queries; q; q = q->next) { 64252190Srpaulo /* Use WSD only if the peer indicates support or it */ 65252190Srpaulo if (q->wsd && !wsd) 66252190Srpaulo continue; 67252190Srpaulo if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO)) 68252190Srpaulo return q; 69252190Srpaulo if (!q->for_all_peers && 70252190Srpaulo os_memcmp(q->peer, dev->info.p2p_device_addr, ETH_ALEN) == 71252190Srpaulo 0) 72252190Srpaulo return q; 73252190Srpaulo } 74252190Srpaulo 75252190Srpaulo return NULL; 76252190Srpaulo} 77252190Srpaulo 78252190Srpaulo 79252190Srpaulostatic int p2p_unlink_sd_query(struct p2p_data *p2p, 80252190Srpaulo struct p2p_sd_query *query) 81252190Srpaulo{ 82252190Srpaulo struct p2p_sd_query *q, *prev; 83252190Srpaulo q = p2p->sd_queries; 84252190Srpaulo prev = NULL; 85252190Srpaulo while (q) { 86252190Srpaulo if (q == query) { 87252190Srpaulo if (prev) 88252190Srpaulo prev->next = q->next; 89252190Srpaulo else 90252190Srpaulo p2p->sd_queries = q->next; 91252190Srpaulo if (p2p->sd_query == query) 92252190Srpaulo p2p->sd_query = NULL; 93252190Srpaulo return 1; 94252190Srpaulo } 95252190Srpaulo prev = q; 96252190Srpaulo q = q->next; 97252190Srpaulo } 98252190Srpaulo return 0; 99252190Srpaulo} 100252190Srpaulo 101252190Srpaulo 102252190Srpaulostatic void p2p_free_sd_query(struct p2p_sd_query *q) 103252190Srpaulo{ 104252190Srpaulo if (q == NULL) 105252190Srpaulo return; 106252190Srpaulo wpabuf_free(q->tlvs); 107252190Srpaulo os_free(q); 108252190Srpaulo} 109252190Srpaulo 110252190Srpaulo 111252190Srpaulovoid p2p_free_sd_queries(struct p2p_data *p2p) 112252190Srpaulo{ 113252190Srpaulo struct p2p_sd_query *q, *prev; 114252190Srpaulo q = p2p->sd_queries; 115252190Srpaulo p2p->sd_queries = NULL; 116252190Srpaulo while (q) { 117252190Srpaulo prev = q; 118252190Srpaulo q = q->next; 119252190Srpaulo p2p_free_sd_query(prev); 120252190Srpaulo } 121252190Srpaulo} 122252190Srpaulo 123252190Srpaulo 124252190Srpaulostatic struct wpabuf * p2p_build_sd_query(u16 update_indic, 125252190Srpaulo struct wpabuf *tlvs) 126252190Srpaulo{ 127252190Srpaulo struct wpabuf *buf; 128252190Srpaulo u8 *len_pos; 129252190Srpaulo 130252190Srpaulo buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs)); 131252190Srpaulo if (buf == NULL) 132252190Srpaulo return NULL; 133252190Srpaulo 134252190Srpaulo /* ANQP Query Request Frame */ 135252190Srpaulo len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); 136252190Srpaulo wpabuf_put_be24(buf, OUI_WFA); 137252190Srpaulo wpabuf_put_u8(buf, P2P_OUI_TYPE); 138252190Srpaulo wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */ 139252190Srpaulo wpabuf_put_buf(buf, tlvs); 140252190Srpaulo gas_anqp_set_element_len(buf, len_pos); 141252190Srpaulo 142252190Srpaulo gas_anqp_set_len(buf); 143252190Srpaulo 144252190Srpaulo return buf; 145252190Srpaulo} 146252190Srpaulo 147252190Srpaulo 148252190Srpaulostatic void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst, 149252190Srpaulo u8 dialog_token, int freq) 150252190Srpaulo{ 151252190Srpaulo struct wpabuf *req; 152252190Srpaulo 153252190Srpaulo req = gas_build_comeback_req(dialog_token); 154252190Srpaulo if (req == NULL) 155252190Srpaulo return; 156252190Srpaulo 157252190Srpaulo p2p->pending_action_state = P2P_NO_PENDING_ACTION; 158252190Srpaulo if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst, 159252190Srpaulo wpabuf_head(req), wpabuf_len(req), 200) < 0) 160252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 161252190Srpaulo "P2P: Failed to send Action frame"); 162252190Srpaulo 163252190Srpaulo wpabuf_free(req); 164252190Srpaulo} 165252190Srpaulo 166252190Srpaulo 167252190Srpaulostatic struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code, 168252190Srpaulo u16 comeback_delay, 169252190Srpaulo u16 update_indic, 170252190Srpaulo const struct wpabuf *tlvs) 171252190Srpaulo{ 172252190Srpaulo struct wpabuf *buf; 173252190Srpaulo u8 *len_pos; 174252190Srpaulo 175252190Srpaulo buf = gas_anqp_build_initial_resp(dialog_token, status_code, 176252190Srpaulo comeback_delay, 177252190Srpaulo 100 + (tlvs ? wpabuf_len(tlvs) : 0)); 178252190Srpaulo if (buf == NULL) 179252190Srpaulo return NULL; 180252190Srpaulo 181252190Srpaulo if (tlvs) { 182252190Srpaulo /* ANQP Query Response Frame */ 183252190Srpaulo len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); 184252190Srpaulo wpabuf_put_be24(buf, OUI_WFA); 185252190Srpaulo wpabuf_put_u8(buf, P2P_OUI_TYPE); 186252190Srpaulo /* Service Update Indicator */ 187252190Srpaulo wpabuf_put_le16(buf, update_indic); 188252190Srpaulo wpabuf_put_buf(buf, tlvs); 189252190Srpaulo gas_anqp_set_element_len(buf, len_pos); 190252190Srpaulo } 191252190Srpaulo 192252190Srpaulo gas_anqp_set_len(buf); 193252190Srpaulo 194252190Srpaulo return buf; 195252190Srpaulo} 196252190Srpaulo 197252190Srpaulo 198252190Srpaulostatic struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token, 199252190Srpaulo u16 status_code, 200252190Srpaulo u16 update_indic, 201252190Srpaulo const u8 *data, size_t len, 202252190Srpaulo u8 frag_id, u8 more, 203252190Srpaulo u16 total_len) 204252190Srpaulo{ 205252190Srpaulo struct wpabuf *buf; 206252190Srpaulo 207252190Srpaulo buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id, 208252190Srpaulo more, 0, 100 + len); 209252190Srpaulo if (buf == NULL) 210252190Srpaulo return NULL; 211252190Srpaulo 212252190Srpaulo if (frag_id == 0) { 213252190Srpaulo /* ANQP Query Response Frame */ 214252190Srpaulo wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */ 215252190Srpaulo wpabuf_put_le16(buf, 3 + 1 + 2 + total_len); 216252190Srpaulo wpabuf_put_be24(buf, OUI_WFA); 217252190Srpaulo wpabuf_put_u8(buf, P2P_OUI_TYPE); 218252190Srpaulo /* Service Update Indicator */ 219252190Srpaulo wpabuf_put_le16(buf, update_indic); 220252190Srpaulo } 221252190Srpaulo 222252190Srpaulo wpabuf_put_data(buf, data, len); 223252190Srpaulo gas_anqp_set_len(buf); 224252190Srpaulo 225252190Srpaulo return buf; 226252190Srpaulo} 227252190Srpaulo 228252190Srpaulo 229252190Srpauloint p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev) 230252190Srpaulo{ 231252190Srpaulo struct wpabuf *req; 232252190Srpaulo int ret = 0; 233252190Srpaulo struct p2p_sd_query *query; 234252190Srpaulo int freq; 235252190Srpaulo 236252190Srpaulo freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq; 237252190Srpaulo if (freq <= 0) { 238252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 239252190Srpaulo "P2P: No Listen/Operating frequency known for the " 240252190Srpaulo "peer " MACSTR " to send SD Request", 241252190Srpaulo MAC2STR(dev->info.p2p_device_addr)); 242252190Srpaulo return -1; 243252190Srpaulo } 244252190Srpaulo 245252190Srpaulo query = p2p_pending_sd_req(p2p, dev); 246252190Srpaulo if (query == NULL) 247252190Srpaulo return -1; 248252190Srpaulo 249252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 250252190Srpaulo "P2P: Start Service Discovery with " MACSTR, 251252190Srpaulo MAC2STR(dev->info.p2p_device_addr)); 252252190Srpaulo 253252190Srpaulo req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs); 254252190Srpaulo if (req == NULL) 255252190Srpaulo return -1; 256252190Srpaulo 257252190Srpaulo p2p->sd_peer = dev; 258252190Srpaulo p2p->sd_query = query; 259252190Srpaulo p2p->pending_action_state = P2P_PENDING_SD; 260252190Srpaulo 261252190Srpaulo if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr, 262252190Srpaulo p2p->cfg->dev_addr, dev->info.p2p_device_addr, 263252190Srpaulo wpabuf_head(req), wpabuf_len(req), 5000) < 0) { 264252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 265252190Srpaulo "P2P: Failed to send Action frame"); 266252190Srpaulo ret = -1; 267252190Srpaulo } 268252190Srpaulo 269252190Srpaulo wpabuf_free(req); 270252190Srpaulo 271252190Srpaulo return ret; 272252190Srpaulo} 273252190Srpaulo 274252190Srpaulo 275252190Srpaulovoid p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa, 276252190Srpaulo const u8 *data, size_t len, int rx_freq) 277252190Srpaulo{ 278252190Srpaulo const u8 *pos = data; 279252190Srpaulo const u8 *end = data + len; 280252190Srpaulo const u8 *next; 281252190Srpaulo u8 dialog_token; 282252190Srpaulo u16 slen; 283252190Srpaulo int freq; 284252190Srpaulo u16 update_indic; 285252190Srpaulo 286252190Srpaulo 287252190Srpaulo if (p2p->cfg->sd_request == NULL) 288252190Srpaulo return; 289252190Srpaulo 290252190Srpaulo if (rx_freq > 0) 291252190Srpaulo freq = rx_freq; 292252190Srpaulo else 293252190Srpaulo freq = p2p_channel_to_freq(p2p->cfg->country, 294252190Srpaulo p2p->cfg->reg_class, 295252190Srpaulo p2p->cfg->channel); 296252190Srpaulo if (freq < 0) 297252190Srpaulo return; 298252190Srpaulo 299252190Srpaulo if (len < 1 + 2) 300252190Srpaulo return; 301252190Srpaulo 302252190Srpaulo dialog_token = *pos++; 303252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 304252190Srpaulo "P2P: GAS Initial Request from " MACSTR " (dialog token %u, " 305252190Srpaulo "freq %d)", 306252190Srpaulo MAC2STR(sa), dialog_token, rx_freq); 307252190Srpaulo 308252190Srpaulo if (*pos != WLAN_EID_ADV_PROTO) { 309252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 310252190Srpaulo "P2P: Unexpected IE in GAS Initial Request: %u", *pos); 311252190Srpaulo return; 312252190Srpaulo } 313252190Srpaulo pos++; 314252190Srpaulo 315252190Srpaulo slen = *pos++; 316252190Srpaulo next = pos + slen; 317252190Srpaulo if (next > end || slen < 2) { 318252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 319252190Srpaulo "P2P: Invalid IE in GAS Initial Request"); 320252190Srpaulo return; 321252190Srpaulo } 322252190Srpaulo pos++; /* skip QueryRespLenLimit and PAME-BI */ 323252190Srpaulo 324252190Srpaulo if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 325252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 326252190Srpaulo "P2P: Unsupported GAS advertisement protocol id %u", 327252190Srpaulo *pos); 328252190Srpaulo return; 329252190Srpaulo } 330252190Srpaulo 331252190Srpaulo pos = next; 332252190Srpaulo /* Query Request */ 333252190Srpaulo if (pos + 2 > end) 334252190Srpaulo return; 335252190Srpaulo slen = WPA_GET_LE16(pos); 336252190Srpaulo pos += 2; 337252190Srpaulo if (pos + slen > end) 338252190Srpaulo return; 339252190Srpaulo end = pos + slen; 340252190Srpaulo 341252190Srpaulo /* ANQP Query Request */ 342252190Srpaulo if (pos + 4 > end) 343252190Srpaulo return; 344252190Srpaulo if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 345252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 346252190Srpaulo "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 347252190Srpaulo return; 348252190Srpaulo } 349252190Srpaulo pos += 2; 350252190Srpaulo 351252190Srpaulo slen = WPA_GET_LE16(pos); 352252190Srpaulo pos += 2; 353252190Srpaulo if (pos + slen > end || slen < 3 + 1) { 354252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 355252190Srpaulo "P2P: Invalid ANQP Query Request length"); 356252190Srpaulo return; 357252190Srpaulo } 358252190Srpaulo 359252190Srpaulo if (WPA_GET_BE24(pos) != OUI_WFA) { 360252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 361252190Srpaulo "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos)); 362252190Srpaulo return; 363252190Srpaulo } 364252190Srpaulo pos += 3; 365252190Srpaulo 366252190Srpaulo if (*pos != P2P_OUI_TYPE) { 367252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 368252190Srpaulo "P2P: Unsupported ANQP vendor type %u", *pos); 369252190Srpaulo return; 370252190Srpaulo } 371252190Srpaulo pos++; 372252190Srpaulo 373252190Srpaulo if (pos + 2 > end) 374252190Srpaulo return; 375252190Srpaulo update_indic = WPA_GET_LE16(pos); 376252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 377252190Srpaulo "P2P: Service Update Indicator: %u", update_indic); 378252190Srpaulo pos += 2; 379252190Srpaulo 380252190Srpaulo p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token, 381252190Srpaulo update_indic, pos, end - pos); 382252190Srpaulo /* the response will be indicated with a call to p2p_sd_response() */ 383252190Srpaulo} 384252190Srpaulo 385252190Srpaulo 386252190Srpaulovoid p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst, 387252190Srpaulo u8 dialog_token, const struct wpabuf *resp_tlvs) 388252190Srpaulo{ 389252190Srpaulo struct wpabuf *resp; 390252190Srpaulo 391252190Srpaulo /* TODO: fix the length limit to match with the maximum frame length */ 392252190Srpaulo if (wpabuf_len(resp_tlvs) > 1400) { 393252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response long " 394252190Srpaulo "enough to require fragmentation"); 395252190Srpaulo if (p2p->sd_resp) { 396252190Srpaulo /* 397252190Srpaulo * TODO: Could consider storing the fragmented response 398252190Srpaulo * separately for each peer to avoid having to drop old 399252190Srpaulo * one if there is more than one pending SD query. 400252190Srpaulo * Though, that would eat more memory, so there are 401252190Srpaulo * also benefits to just using a single buffer. 402252190Srpaulo */ 403252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop " 404252190Srpaulo "previous SD response"); 405252190Srpaulo wpabuf_free(p2p->sd_resp); 406252190Srpaulo } 407252190Srpaulo p2p->sd_resp = wpabuf_dup(resp_tlvs); 408252190Srpaulo if (p2p->sd_resp == NULL) { 409252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_ERROR, "P2P: Failed to " 410252190Srpaulo "allocate SD response fragmentation area"); 411252190Srpaulo return; 412252190Srpaulo } 413252190Srpaulo os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN); 414252190Srpaulo p2p->sd_resp_dialog_token = dialog_token; 415252190Srpaulo p2p->sd_resp_pos = 0; 416252190Srpaulo p2p->sd_frag_id = 0; 417252190Srpaulo resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS, 418252190Srpaulo 1, p2p->srv_update_indic, NULL); 419252190Srpaulo } else { 420252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response fits " 421252190Srpaulo "in initial response"); 422252190Srpaulo resp = p2p_build_sd_response(dialog_token, 423252190Srpaulo WLAN_STATUS_SUCCESS, 0, 424252190Srpaulo p2p->srv_update_indic, resp_tlvs); 425252190Srpaulo } 426252190Srpaulo if (resp == NULL) 427252190Srpaulo return; 428252190Srpaulo 429252190Srpaulo p2p->pending_action_state = P2P_NO_PENDING_ACTION; 430252190Srpaulo if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, 431252190Srpaulo p2p->cfg->dev_addr, 432252190Srpaulo wpabuf_head(resp), wpabuf_len(resp), 200) < 0) 433252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 434252190Srpaulo "P2P: Failed to send Action frame"); 435252190Srpaulo 436252190Srpaulo wpabuf_free(resp); 437252190Srpaulo} 438252190Srpaulo 439252190Srpaulo 440252190Srpaulovoid p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa, 441252190Srpaulo const u8 *data, size_t len, int rx_freq) 442252190Srpaulo{ 443252190Srpaulo const u8 *pos = data; 444252190Srpaulo const u8 *end = data + len; 445252190Srpaulo const u8 *next; 446252190Srpaulo u8 dialog_token; 447252190Srpaulo u16 status_code; 448252190Srpaulo u16 comeback_delay; 449252190Srpaulo u16 slen; 450252190Srpaulo u16 update_indic; 451252190Srpaulo 452252190Srpaulo if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || 453252190Srpaulo os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { 454252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 455252190Srpaulo "P2P: Ignore unexpected GAS Initial Response from " 456252190Srpaulo MACSTR, MAC2STR(sa)); 457252190Srpaulo return; 458252190Srpaulo } 459252190Srpaulo p2p->cfg->send_action_done(p2p->cfg->cb_ctx); 460252190Srpaulo p2p_clear_timeout(p2p); 461252190Srpaulo 462252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 463252190Srpaulo "P2P: Received GAS Initial Response from " MACSTR " (len=%d)", 464252190Srpaulo MAC2STR(sa), (int) len); 465252190Srpaulo 466252190Srpaulo if (len < 5 + 2) { 467252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 468252190Srpaulo "P2P: Too short GAS Initial Response frame"); 469252190Srpaulo return; 470252190Srpaulo } 471252190Srpaulo 472252190Srpaulo dialog_token = *pos++; 473252190Srpaulo /* TODO: check dialog_token match */ 474252190Srpaulo status_code = WPA_GET_LE16(pos); 475252190Srpaulo pos += 2; 476252190Srpaulo comeback_delay = WPA_GET_LE16(pos); 477252190Srpaulo pos += 2; 478252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 479252190Srpaulo "P2P: dialog_token=%u status_code=%u comeback_delay=%u", 480252190Srpaulo dialog_token, status_code, comeback_delay); 481252190Srpaulo if (status_code) { 482252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 483252190Srpaulo "P2P: Service Discovery failed: status code %u", 484252190Srpaulo status_code); 485252190Srpaulo return; 486252190Srpaulo } 487252190Srpaulo 488252190Srpaulo if (*pos != WLAN_EID_ADV_PROTO) { 489252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 490252190Srpaulo "P2P: Unexpected IE in GAS Initial Response: %u", 491252190Srpaulo *pos); 492252190Srpaulo return; 493252190Srpaulo } 494252190Srpaulo pos++; 495252190Srpaulo 496252190Srpaulo slen = *pos++; 497252190Srpaulo next = pos + slen; 498252190Srpaulo if (next > end || slen < 2) { 499252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 500252190Srpaulo "P2P: Invalid IE in GAS Initial Response"); 501252190Srpaulo return; 502252190Srpaulo } 503252190Srpaulo pos++; /* skip QueryRespLenLimit and PAME-BI */ 504252190Srpaulo 505252190Srpaulo if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 506252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 507252190Srpaulo "P2P: Unsupported GAS advertisement protocol id %u", 508252190Srpaulo *pos); 509252190Srpaulo return; 510252190Srpaulo } 511252190Srpaulo 512252190Srpaulo pos = next; 513252190Srpaulo /* Query Response */ 514252190Srpaulo if (pos + 2 > end) { 515252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query " 516252190Srpaulo "Response"); 517252190Srpaulo return; 518252190Srpaulo } 519252190Srpaulo slen = WPA_GET_LE16(pos); 520252190Srpaulo pos += 2; 521252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d", 522252190Srpaulo slen); 523252190Srpaulo if (pos + slen > end) { 524252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query " 525252190Srpaulo "Response data"); 526252190Srpaulo return; 527252190Srpaulo } 528252190Srpaulo end = pos + slen; 529252190Srpaulo 530252190Srpaulo if (comeback_delay) { 531252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Fragmented " 532252190Srpaulo "response - request fragments"); 533252190Srpaulo if (p2p->sd_rx_resp) { 534252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop " 535252190Srpaulo "old SD reassembly buffer"); 536252190Srpaulo wpabuf_free(p2p->sd_rx_resp); 537252190Srpaulo p2p->sd_rx_resp = NULL; 538252190Srpaulo } 539252190Srpaulo p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq); 540252190Srpaulo return; 541252190Srpaulo } 542252190Srpaulo 543252190Srpaulo /* ANQP Query Response */ 544252190Srpaulo if (pos + 4 > end) 545252190Srpaulo return; 546252190Srpaulo if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 547252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 548252190Srpaulo "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 549252190Srpaulo return; 550252190Srpaulo } 551252190Srpaulo pos += 2; 552252190Srpaulo 553252190Srpaulo slen = WPA_GET_LE16(pos); 554252190Srpaulo pos += 2; 555252190Srpaulo if (pos + slen > end || slen < 3 + 1) { 556252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 557252190Srpaulo "P2P: Invalid ANQP Query Response length"); 558252190Srpaulo return; 559252190Srpaulo } 560252190Srpaulo 561252190Srpaulo if (WPA_GET_BE24(pos) != OUI_WFA) { 562252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 563252190Srpaulo "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos)); 564252190Srpaulo return; 565252190Srpaulo } 566252190Srpaulo pos += 3; 567252190Srpaulo 568252190Srpaulo if (*pos != P2P_OUI_TYPE) { 569252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 570252190Srpaulo "P2P: Unsupported ANQP vendor type %u", *pos); 571252190Srpaulo return; 572252190Srpaulo } 573252190Srpaulo pos++; 574252190Srpaulo 575252190Srpaulo if (pos + 2 > end) 576252190Srpaulo return; 577252190Srpaulo update_indic = WPA_GET_LE16(pos); 578252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 579252190Srpaulo "P2P: Service Update Indicator: %u", update_indic); 580252190Srpaulo pos += 2; 581252190Srpaulo 582252190Srpaulo p2p->sd_peer->flags |= P2P_DEV_SD_INFO; 583252190Srpaulo p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE; 584252190Srpaulo p2p->sd_peer = NULL; 585252190Srpaulo 586252190Srpaulo if (p2p->sd_query) { 587252190Srpaulo if (!p2p->sd_query->for_all_peers) { 588252190Srpaulo struct p2p_sd_query *q; 589252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 590252190Srpaulo "P2P: Remove completed SD query %p", 591252190Srpaulo p2p->sd_query); 592252190Srpaulo q = p2p->sd_query; 593252190Srpaulo p2p_unlink_sd_query(p2p, p2p->sd_query); 594252190Srpaulo p2p_free_sd_query(q); 595252190Srpaulo } 596252190Srpaulo p2p->sd_query = NULL; 597252190Srpaulo } 598252190Srpaulo 599252190Srpaulo if (p2p->cfg->sd_response) 600252190Srpaulo p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic, 601252190Srpaulo pos, end - pos); 602252190Srpaulo p2p_continue_find(p2p); 603252190Srpaulo} 604252190Srpaulo 605252190Srpaulo 606252190Srpaulovoid p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa, 607252190Srpaulo const u8 *data, size_t len, int rx_freq) 608252190Srpaulo{ 609252190Srpaulo struct wpabuf *resp; 610252190Srpaulo u8 dialog_token; 611252190Srpaulo size_t frag_len; 612252190Srpaulo int more = 0; 613252190Srpaulo 614252190Srpaulo wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len); 615252190Srpaulo if (len < 1) 616252190Srpaulo return; 617252190Srpaulo dialog_token = *data; 618252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Dialog Token: %u", 619252190Srpaulo dialog_token); 620252190Srpaulo if (dialog_token != p2p->sd_resp_dialog_token) { 621252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD " 622252190Srpaulo "response fragment for dialog token %u", dialog_token); 623252190Srpaulo return; 624252190Srpaulo } 625252190Srpaulo 626252190Srpaulo if (p2p->sd_resp == NULL) { 627252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD " 628252190Srpaulo "response fragment available"); 629252190Srpaulo return; 630252190Srpaulo } 631252190Srpaulo if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) { 632252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD " 633252190Srpaulo "response fragment for " MACSTR, MAC2STR(sa)); 634252190Srpaulo return; 635252190Srpaulo } 636252190Srpaulo 637252190Srpaulo frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos; 638252190Srpaulo if (frag_len > 1400) { 639252190Srpaulo frag_len = 1400; 640252190Srpaulo more = 1; 641252190Srpaulo } 642252190Srpaulo resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS, 643252190Srpaulo p2p->srv_update_indic, 644252190Srpaulo wpabuf_head_u8(p2p->sd_resp) + 645252190Srpaulo p2p->sd_resp_pos, frag_len, 646252190Srpaulo p2p->sd_frag_id, more, 647252190Srpaulo wpabuf_len(p2p->sd_resp)); 648252190Srpaulo if (resp == NULL) 649252190Srpaulo return; 650252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Send GAS Comeback " 651252190Srpaulo "Response (frag_id %d more=%d frag_len=%d)", 652252190Srpaulo p2p->sd_frag_id, more, (int) frag_len); 653252190Srpaulo p2p->sd_frag_id++; 654252190Srpaulo p2p->sd_resp_pos += frag_len; 655252190Srpaulo 656252190Srpaulo if (more) { 657252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: %d more bytes " 658252190Srpaulo "remain to be sent", 659252190Srpaulo (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos)); 660252190Srpaulo } else { 661252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: All fragments of " 662252190Srpaulo "SD response sent"); 663252190Srpaulo wpabuf_free(p2p->sd_resp); 664252190Srpaulo p2p->sd_resp = NULL; 665252190Srpaulo } 666252190Srpaulo 667252190Srpaulo p2p->pending_action_state = P2P_NO_PENDING_ACTION; 668252190Srpaulo if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr, 669252190Srpaulo p2p->cfg->dev_addr, 670252190Srpaulo wpabuf_head(resp), wpabuf_len(resp), 200) < 0) 671252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 672252190Srpaulo "P2P: Failed to send Action frame"); 673252190Srpaulo 674252190Srpaulo wpabuf_free(resp); 675252190Srpaulo} 676252190Srpaulo 677252190Srpaulo 678252190Srpaulovoid p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa, 679252190Srpaulo const u8 *data, size_t len, int rx_freq) 680252190Srpaulo{ 681252190Srpaulo const u8 *pos = data; 682252190Srpaulo const u8 *end = data + len; 683252190Srpaulo const u8 *next; 684252190Srpaulo u8 dialog_token; 685252190Srpaulo u16 status_code; 686252190Srpaulo u8 frag_id; 687252190Srpaulo u8 more_frags; 688252190Srpaulo u16 comeback_delay; 689252190Srpaulo u16 slen; 690252190Srpaulo 691252190Srpaulo wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len); 692252190Srpaulo 693252190Srpaulo if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || 694252190Srpaulo os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { 695252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 696252190Srpaulo "P2P: Ignore unexpected GAS Comeback Response from " 697252190Srpaulo MACSTR, MAC2STR(sa)); 698252190Srpaulo return; 699252190Srpaulo } 700252190Srpaulo p2p->cfg->send_action_done(p2p->cfg->cb_ctx); 701252190Srpaulo p2p_clear_timeout(p2p); 702252190Srpaulo 703252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 704252190Srpaulo "P2P: Received GAS Comeback Response from " MACSTR " (len=%d)", 705252190Srpaulo MAC2STR(sa), (int) len); 706252190Srpaulo 707252190Srpaulo if (len < 6 + 2) { 708252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 709252190Srpaulo "P2P: Too short GAS Comeback Response frame"); 710252190Srpaulo return; 711252190Srpaulo } 712252190Srpaulo 713252190Srpaulo dialog_token = *pos++; 714252190Srpaulo /* TODO: check dialog_token match */ 715252190Srpaulo status_code = WPA_GET_LE16(pos); 716252190Srpaulo pos += 2; 717252190Srpaulo frag_id = *pos & 0x7f; 718252190Srpaulo more_frags = (*pos & 0x80) >> 7; 719252190Srpaulo pos++; 720252190Srpaulo comeback_delay = WPA_GET_LE16(pos); 721252190Srpaulo pos += 2; 722252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 723252190Srpaulo "P2P: dialog_token=%u status_code=%u frag_id=%d more_frags=%d " 724252190Srpaulo "comeback_delay=%u", 725252190Srpaulo dialog_token, status_code, frag_id, more_frags, 726252190Srpaulo comeback_delay); 727252190Srpaulo /* TODO: check frag_id match */ 728252190Srpaulo if (status_code) { 729252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 730252190Srpaulo "P2P: Service Discovery failed: status code %u", 731252190Srpaulo status_code); 732252190Srpaulo return; 733252190Srpaulo } 734252190Srpaulo 735252190Srpaulo if (*pos != WLAN_EID_ADV_PROTO) { 736252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 737252190Srpaulo "P2P: Unexpected IE in GAS Comeback Response: %u", 738252190Srpaulo *pos); 739252190Srpaulo return; 740252190Srpaulo } 741252190Srpaulo pos++; 742252190Srpaulo 743252190Srpaulo slen = *pos++; 744252190Srpaulo next = pos + slen; 745252190Srpaulo if (next > end || slen < 2) { 746252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 747252190Srpaulo "P2P: Invalid IE in GAS Comeback Response"); 748252190Srpaulo return; 749252190Srpaulo } 750252190Srpaulo pos++; /* skip QueryRespLenLimit and PAME-BI */ 751252190Srpaulo 752252190Srpaulo if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { 753252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 754252190Srpaulo "P2P: Unsupported GAS advertisement protocol id %u", 755252190Srpaulo *pos); 756252190Srpaulo return; 757252190Srpaulo } 758252190Srpaulo 759252190Srpaulo pos = next; 760252190Srpaulo /* Query Response */ 761252190Srpaulo if (pos + 2 > end) { 762252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query " 763252190Srpaulo "Response"); 764252190Srpaulo return; 765252190Srpaulo } 766252190Srpaulo slen = WPA_GET_LE16(pos); 767252190Srpaulo pos += 2; 768252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d", 769252190Srpaulo slen); 770252190Srpaulo if (pos + slen > end) { 771252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query " 772252190Srpaulo "Response data"); 773252190Srpaulo return; 774252190Srpaulo } 775252190Srpaulo if (slen == 0) { 776252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No Query Response " 777252190Srpaulo "data"); 778252190Srpaulo return; 779252190Srpaulo } 780252190Srpaulo end = pos + slen; 781252190Srpaulo 782252190Srpaulo if (p2p->sd_rx_resp) { 783252190Srpaulo /* 784252190Srpaulo * ANQP header is only included in the first fragment; rest of 785252190Srpaulo * the fragments start with continue TLVs. 786252190Srpaulo */ 787252190Srpaulo goto skip_nqp_header; 788252190Srpaulo } 789252190Srpaulo 790252190Srpaulo /* ANQP Query Response */ 791252190Srpaulo if (pos + 4 > end) 792252190Srpaulo return; 793252190Srpaulo if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { 794252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 795252190Srpaulo "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); 796252190Srpaulo return; 797252190Srpaulo } 798252190Srpaulo pos += 2; 799252190Srpaulo 800252190Srpaulo slen = WPA_GET_LE16(pos); 801252190Srpaulo pos += 2; 802252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: ANQP Query Response " 803252190Srpaulo "length: %u", slen); 804252190Srpaulo if (slen < 3 + 1) { 805252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 806252190Srpaulo "P2P: Invalid ANQP Query Response length"); 807252190Srpaulo return; 808252190Srpaulo } 809252190Srpaulo if (pos + 4 > end) 810252190Srpaulo return; 811252190Srpaulo 812252190Srpaulo if (WPA_GET_BE24(pos) != OUI_WFA) { 813252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 814252190Srpaulo "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos)); 815252190Srpaulo return; 816252190Srpaulo } 817252190Srpaulo pos += 3; 818252190Srpaulo 819252190Srpaulo if (*pos != P2P_OUI_TYPE) { 820252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 821252190Srpaulo "P2P: Unsupported ANQP vendor type %u", *pos); 822252190Srpaulo return; 823252190Srpaulo } 824252190Srpaulo pos++; 825252190Srpaulo 826252190Srpaulo if (pos + 2 > end) 827252190Srpaulo return; 828252190Srpaulo p2p->sd_rx_update_indic = WPA_GET_LE16(pos); 829252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 830252190Srpaulo "P2P: Service Update Indicator: %u", p2p->sd_rx_update_indic); 831252190Srpaulo pos += 2; 832252190Srpaulo 833252190Srpauloskip_nqp_header: 834252190Srpaulo if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0) 835252190Srpaulo return; 836252190Srpaulo wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos); 837252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Current SD reassembly " 838252190Srpaulo "buffer length: %u", 839252190Srpaulo (unsigned int) wpabuf_len(p2p->sd_rx_resp)); 840252190Srpaulo 841252190Srpaulo if (more_frags) { 842252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: More fragments " 843252190Srpaulo "remains"); 844252190Srpaulo /* TODO: what would be a good size limit? */ 845252190Srpaulo if (wpabuf_len(p2p->sd_rx_resp) > 64000) { 846252190Srpaulo wpabuf_free(p2p->sd_rx_resp); 847252190Srpaulo p2p->sd_rx_resp = NULL; 848252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too long " 849252190Srpaulo "SD response - drop it"); 850252190Srpaulo return; 851252190Srpaulo } 852252190Srpaulo p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq); 853252190Srpaulo return; 854252190Srpaulo } 855252190Srpaulo 856252190Srpaulo p2p->sd_peer->flags |= P2P_DEV_SD_INFO; 857252190Srpaulo p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE; 858252190Srpaulo p2p->sd_peer = NULL; 859252190Srpaulo 860252190Srpaulo if (p2p->sd_query) { 861252190Srpaulo if (!p2p->sd_query->for_all_peers) { 862252190Srpaulo struct p2p_sd_query *q; 863252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 864252190Srpaulo "P2P: Remove completed SD query %p", 865252190Srpaulo p2p->sd_query); 866252190Srpaulo q = p2p->sd_query; 867252190Srpaulo p2p_unlink_sd_query(p2p, p2p->sd_query); 868252190Srpaulo p2p_free_sd_query(q); 869252190Srpaulo } 870252190Srpaulo p2p->sd_query = NULL; 871252190Srpaulo } 872252190Srpaulo 873252190Srpaulo if (p2p->cfg->sd_response) 874252190Srpaulo p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, 875252190Srpaulo p2p->sd_rx_update_indic, 876252190Srpaulo wpabuf_head(p2p->sd_rx_resp), 877252190Srpaulo wpabuf_len(p2p->sd_rx_resp)); 878252190Srpaulo wpabuf_free(p2p->sd_rx_resp); 879252190Srpaulo p2p->sd_rx_resp = NULL; 880252190Srpaulo 881252190Srpaulo p2p_continue_find(p2p); 882252190Srpaulo} 883252190Srpaulo 884252190Srpaulo 885252190Srpaulovoid * p2p_sd_request(struct p2p_data *p2p, const u8 *dst, 886252190Srpaulo const struct wpabuf *tlvs) 887252190Srpaulo{ 888252190Srpaulo struct p2p_sd_query *q; 889252190Srpaulo 890252190Srpaulo q = os_zalloc(sizeof(*q)); 891252190Srpaulo if (q == NULL) 892252190Srpaulo return NULL; 893252190Srpaulo 894252190Srpaulo if (dst) 895252190Srpaulo os_memcpy(q->peer, dst, ETH_ALEN); 896252190Srpaulo else 897252190Srpaulo q->for_all_peers = 1; 898252190Srpaulo 899252190Srpaulo q->tlvs = wpabuf_dup(tlvs); 900252190Srpaulo if (q->tlvs == NULL) { 901252190Srpaulo p2p_free_sd_query(q); 902252190Srpaulo return NULL; 903252190Srpaulo } 904252190Srpaulo 905252190Srpaulo q->next = p2p->sd_queries; 906252190Srpaulo p2p->sd_queries = q; 907252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p", q); 908252190Srpaulo 909252190Srpaulo if (dst == NULL) { 910252190Srpaulo struct p2p_device *dev; 911252190Srpaulo dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) 912252190Srpaulo dev->flags &= ~P2P_DEV_SD_INFO; 913252190Srpaulo } 914252190Srpaulo 915252190Srpaulo return q; 916252190Srpaulo} 917252190Srpaulo 918252190Srpaulo 919252190Srpaulo#ifdef CONFIG_WIFI_DISPLAY 920252190Srpaulovoid * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst, 921252190Srpaulo const struct wpabuf *tlvs) 922252190Srpaulo{ 923252190Srpaulo struct p2p_sd_query *q; 924252190Srpaulo q = p2p_sd_request(p2p, dst, tlvs); 925252190Srpaulo if (q) 926252190Srpaulo q->wsd = 1; 927252190Srpaulo return q; 928252190Srpaulo} 929252190Srpaulo#endif /* CONFIG_WIFI_DISPLAY */ 930252190Srpaulo 931252190Srpaulo 932252190Srpaulovoid p2p_sd_service_update(struct p2p_data *p2p) 933252190Srpaulo{ 934252190Srpaulo p2p->srv_update_indic++; 935252190Srpaulo} 936252190Srpaulo 937252190Srpaulo 938252190Srpauloint p2p_sd_cancel_request(struct p2p_data *p2p, void *req) 939252190Srpaulo{ 940252190Srpaulo if (p2p_unlink_sd_query(p2p, req)) { 941252190Srpaulo wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, 942252190Srpaulo "P2P: Cancel pending SD query %p", req); 943252190Srpaulo p2p_free_sd_query(req); 944252190Srpaulo return 0; 945252190Srpaulo } 946252190Srpaulo return -1; 947252190Srpaulo} 948