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