Branch data Line data Source code
1 : : /*
2 : : * Wi-Fi Direct - P2P service discovery
3 : : * Copyright (c) 2009, Atheros Communications
4 : : *
5 : : * This software may be distributed under the terms of the BSD license.
6 : : * See README for more details.
7 : : */
8 : :
9 : : #include "includes.h"
10 : :
11 : : #include "common.h"
12 : : #include "common/ieee802_11_defs.h"
13 : : #include "common/gas.h"
14 : : #include "p2p_i.h"
15 : : #include "p2p.h"
16 : :
17 : :
18 : : #ifdef CONFIG_WIFI_DISPLAY
19 : 463 : static int wfd_wsd_supported(struct wpabuf *wfd)
20 : : {
21 : : const u8 *pos, *end;
22 : : u8 subelem;
23 : : u16 len;
24 : :
25 [ + + ]: 463 : if (wfd == NULL)
26 : 62 : return 0;
27 : :
28 : 401 : pos = wpabuf_head(wfd);
29 : 401 : end = pos + wpabuf_len(wfd);
30 : :
31 [ + - ]: 401 : while (pos + 3 <= end) {
32 : 401 : subelem = *pos++;
33 : 401 : len = WPA_GET_BE16(pos);
34 : 401 : pos += 2;
35 [ - + ]: 401 : if (pos + len > end)
36 : 0 : break;
37 : :
38 [ + - ][ + - ]: 401 : if (subelem == WFD_SUBELEM_DEVICE_INFO && len >= 6) {
39 : 401 : u16 info = WPA_GET_BE16(pos);
40 : 401 : return !!(info & 0x0040);
41 : : }
42 : :
43 : 0 : pos += len;
44 : : }
45 : :
46 : 463 : return 0;
47 : : }
48 : : #endif /* CONFIG_WIFI_DISPLAY */
49 : :
50 : 463 : struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p,
51 : : struct p2p_device *dev)
52 : : {
53 : : struct p2p_sd_query *q;
54 : 463 : int wsd = 0;
55 : :
56 [ - + ]: 463 : if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY))
57 : 0 : return NULL; /* peer does not support SD */
58 : : #ifdef CONFIG_WIFI_DISPLAY
59 [ - + ]: 463 : if (wfd_wsd_supported(dev->info.wfd_subelems))
60 : 0 : wsd = 1;
61 : : #endif /* CONFIG_WIFI_DISPLAY */
62 : :
63 [ + + ]: 478 : for (q = p2p->sd_queries; q; q = q->next) {
64 : : /* Use WSD only if the peer indicates support or it */
65 [ - + ][ # # ]: 31 : if (q->wsd && !wsd)
66 : 0 : continue;
67 [ + + ][ + + ]: 31 : if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO))
68 : 14 : return q;
69 [ + + ][ + - ]: 17 : if (!q->for_all_peers &&
70 : 2 : os_memcmp(q->peer, dev->info.p2p_device_addr, ETH_ALEN) ==
71 : : 0)
72 : 2 : return q;
73 : : }
74 : :
75 : 463 : return NULL;
76 : : }
77 : :
78 : :
79 : 6 : static int p2p_unlink_sd_query(struct p2p_data *p2p,
80 : : struct p2p_sd_query *query)
81 : : {
82 : : struct p2p_sd_query *q, *prev;
83 : 6 : q = p2p->sd_queries;
84 : 6 : prev = NULL;
85 [ + + ]: 8 : while (q) {
86 [ + + ]: 7 : if (q == query) {
87 [ + + ]: 5 : if (prev)
88 : 2 : prev->next = q->next;
89 : : else
90 : 3 : p2p->sd_queries = q->next;
91 [ + + ]: 5 : if (p2p->sd_query == query)
92 : 1 : p2p->sd_query = NULL;
93 : 5 : return 1;
94 : : }
95 : 2 : prev = q;
96 : 2 : q = q->next;
97 : : }
98 : 6 : return 0;
99 : : }
100 : :
101 : :
102 : 13 : static void p2p_free_sd_query(struct p2p_sd_query *q)
103 : : {
104 [ - + ]: 13 : if (q == NULL)
105 : 13 : return;
106 : 13 : wpabuf_free(q->tlvs);
107 : 13 : os_free(q);
108 : : }
109 : :
110 : :
111 : 745 : void p2p_free_sd_queries(struct p2p_data *p2p)
112 : : {
113 : : struct p2p_sd_query *q, *prev;
114 : 745 : q = p2p->sd_queries;
115 : 745 : p2p->sd_queries = NULL;
116 [ + + ]: 753 : while (q) {
117 : 8 : prev = q;
118 : 8 : q = q->next;
119 : 8 : p2p_free_sd_query(prev);
120 : : }
121 : 745 : }
122 : :
123 : :
124 : 8 : static struct wpabuf * p2p_build_sd_query(u16 update_indic,
125 : : struct wpabuf *tlvs)
126 : : {
127 : : struct wpabuf *buf;
128 : : u8 *len_pos;
129 : :
130 : 8 : buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs));
131 [ - + ]: 8 : if (buf == NULL)
132 : 0 : return NULL;
133 : :
134 : : /* ANQP Query Request Frame */
135 : 8 : len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
136 : 8 : wpabuf_put_be24(buf, OUI_WFA);
137 : 8 : wpabuf_put_u8(buf, P2P_OUI_TYPE);
138 : 8 : wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
139 : 8 : wpabuf_put_buf(buf, tlvs);
140 : 8 : gas_anqp_set_element_len(buf, len_pos);
141 : :
142 : 8 : gas_anqp_set_len(buf);
143 : :
144 : 8 : return buf;
145 : : }
146 : :
147 : :
148 : 13 : static void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst,
149 : : u8 dialog_token, int freq)
150 : : {
151 : : struct wpabuf *req;
152 : :
153 : 13 : req = gas_build_comeback_req(dialog_token);
154 [ - + ]: 13 : if (req == NULL)
155 : 13 : return;
156 : :
157 : 13 : p2p->pending_action_state = P2P_NO_PENDING_ACTION;
158 [ - + ]: 26 : if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst,
159 : 13 : wpabuf_head(req), wpabuf_len(req), 200) < 0)
160 : 0 : p2p_dbg(p2p, "Failed to send Action frame");
161 : :
162 : 13 : wpabuf_free(req);
163 : : }
164 : :
165 : :
166 : 8 : static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
167 : : u16 comeback_delay,
168 : : u16 update_indic,
169 : : const struct wpabuf *tlvs)
170 : : {
171 : : struct wpabuf *buf;
172 : : u8 *len_pos;
173 : :
174 [ + + ]: 14 : buf = gas_anqp_build_initial_resp(dialog_token, status_code,
175 : : comeback_delay,
176 : 6 : 100 + (tlvs ? wpabuf_len(tlvs) : 0));
177 [ - + ]: 8 : if (buf == NULL)
178 : 0 : return NULL;
179 : :
180 [ + + ]: 8 : if (tlvs) {
181 : : /* ANQP Query Response Frame */
182 : 6 : len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
183 : 6 : wpabuf_put_be24(buf, OUI_WFA);
184 : 6 : wpabuf_put_u8(buf, P2P_OUI_TYPE);
185 : : /* Service Update Indicator */
186 : 6 : wpabuf_put_le16(buf, update_indic);
187 : 6 : wpabuf_put_buf(buf, tlvs);
188 : 6 : gas_anqp_set_element_len(buf, len_pos);
189 : : }
190 : :
191 : 8 : gas_anqp_set_len(buf);
192 : :
193 : 8 : return buf;
194 : : }
195 : :
196 : :
197 : 13 : static struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token,
198 : : u16 status_code,
199 : : u16 update_indic,
200 : : const u8 *data, size_t len,
201 : : u8 frag_id, u8 more,
202 : : u16 total_len)
203 : : {
204 : : struct wpabuf *buf;
205 : :
206 : 13 : buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id,
207 : : more, 0, 100 + len);
208 [ - + ]: 13 : if (buf == NULL)
209 : 0 : return NULL;
210 : :
211 [ + + ]: 13 : if (frag_id == 0) {
212 : : /* ANQP Query Response Frame */
213 : 2 : wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */
214 : 2 : wpabuf_put_le16(buf, 3 + 1 + 2 + total_len);
215 : 2 : wpabuf_put_be24(buf, OUI_WFA);
216 : 2 : wpabuf_put_u8(buf, P2P_OUI_TYPE);
217 : : /* Service Update Indicator */
218 : 2 : wpabuf_put_le16(buf, update_indic);
219 : : }
220 : :
221 : 13 : wpabuf_put_data(buf, data, len);
222 : 13 : gas_anqp_set_len(buf);
223 : :
224 : 13 : return buf;
225 : : }
226 : :
227 : :
228 : 8 : int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev)
229 : : {
230 : : struct wpabuf *req;
231 : 8 : int ret = 0;
232 : : struct p2p_sd_query *query;
233 : : int freq;
234 : :
235 [ + - ]: 8 : freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq;
236 [ - + ]: 8 : if (freq <= 0) {
237 : 0 : p2p_dbg(p2p, "No Listen/Operating frequency known for the peer "
238 : : MACSTR " to send SD Request",
239 : 0 : MAC2STR(dev->info.p2p_device_addr));
240 : 0 : return -1;
241 : : }
242 : :
243 : 8 : query = p2p_pending_sd_req(p2p, dev);
244 [ - + ]: 8 : if (query == NULL)
245 : 0 : return -1;
246 : :
247 : 8 : p2p_dbg(p2p, "Start Service Discovery with " MACSTR,
248 : 48 : MAC2STR(dev->info.p2p_device_addr));
249 : :
250 : 8 : req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs);
251 [ - + ]: 8 : if (req == NULL)
252 : 0 : return -1;
253 : :
254 : 8 : p2p->sd_peer = dev;
255 : 8 : p2p->sd_query = query;
256 : 8 : p2p->pending_action_state = P2P_PENDING_SD;
257 : :
258 [ - + ]: 16 : if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr,
259 : 8 : p2p->cfg->dev_addr, dev->info.p2p_device_addr,
260 : 8 : wpabuf_head(req), wpabuf_len(req), 5000) < 0) {
261 : 0 : p2p_dbg(p2p, "Failed to send Action frame");
262 : 0 : ret = -1;
263 : : }
264 : :
265 : 8 : wpabuf_free(req);
266 : :
267 : 8 : return ret;
268 : : }
269 : :
270 : :
271 : 8 : void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
272 : : const u8 *data, size_t len, int rx_freq)
273 : : {
274 : 8 : const u8 *pos = data;
275 : 8 : const u8 *end = data + len;
276 : : const u8 *next;
277 : : u8 dialog_token;
278 : : u16 slen;
279 : : int freq;
280 : : u16 update_indic;
281 : :
282 : :
283 [ - + ]: 8 : if (p2p->cfg->sd_request == NULL)
284 : 0 : return;
285 : :
286 [ + - ]: 8 : if (rx_freq > 0)
287 : 8 : freq = rx_freq;
288 : : else
289 : 0 : freq = p2p_channel_to_freq(p2p->cfg->reg_class,
290 : 0 : p2p->cfg->channel);
291 [ - + ]: 8 : if (freq < 0)
292 : 0 : return;
293 : :
294 [ - + ]: 8 : if (len < 1 + 2)
295 : 0 : return;
296 : :
297 : 8 : dialog_token = *pos++;
298 : 8 : p2p_dbg(p2p, "GAS Initial Request from " MACSTR
299 : : " (dialog token %u, freq %d)",
300 : 48 : MAC2STR(sa), dialog_token, rx_freq);
301 : :
302 [ - + ]: 8 : if (*pos != WLAN_EID_ADV_PROTO) {
303 : 0 : p2p_dbg(p2p, "Unexpected IE in GAS Initial Request: %u", *pos);
304 : 0 : return;
305 : : }
306 : 8 : pos++;
307 : :
308 : 8 : slen = *pos++;
309 : 8 : next = pos + slen;
310 [ + - ][ - + ]: 8 : if (next > end || slen < 2) {
311 : 0 : p2p_dbg(p2p, "Invalid IE in GAS Initial Request");
312 : 0 : return;
313 : : }
314 : 8 : pos++; /* skip QueryRespLenLimit and PAME-BI */
315 : :
316 [ - + ]: 8 : if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
317 : 0 : p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
318 : 0 : *pos);
319 : 0 : return;
320 : : }
321 : :
322 : 8 : pos = next;
323 : : /* Query Request */
324 [ - + ]: 8 : if (pos + 2 > end)
325 : 0 : return;
326 : 8 : slen = WPA_GET_LE16(pos);
327 : 8 : pos += 2;
328 [ - + ]: 8 : if (pos + slen > end)
329 : 0 : return;
330 : 8 : end = pos + slen;
331 : :
332 : : /* ANQP Query Request */
333 [ - + ]: 8 : if (pos + 4 > end)
334 : 0 : return;
335 [ - + ]: 8 : if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
336 : 0 : p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
337 : 0 : return;
338 : : }
339 : 8 : pos += 2;
340 : :
341 : 8 : slen = WPA_GET_LE16(pos);
342 : 8 : pos += 2;
343 [ + - ][ - + ]: 8 : if (pos + slen > end || slen < 3 + 1) {
344 : 0 : p2p_dbg(p2p, "Invalid ANQP Query Request length");
345 : 0 : return;
346 : : }
347 : :
348 [ - + ]: 8 : if (WPA_GET_BE24(pos) != OUI_WFA) {
349 : 0 : p2p_dbg(p2p, "Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
350 : 0 : return;
351 : : }
352 : 8 : pos += 3;
353 : :
354 [ - + ]: 8 : if (*pos != P2P_OUI_TYPE) {
355 : 0 : p2p_dbg(p2p, "Unsupported ANQP vendor type %u", *pos);
356 : 0 : return;
357 : : }
358 : 8 : pos++;
359 : :
360 [ - + ]: 8 : if (pos + 2 > end)
361 : 0 : return;
362 : 8 : update_indic = WPA_GET_LE16(pos);
363 : 8 : p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
364 : 8 : pos += 2;
365 : :
366 : 8 : p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token,
367 : 8 : update_indic, pos, end - pos);
368 : : /* the response will be indicated with a call to p2p_sd_response() */
369 : : }
370 : :
371 : :
372 : 8 : void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
373 : : u8 dialog_token, const struct wpabuf *resp_tlvs)
374 : : {
375 : : struct wpabuf *resp;
376 : :
377 : : /* TODO: fix the length limit to match with the maximum frame length */
378 [ + + ]: 8 : if (wpabuf_len(resp_tlvs) > 1400) {
379 : 2 : p2p_dbg(p2p, "SD response long enough to require fragmentation");
380 [ - + ]: 2 : if (p2p->sd_resp) {
381 : : /*
382 : : * TODO: Could consider storing the fragmented response
383 : : * separately for each peer to avoid having to drop old
384 : : * one if there is more than one pending SD query.
385 : : * Though, that would eat more memory, so there are
386 : : * also benefits to just using a single buffer.
387 : : */
388 : 0 : p2p_dbg(p2p, "Drop previous SD response");
389 : 0 : wpabuf_free(p2p->sd_resp);
390 : : }
391 : 2 : p2p->sd_resp = wpabuf_dup(resp_tlvs);
392 [ - + ]: 2 : if (p2p->sd_resp == NULL) {
393 : 0 : p2p_err(p2p, "Failed to allocate SD response fragmentation area");
394 : 0 : return;
395 : : }
396 : 2 : os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
397 : 2 : p2p->sd_resp_dialog_token = dialog_token;
398 : 2 : p2p->sd_resp_pos = 0;
399 : 2 : p2p->sd_frag_id = 0;
400 : 2 : resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
401 : 2 : 1, p2p->srv_update_indic, NULL);
402 : : } else {
403 : 6 : p2p_dbg(p2p, "SD response fits in initial response");
404 : 6 : resp = p2p_build_sd_response(dialog_token,
405 : : WLAN_STATUS_SUCCESS, 0,
406 : 6 : p2p->srv_update_indic, resp_tlvs);
407 : : }
408 [ - + ]: 8 : if (resp == NULL)
409 : 0 : return;
410 : :
411 : 8 : p2p->pending_action_state = P2P_NO_PENDING_ACTION;
412 [ - + ]: 16 : if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr,
413 : 8 : p2p->cfg->dev_addr,
414 : 8 : wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
415 : 0 : p2p_dbg(p2p, "Failed to send Action frame");
416 : :
417 : 8 : wpabuf_free(resp);
418 : : }
419 : :
420 : :
421 : 8 : void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
422 : : const u8 *data, size_t len, int rx_freq)
423 : : {
424 : 8 : const u8 *pos = data;
425 : 8 : const u8 *end = data + len;
426 : : const u8 *next;
427 : : u8 dialog_token;
428 : : u16 status_code;
429 : : u16 comeback_delay;
430 : : u16 slen;
431 : : u16 update_indic;
432 : :
433 [ + - ][ + - ]: 8 : if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
[ - + ]
434 : 8 : os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
435 : 0 : p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from "
436 : 0 : MACSTR, MAC2STR(sa));
437 : 0 : return;
438 : : }
439 : 8 : p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
440 : 8 : p2p_clear_timeout(p2p);
441 : :
442 : 8 : p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)",
443 : 48 : MAC2STR(sa), (int) len);
444 : :
445 [ - + ]: 8 : if (len < 5 + 2) {
446 : 0 : p2p_dbg(p2p, "Too short GAS Initial Response frame");
447 : 0 : return;
448 : : }
449 : :
450 : 8 : dialog_token = *pos++;
451 : : /* TODO: check dialog_token match */
452 : 8 : status_code = WPA_GET_LE16(pos);
453 : 8 : pos += 2;
454 : 8 : comeback_delay = WPA_GET_LE16(pos);
455 : 8 : pos += 2;
456 : 8 : p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u",
457 : : dialog_token, status_code, comeback_delay);
458 [ - + ]: 8 : if (status_code) {
459 : 0 : p2p_dbg(p2p, "Service Discovery failed: status code %u",
460 : : status_code);
461 : 0 : return;
462 : : }
463 : :
464 [ - + ]: 8 : if (*pos != WLAN_EID_ADV_PROTO) {
465 : 0 : p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos);
466 : 0 : return;
467 : : }
468 : 8 : pos++;
469 : :
470 : 8 : slen = *pos++;
471 : 8 : next = pos + slen;
472 [ + - ][ - + ]: 8 : if (next > end || slen < 2) {
473 : 0 : p2p_dbg(p2p, "Invalid IE in GAS Initial Response");
474 : 0 : return;
475 : : }
476 : 8 : pos++; /* skip QueryRespLenLimit and PAME-BI */
477 : :
478 [ - + ]: 8 : if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
479 : 0 : p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
480 : 0 : *pos);
481 : 0 : return;
482 : : }
483 : :
484 : 8 : pos = next;
485 : : /* Query Response */
486 [ - + ]: 8 : if (pos + 2 > end) {
487 : 0 : p2p_dbg(p2p, "Too short Query Response");
488 : 0 : return;
489 : : }
490 : 8 : slen = WPA_GET_LE16(pos);
491 : 8 : pos += 2;
492 : 8 : p2p_dbg(p2p, "Query Response Length: %d", slen);
493 [ - + ]: 8 : if (pos + slen > end) {
494 : 0 : p2p_dbg(p2p, "Not enough Query Response data");
495 : 0 : return;
496 : : }
497 : 8 : end = pos + slen;
498 : :
499 [ + + ]: 8 : if (comeback_delay) {
500 : 2 : p2p_dbg(p2p, "Fragmented response - request fragments");
501 [ - + ]: 2 : if (p2p->sd_rx_resp) {
502 : 0 : p2p_dbg(p2p, "Drop old SD reassembly buffer");
503 : 0 : wpabuf_free(p2p->sd_rx_resp);
504 : 0 : p2p->sd_rx_resp = NULL;
505 : : }
506 : 2 : p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
507 : 2 : return;
508 : : }
509 : :
510 : : /* ANQP Query Response */
511 [ - + ]: 6 : if (pos + 4 > end)
512 : 0 : return;
513 [ - + ]: 6 : if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
514 : 0 : p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
515 : 0 : return;
516 : : }
517 : 6 : pos += 2;
518 : :
519 : 6 : slen = WPA_GET_LE16(pos);
520 : 6 : pos += 2;
521 [ + - ][ - + ]: 6 : if (pos + slen > end || slen < 3 + 1) {
522 : 0 : p2p_dbg(p2p, "Invalid ANQP Query Response length");
523 : 0 : return;
524 : : }
525 : :
526 [ - + ]: 6 : if (WPA_GET_BE24(pos) != OUI_WFA) {
527 : 0 : p2p_dbg(p2p, "Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
528 : 0 : return;
529 : : }
530 : 6 : pos += 3;
531 : :
532 [ - + ]: 6 : if (*pos != P2P_OUI_TYPE) {
533 : 0 : p2p_dbg(p2p, "Unsupported ANQP vendor type %u", *pos);
534 : 0 : return;
535 : : }
536 : 6 : pos++;
537 : :
538 [ - + ]: 6 : if (pos + 2 > end)
539 : 0 : return;
540 : 6 : update_indic = WPA_GET_LE16(pos);
541 : 6 : p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
542 : 6 : pos += 2;
543 : :
544 : 6 : p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
545 : 6 : p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
546 : 6 : p2p->sd_peer = NULL;
547 : :
548 [ + - ]: 6 : if (p2p->sd_query) {
549 [ - + ]: 6 : if (!p2p->sd_query->for_all_peers) {
550 : : struct p2p_sd_query *q;
551 : 0 : p2p_dbg(p2p, "Remove completed SD query %p",
552 : : p2p->sd_query);
553 : 0 : q = p2p->sd_query;
554 : 0 : p2p_unlink_sd_query(p2p, p2p->sd_query);
555 : 0 : p2p_free_sd_query(q);
556 : : }
557 : 6 : p2p->sd_query = NULL;
558 : : }
559 : :
560 [ + - ]: 6 : if (p2p->cfg->sd_response)
561 : 6 : p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
562 : 6 : pos, end - pos);
563 : 8 : p2p_continue_find(p2p);
564 : : }
565 : :
566 : :
567 : 13 : void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
568 : : const u8 *data, size_t len, int rx_freq)
569 : : {
570 : : struct wpabuf *resp;
571 : : u8 dialog_token;
572 : : size_t frag_len;
573 : 13 : int more = 0;
574 : :
575 : 13 : wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
576 [ - + ]: 13 : if (len < 1)
577 : 0 : return;
578 : 13 : dialog_token = *data;
579 : 13 : p2p_dbg(p2p, "Dialog Token: %u", dialog_token);
580 [ - + ]: 13 : if (dialog_token != p2p->sd_resp_dialog_token) {
581 : 0 : p2p_dbg(p2p, "No pending SD response fragment for dialog token %u",
582 : : dialog_token);
583 : 0 : return;
584 : : }
585 : :
586 [ - + ]: 13 : if (p2p->sd_resp == NULL) {
587 : 0 : p2p_dbg(p2p, "No pending SD response fragment available");
588 : 0 : return;
589 : : }
590 [ - + ]: 13 : if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
591 : 0 : p2p_dbg(p2p, "No pending SD response fragment for " MACSTR,
592 : 0 : MAC2STR(sa));
593 : 0 : return;
594 : : }
595 : :
596 : 13 : frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
597 [ + + ]: 13 : if (frag_len > 1400) {
598 : 11 : frag_len = 1400;
599 : 11 : more = 1;
600 : : }
601 : 13 : resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
602 : 13 : p2p->srv_update_indic,
603 : 13 : wpabuf_head_u8(p2p->sd_resp) +
604 : : p2p->sd_resp_pos, frag_len,
605 : 13 : p2p->sd_frag_id, more,
606 : 13 : wpabuf_len(p2p->sd_resp));
607 [ - + ]: 13 : if (resp == NULL)
608 : 0 : return;
609 : 13 : p2p_dbg(p2p, "Send GAS Comeback Response (frag_id %d more=%d frag_len=%d)",
610 : 13 : p2p->sd_frag_id, more, (int) frag_len);
611 : 13 : p2p->sd_frag_id++;
612 : 13 : p2p->sd_resp_pos += frag_len;
613 : :
614 [ + + ]: 13 : if (more) {
615 : 11 : p2p_dbg(p2p, "%d more bytes remain to be sent",
616 : 11 : (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
617 : : } else {
618 : 2 : p2p_dbg(p2p, "All fragments of SD response sent");
619 : 2 : wpabuf_free(p2p->sd_resp);
620 : 2 : p2p->sd_resp = NULL;
621 : : }
622 : :
623 : 13 : p2p->pending_action_state = P2P_NO_PENDING_ACTION;
624 [ - + ]: 26 : if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr,
625 : 13 : p2p->cfg->dev_addr,
626 : 13 : wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
627 : 0 : p2p_dbg(p2p, "Failed to send Action frame");
628 : :
629 : 13 : wpabuf_free(resp);
630 : : }
631 : :
632 : :
633 : 13 : void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
634 : : const u8 *data, size_t len, int rx_freq)
635 : : {
636 : 13 : const u8 *pos = data;
637 : 13 : const u8 *end = data + len;
638 : : const u8 *next;
639 : : u8 dialog_token;
640 : : u16 status_code;
641 : : u8 frag_id;
642 : : u8 more_frags;
643 : : u16 comeback_delay;
644 : : u16 slen;
645 : :
646 : 13 : wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
647 : :
648 [ + - ][ + - ]: 13 : if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
[ - + ]
649 : 13 : os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
650 : 0 : p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from "
651 : 0 : MACSTR, MAC2STR(sa));
652 : 0 : return;
653 : : }
654 : 13 : p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
655 : 13 : p2p_clear_timeout(p2p);
656 : :
657 : 13 : p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)",
658 : 78 : MAC2STR(sa), (int) len);
659 : :
660 [ - + ]: 13 : if (len < 6 + 2) {
661 : 0 : p2p_dbg(p2p, "Too short GAS Comeback Response frame");
662 : 0 : return;
663 : : }
664 : :
665 : 13 : dialog_token = *pos++;
666 : : /* TODO: check dialog_token match */
667 : 13 : status_code = WPA_GET_LE16(pos);
668 : 13 : pos += 2;
669 : 13 : frag_id = *pos & 0x7f;
670 : 13 : more_frags = (*pos & 0x80) >> 7;
671 : 13 : pos++;
672 : 13 : comeback_delay = WPA_GET_LE16(pos);
673 : 13 : pos += 2;
674 : 13 : p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
675 : : "comeback_delay=%u",
676 : : dialog_token, status_code, frag_id, more_frags,
677 : : comeback_delay);
678 : : /* TODO: check frag_id match */
679 [ - + ]: 13 : if (status_code) {
680 : 0 : p2p_dbg(p2p, "Service Discovery failed: status code %u",
681 : : status_code);
682 : 0 : return;
683 : : }
684 : :
685 [ - + ]: 13 : if (*pos != WLAN_EID_ADV_PROTO) {
686 : 0 : p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u",
687 : 0 : *pos);
688 : 0 : return;
689 : : }
690 : 13 : pos++;
691 : :
692 : 13 : slen = *pos++;
693 : 13 : next = pos + slen;
694 [ + - ][ - + ]: 13 : if (next > end || slen < 2) {
695 : 0 : p2p_dbg(p2p, "Invalid IE in GAS Comeback Response");
696 : 0 : return;
697 : : }
698 : 13 : pos++; /* skip QueryRespLenLimit and PAME-BI */
699 : :
700 [ - + ]: 13 : if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
701 : 0 : p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
702 : 0 : *pos);
703 : 0 : return;
704 : : }
705 : :
706 : 13 : pos = next;
707 : : /* Query Response */
708 [ - + ]: 13 : if (pos + 2 > end) {
709 : 0 : p2p_dbg(p2p, "Too short Query Response");
710 : 0 : return;
711 : : }
712 : 13 : slen = WPA_GET_LE16(pos);
713 : 13 : pos += 2;
714 : 13 : p2p_dbg(p2p, "Query Response Length: %d", slen);
715 [ - + ]: 13 : if (pos + slen > end) {
716 : 0 : p2p_dbg(p2p, "Not enough Query Response data");
717 : 0 : return;
718 : : }
719 [ - + ]: 13 : if (slen == 0) {
720 : 0 : p2p_dbg(p2p, "No Query Response data");
721 : 0 : return;
722 : : }
723 : 13 : end = pos + slen;
724 : :
725 [ + + ]: 13 : if (p2p->sd_rx_resp) {
726 : : /*
727 : : * ANQP header is only included in the first fragment; rest of
728 : : * the fragments start with continue TLVs.
729 : : */
730 : 11 : goto skip_nqp_header;
731 : : }
732 : :
733 : : /* ANQP Query Response */
734 [ - + ]: 2 : if (pos + 4 > end)
735 : 0 : return;
736 [ - + ]: 2 : if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
737 : 0 : p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
738 : 0 : return;
739 : : }
740 : 2 : pos += 2;
741 : :
742 : 2 : slen = WPA_GET_LE16(pos);
743 : 2 : pos += 2;
744 : 2 : p2p_dbg(p2p, "ANQP Query Response length: %u", slen);
745 [ - + ]: 2 : if (slen < 3 + 1) {
746 : 0 : p2p_dbg(p2p, "Invalid ANQP Query Response length");
747 : 0 : return;
748 : : }
749 [ - + ]: 2 : if (pos + 4 > end)
750 : 0 : return;
751 : :
752 [ - + ]: 2 : if (WPA_GET_BE24(pos) != OUI_WFA) {
753 : 0 : p2p_dbg(p2p, "Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
754 : 0 : return;
755 : : }
756 : 2 : pos += 3;
757 : :
758 [ - + ]: 2 : if (*pos != P2P_OUI_TYPE) {
759 : 0 : p2p_dbg(p2p, "Unsupported ANQP vendor type %u", *pos);
760 : 0 : return;
761 : : }
762 : 2 : pos++;
763 : :
764 [ - + ]: 2 : if (pos + 2 > end)
765 : 0 : return;
766 : 2 : p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
767 : 2 : p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic);
768 : 2 : pos += 2;
769 : :
770 : : skip_nqp_header:
771 [ - + ]: 13 : if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
772 : 0 : return;
773 : 13 : wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
774 : 13 : p2p_dbg(p2p, "Current SD reassembly buffer length: %u",
775 : 13 : (unsigned int) wpabuf_len(p2p->sd_rx_resp));
776 : :
777 [ + + ]: 13 : if (more_frags) {
778 : 11 : p2p_dbg(p2p, "More fragments remains");
779 : : /* TODO: what would be a good size limit? */
780 [ - + ]: 11 : if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
781 : 0 : wpabuf_free(p2p->sd_rx_resp);
782 : 0 : p2p->sd_rx_resp = NULL;
783 : 0 : p2p_dbg(p2p, "Too long SD response - drop it");
784 : 0 : return;
785 : : }
786 : 11 : p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
787 : 11 : return;
788 : : }
789 : :
790 : 2 : p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
791 : 2 : p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
792 : 2 : p2p->sd_peer = NULL;
793 : :
794 [ + - ]: 2 : if (p2p->sd_query) {
795 [ + + ]: 2 : if (!p2p->sd_query->for_all_peers) {
796 : : struct p2p_sd_query *q;
797 : 1 : p2p_dbg(p2p, "Remove completed SD query %p",
798 : : p2p->sd_query);
799 : 1 : q = p2p->sd_query;
800 : 1 : p2p_unlink_sd_query(p2p, p2p->sd_query);
801 : 1 : p2p_free_sd_query(q);
802 : : }
803 : 2 : p2p->sd_query = NULL;
804 : : }
805 : :
806 [ + - ]: 2 : if (p2p->cfg->sd_response)
807 : 4 : p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
808 : 2 : p2p->sd_rx_update_indic,
809 : 2 : wpabuf_head(p2p->sd_rx_resp),
810 : 2 : wpabuf_len(p2p->sd_rx_resp));
811 : 2 : wpabuf_free(p2p->sd_rx_resp);
812 : 2 : p2p->sd_rx_resp = NULL;
813 : :
814 : 13 : p2p_continue_find(p2p);
815 : : }
816 : :
817 : :
818 : 13 : void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
819 : : const struct wpabuf *tlvs)
820 : : {
821 : : struct p2p_sd_query *q;
822 : :
823 : 13 : q = os_zalloc(sizeof(*q));
824 [ - + ]: 13 : if (q == NULL)
825 : 0 : return NULL;
826 : :
827 [ + + ]: 13 : if (dst)
828 : 6 : os_memcpy(q->peer, dst, ETH_ALEN);
829 : : else
830 : 7 : q->for_all_peers = 1;
831 : :
832 : 13 : q->tlvs = wpabuf_dup(tlvs);
833 [ - + ]: 13 : if (q->tlvs == NULL) {
834 : 0 : p2p_free_sd_query(q);
835 : 0 : return NULL;
836 : : }
837 : :
838 : 13 : q->next = p2p->sd_queries;
839 : 13 : p2p->sd_queries = q;
840 : 13 : p2p_dbg(p2p, "Added SD Query %p", q);
841 : :
842 [ + + ]: 13 : if (dst == NULL) {
843 : : struct p2p_device *dev;
844 [ - + ]: 7 : dl_list_for_each(dev, &p2p->devices, struct p2p_device, list)
845 : 0 : dev->flags &= ~P2P_DEV_SD_INFO;
846 : : }
847 : :
848 : 13 : return q;
849 : : }
850 : :
851 : :
852 : : #ifdef CONFIG_WIFI_DISPLAY
853 : 0 : void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
854 : : const struct wpabuf *tlvs)
855 : : {
856 : : struct p2p_sd_query *q;
857 : 0 : q = p2p_sd_request(p2p, dst, tlvs);
858 [ # # ]: 0 : if (q)
859 : 0 : q->wsd = 1;
860 : 0 : return q;
861 : : }
862 : : #endif /* CONFIG_WIFI_DISPLAY */
863 : :
864 : :
865 : 994 : void p2p_sd_service_update(struct p2p_data *p2p)
866 : : {
867 : 994 : p2p->srv_update_indic++;
868 : 994 : }
869 : :
870 : :
871 : 5 : int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
872 : : {
873 [ + + ]: 5 : if (p2p_unlink_sd_query(p2p, req)) {
874 : 4 : p2p_dbg(p2p, "Cancel pending SD query %p", req);
875 : 4 : p2p_free_sd_query(req);
876 : 4 : return 0;
877 : : }
878 : 5 : return -1;
879 : : }
|
