blob: d8daa59d24f983a356373f60d251904c36039501 [file] [log] [blame]
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -07001/*
2 * Wi-Fi Direct - P2P service discovery
3 * Copyright (c) 2009, Atheros Communications
4 *
Dmitry Shmidtc5ec7f52012-03-06 16:33:24 -08005 * This software may be distributed under the terms of the BSD license.
6 * See README for more details.
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -07007 */
8
9#include "includes.h"
10
11#include "common.h"
12#include "common/ieee802_11_defs.h"
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -080013#include "common/gas.h"
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -070014#include "p2p_i.h"
15#include "p2p.h"
16
17
Dmitry Shmidt61d9df32012-08-29 16:22:06 -070018#ifdef CONFIG_WIFI_DISPLAY
19static int wfd_wsd_supported(struct wpabuf *wfd)
20{
21 const u8 *pos, *end;
22 u8 subelem;
23 u16 len;
24
25 if (wfd == NULL)
26 return 0;
27
28 pos = wpabuf_head(wfd);
29 end = pos + wpabuf_len(wfd);
30
31 while (pos + 3 <= end) {
32 subelem = *pos++;
33 len = WPA_GET_BE16(pos);
34 pos += 2;
35 if (pos + len > end)
36 break;
37
38 if (subelem == WFD_SUBELEM_DEVICE_INFO && len >= 6) {
39 u16 info = WPA_GET_BE16(pos);
40 return !!(info & 0x0040);
41 }
42
43 pos += len;
44 }
45
46 return 0;
47}
48#endif /* CONFIG_WIFI_DISPLAY */
49
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -070050struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p,
51 struct p2p_device *dev)
52{
53 struct p2p_sd_query *q;
Dmitry Shmidt61d9df32012-08-29 16:22:06 -070054 int wsd = 0;
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -070055
56 if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY))
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -080057 return NULL; /* peer does not support SD */
Dmitry Shmidt61d9df32012-08-29 16:22:06 -070058#ifdef CONFIG_WIFI_DISPLAY
59 if (wfd_wsd_supported(dev->info.wfd_subelems))
60 wsd = 1;
61#endif /* CONFIG_WIFI_DISPLAY */
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -070062
63 for (q = p2p->sd_queries; q; q = q->next) {
Dmitry Shmidt61d9df32012-08-29 16:22:06 -070064 /* Use WSD only if the peer indicates support or it */
65 if (q->wsd && !wsd)
66 continue;
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -070067 if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO))
68 return q;
69 if (!q->for_all_peers &&
70 os_memcmp(q->peer, dev->info.p2p_device_addr, ETH_ALEN) ==
71 0)
72 return q;
73 }
74
75 return NULL;
76}
77
78
79static int p2p_unlink_sd_query(struct p2p_data *p2p,
80 struct p2p_sd_query *query)
81{
82 struct p2p_sd_query *q, *prev;
83 q = p2p->sd_queries;
84 prev = NULL;
85 while (q) {
86 if (q == query) {
87 if (prev)
88 prev->next = q->next;
89 else
90 p2p->sd_queries = q->next;
91 if (p2p->sd_query == query)
92 p2p->sd_query = NULL;
93 return 1;
94 }
95 prev = q;
96 q = q->next;
97 }
98 return 0;
99}
100
101
102static void p2p_free_sd_query(struct p2p_sd_query *q)
103{
104 if (q == NULL)
105 return;
106 wpabuf_free(q->tlvs);
107 os_free(q);
108}
109
110
111void p2p_free_sd_queries(struct p2p_data *p2p)
112{
113 struct p2p_sd_query *q, *prev;
114 q = p2p->sd_queries;
115 p2p->sd_queries = NULL;
116 while (q) {
117 prev = q;
118 q = q->next;
119 p2p_free_sd_query(prev);
120 }
121}
122
123
124static struct wpabuf * p2p_build_sd_query(u16 update_indic,
125 struct wpabuf *tlvs)
126{
127 struct wpabuf *buf;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800128 u8 *len_pos;
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700129
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800130 buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700131 if (buf == NULL)
132 return NULL;
133
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800134 /* ANQP Query Request Frame */
135 len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700136 wpabuf_put_be24(buf, OUI_WFA);
137 wpabuf_put_u8(buf, P2P_OUI_TYPE);
138 wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
139 wpabuf_put_buf(buf, tlvs);
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800140 gas_anqp_set_element_len(buf, len_pos);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700141
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800142 gas_anqp_set_len(buf);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700143
144 return buf;
145}
146
147
148static 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
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800153 req = gas_build_comeback_req(dialog_token);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700154 if (req == NULL)
155 return;
156
157 p2p->pending_action_state = P2P_NO_PENDING_ACTION;
158 if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst,
159 wpabuf_head(req), wpabuf_len(req), 200) < 0)
160 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
161 "P2P: Failed to send Action frame");
162
163 wpabuf_free(req);
164}
165
166
167static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
168 u16 comeback_delay,
169 u16 update_indic,
170 const struct wpabuf *tlvs)
171{
172 struct wpabuf *buf;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800173 u8 *len_pos;
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700174
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800175 buf = gas_anqp_build_initial_resp(dialog_token, status_code,
176 comeback_delay,
177 100 + (tlvs ? wpabuf_len(tlvs) : 0));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700178 if (buf == NULL)
179 return NULL;
180
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700181 if (tlvs) {
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800182 /* ANQP Query Response Frame */
183 len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700184 wpabuf_put_be24(buf, OUI_WFA);
185 wpabuf_put_u8(buf, P2P_OUI_TYPE);
186 /* Service Update Indicator */
187 wpabuf_put_le16(buf, update_indic);
188 wpabuf_put_buf(buf, tlvs);
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800189 gas_anqp_set_element_len(buf, len_pos);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700190 }
191
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800192 gas_anqp_set_len(buf);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700193
194 return buf;
195}
196
197
198static struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token,
199 u16 status_code,
200 u16 update_indic,
201 const u8 *data, size_t len,
202 u8 frag_id, u8 more,
203 u16 total_len)
204{
205 struct wpabuf *buf;
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700206
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800207 buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id,
208 more, 0, 100 + len);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700209 if (buf == NULL)
210 return NULL;
211
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700212 if (frag_id == 0) {
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800213 /* ANQP Query Response Frame */
214 wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700215 wpabuf_put_le16(buf, 3 + 1 + 2 + total_len);
216 wpabuf_put_be24(buf, OUI_WFA);
217 wpabuf_put_u8(buf, P2P_OUI_TYPE);
218 /* Service Update Indicator */
219 wpabuf_put_le16(buf, update_indic);
220 }
221
222 wpabuf_put_data(buf, data, len);
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800223 gas_anqp_set_len(buf);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700224
225 return buf;
226}
227
228
229int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev)
230{
231 struct wpabuf *req;
232 int ret = 0;
233 struct p2p_sd_query *query;
234 int freq;
235
236 freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq;
237 if (freq <= 0) {
238 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
239 "P2P: No Listen/Operating frequency known for the "
240 "peer " MACSTR " to send SD Request",
241 MAC2STR(dev->info.p2p_device_addr));
242 return -1;
243 }
244
245 query = p2p_pending_sd_req(p2p, dev);
246 if (query == NULL)
247 return -1;
248
249 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
250 "P2P: Start Service Discovery with " MACSTR,
251 MAC2STR(dev->info.p2p_device_addr));
252
253 req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs);
254 if (req == NULL)
255 return -1;
256
257 p2p->sd_peer = dev;
258 p2p->sd_query = query;
259 p2p->pending_action_state = P2P_PENDING_SD;
260
261 if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr,
262 p2p->cfg->dev_addr, dev->info.p2p_device_addr,
263 wpabuf_head(req), wpabuf_len(req), 5000) < 0) {
264 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
265 "P2P: Failed to send Action frame");
266 ret = -1;
267 }
268
269 wpabuf_free(req);
270
271 return ret;
272}
273
274
275void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
276 const u8 *data, size_t len, int rx_freq)
277{
278 const u8 *pos = data;
279 const u8 *end = data + len;
280 const u8 *next;
281 u8 dialog_token;
282 u16 slen;
283 int freq;
284 u16 update_indic;
285
286
287 if (p2p->cfg->sd_request == NULL)
288 return;
289
290 if (rx_freq > 0)
291 freq = rx_freq;
292 else
Dmitry Shmidt4b060592013-04-29 16:42:49 -0700293 freq = p2p_channel_to_freq(p2p->cfg->reg_class,
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700294 p2p->cfg->channel);
295 if (freq < 0)
296 return;
297
298 if (len < 1 + 2)
299 return;
300
301 dialog_token = *pos++;
302 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
303 "P2P: GAS Initial Request from " MACSTR " (dialog token %u, "
304 "freq %d)",
305 MAC2STR(sa), dialog_token, rx_freq);
306
307 if (*pos != WLAN_EID_ADV_PROTO) {
308 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
309 "P2P: Unexpected IE in GAS Initial Request: %u", *pos);
310 return;
311 }
312 pos++;
313
314 slen = *pos++;
315 next = pos + slen;
316 if (next > end || slen < 2) {
317 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
318 "P2P: Invalid IE in GAS Initial Request");
319 return;
320 }
321 pos++; /* skip QueryRespLenLimit and PAME-BI */
322
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800323 if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700324 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
325 "P2P: Unsupported GAS advertisement protocol id %u",
326 *pos);
327 return;
328 }
329
330 pos = next;
331 /* Query Request */
332 if (pos + 2 > end)
333 return;
334 slen = WPA_GET_LE16(pos);
335 pos += 2;
336 if (pos + slen > end)
337 return;
338 end = pos + slen;
339
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800340 /* ANQP Query Request */
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700341 if (pos + 4 > end)
342 return;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800343 if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700344 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800345 "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700346 return;
347 }
348 pos += 2;
349
350 slen = WPA_GET_LE16(pos);
351 pos += 2;
352 if (pos + slen > end || slen < 3 + 1) {
353 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800354 "P2P: Invalid ANQP Query Request length");
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700355 return;
356 }
357
358 if (WPA_GET_BE24(pos) != OUI_WFA) {
359 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800360 "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700361 return;
362 }
363 pos += 3;
364
365 if (*pos != P2P_OUI_TYPE) {
366 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800367 "P2P: Unsupported ANQP vendor type %u", *pos);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700368 return;
369 }
370 pos++;
371
372 if (pos + 2 > end)
373 return;
374 update_indic = WPA_GET_LE16(pos);
375 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
376 "P2P: Service Update Indicator: %u", update_indic);
377 pos += 2;
378
379 p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token,
380 update_indic, pos, end - pos);
381 /* the response will be indicated with a call to p2p_sd_response() */
382}
383
384
385void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
386 u8 dialog_token, const struct wpabuf *resp_tlvs)
387{
388 struct wpabuf *resp;
389
390 /* TODO: fix the length limit to match with the maximum frame length */
391 if (wpabuf_len(resp_tlvs) > 1400) {
392 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response long "
393 "enough to require fragmentation");
394 if (p2p->sd_resp) {
395 /*
396 * TODO: Could consider storing the fragmented response
397 * separately for each peer to avoid having to drop old
398 * one if there is more than one pending SD query.
399 * Though, that would eat more memory, so there are
400 * also benefits to just using a single buffer.
401 */
402 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop "
403 "previous SD response");
404 wpabuf_free(p2p->sd_resp);
405 }
Dmitry Shmidt04949592012-07-19 12:16:46 -0700406 p2p->sd_resp = wpabuf_dup(resp_tlvs);
407 if (p2p->sd_resp == NULL) {
408 wpa_msg(p2p->cfg->msg_ctx, MSG_ERROR, "P2P: Failed to "
409 "allocate SD response fragmentation area");
410 return;
411 }
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700412 os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
413 p2p->sd_resp_dialog_token = dialog_token;
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700414 p2p->sd_resp_pos = 0;
415 p2p->sd_frag_id = 0;
416 resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
417 1, p2p->srv_update_indic, NULL);
418 } else {
419 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response fits "
420 "in initial response");
421 resp = p2p_build_sd_response(dialog_token,
422 WLAN_STATUS_SUCCESS, 0,
423 p2p->srv_update_indic, resp_tlvs);
424 }
425 if (resp == NULL)
426 return;
427
428 p2p->pending_action_state = P2P_NO_PENDING_ACTION;
429 if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr,
430 p2p->cfg->dev_addr,
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700431 wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
432 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
433 "P2P: Failed to send Action frame");
434
435 wpabuf_free(resp);
436}
437
438
439void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
440 const u8 *data, size_t len, int rx_freq)
441{
442 const u8 *pos = data;
443 const u8 *end = data + len;
444 const u8 *next;
445 u8 dialog_token;
446 u16 status_code;
447 u16 comeback_delay;
448 u16 slen;
449 u16 update_indic;
450
Dmitry Shmidt04949592012-07-19 12:16:46 -0700451#ifdef ANDROID_P2P
452 if (p2p->state != P2P_SD_DURING_FIND) {
453 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
454 "P2P: #### Not ignoring unexpected GAS Initial Response from "
455 MACSTR " state %d", MAC2STR(sa), p2p->state);
456 }
457 if (p2p->sd_peer == NULL ||
458#else
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700459 if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
Dmitry Shmidt04949592012-07-19 12:16:46 -0700460#endif
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700461 os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
Dmitry Shmidt61d9df32012-08-29 16:22:06 -0700462 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700463 "P2P: Ignore unexpected GAS Initial Response from "
464 MACSTR, MAC2STR(sa));
465 return;
466 }
467 p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
468 p2p_clear_timeout(p2p);
469
470 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
471 "P2P: Received GAS Initial Response from " MACSTR " (len=%d)",
472 MAC2STR(sa), (int) len);
473
474 if (len < 5 + 2) {
475 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
476 "P2P: Too short GAS Initial Response frame");
477 return;
478 }
479
480 dialog_token = *pos++;
481 /* TODO: check dialog_token match */
482 status_code = WPA_GET_LE16(pos);
483 pos += 2;
484 comeback_delay = WPA_GET_LE16(pos);
485 pos += 2;
486 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
487 "P2P: dialog_token=%u status_code=%u comeback_delay=%u",
488 dialog_token, status_code, comeback_delay);
489 if (status_code) {
490 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
491 "P2P: Service Discovery failed: status code %u",
492 status_code);
493 return;
494 }
495
496 if (*pos != WLAN_EID_ADV_PROTO) {
497 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
498 "P2P: Unexpected IE in GAS Initial Response: %u",
499 *pos);
500 return;
501 }
502 pos++;
503
504 slen = *pos++;
505 next = pos + slen;
506 if (next > end || slen < 2) {
507 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
508 "P2P: Invalid IE in GAS Initial Response");
509 return;
510 }
511 pos++; /* skip QueryRespLenLimit and PAME-BI */
512
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800513 if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700514 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
515 "P2P: Unsupported GAS advertisement protocol id %u",
516 *pos);
517 return;
518 }
519
520 pos = next;
521 /* Query Response */
522 if (pos + 2 > end) {
523 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
524 "Response");
525 return;
526 }
527 slen = WPA_GET_LE16(pos);
528 pos += 2;
529 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
530 slen);
531 if (pos + slen > end) {
532 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
533 "Response data");
534 return;
535 }
536 end = pos + slen;
537
538 if (comeback_delay) {
539 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Fragmented "
540 "response - request fragments");
541 if (p2p->sd_rx_resp) {
542 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop "
543 "old SD reassembly buffer");
544 wpabuf_free(p2p->sd_rx_resp);
545 p2p->sd_rx_resp = NULL;
546 }
547 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
548 return;
549 }
550
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800551 /* ANQP Query Response */
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700552 if (pos + 4 > end)
553 return;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800554 if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700555 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800556 "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700557 return;
558 }
559 pos += 2;
560
561 slen = WPA_GET_LE16(pos);
562 pos += 2;
563 if (pos + slen > end || slen < 3 + 1) {
564 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800565 "P2P: Invalid ANQP Query Response length");
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700566 return;
567 }
568
569 if (WPA_GET_BE24(pos) != OUI_WFA) {
570 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800571 "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700572 return;
573 }
574 pos += 3;
575
576 if (*pos != P2P_OUI_TYPE) {
577 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800578 "P2P: Unsupported ANQP vendor type %u", *pos);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700579 return;
580 }
581 pos++;
582
583 if (pos + 2 > end)
584 return;
585 update_indic = WPA_GET_LE16(pos);
586 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
587 "P2P: Service Update Indicator: %u", update_indic);
588 pos += 2;
589
590 p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
591 p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
592 p2p->sd_peer = NULL;
593
594 if (p2p->sd_query) {
595 if (!p2p->sd_query->for_all_peers) {
596 struct p2p_sd_query *q;
597 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
598 "P2P: Remove completed SD query %p",
599 p2p->sd_query);
600 q = p2p->sd_query;
601 p2p_unlink_sd_query(p2p, p2p->sd_query);
602 p2p_free_sd_query(q);
603 }
604 p2p->sd_query = NULL;
605 }
606
607 if (p2p->cfg->sd_response)
608 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
609 pos, end - pos);
610 p2p_continue_find(p2p);
611}
612
613
614void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
615 const u8 *data, size_t len, int rx_freq)
616{
617 struct wpabuf *resp;
618 u8 dialog_token;
619 size_t frag_len;
620 int more = 0;
621
622 wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
623 if (len < 1)
624 return;
625 dialog_token = *data;
626 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Dialog Token: %u",
627 dialog_token);
628 if (dialog_token != p2p->sd_resp_dialog_token) {
629 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
630 "response fragment for dialog token %u", dialog_token);
631 return;
632 }
633
634 if (p2p->sd_resp == NULL) {
635 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
636 "response fragment available");
637 return;
638 }
639 if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
640 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
641 "response fragment for " MACSTR, MAC2STR(sa));
642 return;
643 }
644
645 frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
646 if (frag_len > 1400) {
647 frag_len = 1400;
648 more = 1;
649 }
650 resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
651 p2p->srv_update_indic,
652 wpabuf_head_u8(p2p->sd_resp) +
653 p2p->sd_resp_pos, frag_len,
654 p2p->sd_frag_id, more,
655 wpabuf_len(p2p->sd_resp));
656 if (resp == NULL)
657 return;
658 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Send GAS Comeback "
659 "Response (frag_id %d more=%d frag_len=%d)",
660 p2p->sd_frag_id, more, (int) frag_len);
661 p2p->sd_frag_id++;
662 p2p->sd_resp_pos += frag_len;
663
664 if (more) {
665 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: %d more bytes "
666 "remain to be sent",
667 (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
668 } else {
669 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: All fragments of "
670 "SD response sent");
671 wpabuf_free(p2p->sd_resp);
672 p2p->sd_resp = NULL;
673 }
674
675 p2p->pending_action_state = P2P_NO_PENDING_ACTION;
676 if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr,
677 p2p->cfg->dev_addr,
678 wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
679 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
680 "P2P: Failed to send Action frame");
681
682 wpabuf_free(resp);
683}
684
685
686void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
687 const u8 *data, size_t len, int rx_freq)
688{
689 const u8 *pos = data;
690 const u8 *end = data + len;
691 const u8 *next;
692 u8 dialog_token;
693 u16 status_code;
694 u8 frag_id;
695 u8 more_frags;
696 u16 comeback_delay;
697 u16 slen;
698
699 wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
700
Dmitry Shmidt04949592012-07-19 12:16:46 -0700701#ifdef ANDROID_P2P
702 if (p2p->state != P2P_SD_DURING_FIND) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700703 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt04949592012-07-19 12:16:46 -0700704 "P2P: #### Not ignoring unexpected GAS Comeback Response from "
705 MACSTR " state %d", MAC2STR(sa), p2p->state);
706 }
707 if (p2p->sd_peer == NULL ||
708#else
709 if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
710#endif
711 os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
Dmitry Shmidt61d9df32012-08-29 16:22:06 -0700712 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700713 "P2P: Ignore unexpected GAS Comeback Response from "
714 MACSTR, MAC2STR(sa));
715 return;
716 }
717 p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
718 p2p_clear_timeout(p2p);
719
720 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
721 "P2P: Received GAS Comeback Response from " MACSTR " (len=%d)",
722 MAC2STR(sa), (int) len);
723
724 if (len < 6 + 2) {
725 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
726 "P2P: Too short GAS Comeback Response frame");
727 return;
728 }
729
730 dialog_token = *pos++;
731 /* TODO: check dialog_token match */
732 status_code = WPA_GET_LE16(pos);
733 pos += 2;
734 frag_id = *pos & 0x7f;
735 more_frags = (*pos & 0x80) >> 7;
736 pos++;
737 comeback_delay = WPA_GET_LE16(pos);
738 pos += 2;
739 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
740 "P2P: dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
741 "comeback_delay=%u",
742 dialog_token, status_code, frag_id, more_frags,
743 comeback_delay);
744 /* TODO: check frag_id match */
745 if (status_code) {
746 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
747 "P2P: Service Discovery failed: status code %u",
748 status_code);
749 return;
750 }
751
752 if (*pos != WLAN_EID_ADV_PROTO) {
753 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
754 "P2P: Unexpected IE in GAS Comeback Response: %u",
755 *pos);
756 return;
757 }
758 pos++;
759
760 slen = *pos++;
761 next = pos + slen;
762 if (next > end || slen < 2) {
763 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
764 "P2P: Invalid IE in GAS Comeback Response");
765 return;
766 }
767 pos++; /* skip QueryRespLenLimit and PAME-BI */
768
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800769 if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700770 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
771 "P2P: Unsupported GAS advertisement protocol id %u",
772 *pos);
773 return;
774 }
775
776 pos = next;
777 /* Query Response */
778 if (pos + 2 > end) {
779 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
780 "Response");
781 return;
782 }
783 slen = WPA_GET_LE16(pos);
784 pos += 2;
785 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
786 slen);
787 if (pos + slen > end) {
788 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
789 "Response data");
790 return;
791 }
792 if (slen == 0) {
793 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No Query Response "
794 "data");
795 return;
796 }
797 end = pos + slen;
798
799 if (p2p->sd_rx_resp) {
800 /*
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800801 * ANQP header is only included in the first fragment; rest of
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700802 * the fragments start with continue TLVs.
803 */
804 goto skip_nqp_header;
805 }
806
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800807 /* ANQP Query Response */
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700808 if (pos + 4 > end)
809 return;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800810 if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700811 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800812 "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700813 return;
814 }
815 pos += 2;
816
817 slen = WPA_GET_LE16(pos);
818 pos += 2;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800819 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: ANQP Query Response "
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700820 "length: %u", slen);
821 if (slen < 3 + 1) {
822 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800823 "P2P: Invalid ANQP Query Response length");
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700824 return;
825 }
826 if (pos + 4 > end)
827 return;
828
829 if (WPA_GET_BE24(pos) != OUI_WFA) {
830 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800831 "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700832 return;
833 }
834 pos += 3;
835
836 if (*pos != P2P_OUI_TYPE) {
837 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800838 "P2P: Unsupported ANQP vendor type %u", *pos);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700839 return;
840 }
841 pos++;
842
843 if (pos + 2 > end)
844 return;
845 p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
846 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
847 "P2P: Service Update Indicator: %u", p2p->sd_rx_update_indic);
848 pos += 2;
849
850skip_nqp_header:
851 if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
852 return;
853 wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
854 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Current SD reassembly "
855 "buffer length: %u",
856 (unsigned int) wpabuf_len(p2p->sd_rx_resp));
857
858 if (more_frags) {
859 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: More fragments "
860 "remains");
861 /* TODO: what would be a good size limit? */
862 if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
863 wpabuf_free(p2p->sd_rx_resp);
864 p2p->sd_rx_resp = NULL;
865 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too long "
866 "SD response - drop it");
867 return;
868 }
869 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
870 return;
871 }
872
873 p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
874 p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
875 p2p->sd_peer = NULL;
876
877 if (p2p->sd_query) {
878 if (!p2p->sd_query->for_all_peers) {
879 struct p2p_sd_query *q;
880 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
881 "P2P: Remove completed SD query %p",
882 p2p->sd_query);
883 q = p2p->sd_query;
884 p2p_unlink_sd_query(p2p, p2p->sd_query);
885 p2p_free_sd_query(q);
886 }
887 p2p->sd_query = NULL;
888 }
889
890 if (p2p->cfg->sd_response)
891 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
892 p2p->sd_rx_update_indic,
893 wpabuf_head(p2p->sd_rx_resp),
894 wpabuf_len(p2p->sd_rx_resp));
895 wpabuf_free(p2p->sd_rx_resp);
896 p2p->sd_rx_resp = NULL;
897
898 p2p_continue_find(p2p);
899}
900
901
902void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
903 const struct wpabuf *tlvs)
904{
905 struct p2p_sd_query *q;
Irfan Sherifff44b9c42012-06-13 11:09:23 -0700906#ifdef ANDROID_P2P
Dmitry Shmidt04949592012-07-19 12:16:46 -0700907 /* Currently, supplicant doesn't support more than one pending broadcast SD request.
Irfan Sherifff44b9c42012-06-13 11:09:23 -0700908 * So reject if application is registering another one before cancelling the existing one.
909 */
910 for (q = p2p->sd_queries; q; q = q->next) {
911 if( (q->for_all_peers == 1) && (!dst)) {
912 wpa_printf(MSG_ERROR, "P2P: Already one pending"
913 " Broadcast request. Please cancel the current one"
914 " before adding a new one");
915 return NULL;
916 }
917 }
918#endif
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700919
920 q = os_zalloc(sizeof(*q));
921 if (q == NULL)
922 return NULL;
923
924 if (dst)
925 os_memcpy(q->peer, dst, ETH_ALEN);
926 else
927 q->for_all_peers = 1;
928
929 q->tlvs = wpabuf_dup(tlvs);
930 if (q->tlvs == NULL) {
931 p2p_free_sd_query(q);
932 return NULL;
933 }
934
935 q->next = p2p->sd_queries;
936 p2p->sd_queries = q;
Dmitry Shmidt04949592012-07-19 12:16:46 -0700937 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p", q);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700938
Irfan Sheriff96161872012-04-11 18:07:13 -0700939 if (dst == NULL) {
940 struct p2p_device *dev;
941 dl_list_for_each(dev, &p2p->devices, struct p2p_device, list)
942 dev->flags &= ~P2P_DEV_SD_INFO;
943 }
944
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700945 return q;
946}
947
948
Dmitry Shmidt61d9df32012-08-29 16:22:06 -0700949#ifdef CONFIG_WIFI_DISPLAY
950void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
951 const struct wpabuf *tlvs)
952{
953 struct p2p_sd_query *q;
954 q = p2p_sd_request(p2p, dst, tlvs);
955 if (q)
956 q->wsd = 1;
957 return q;
958}
959#endif /* CONFIG_WIFI_DISPLAY */
960
961
Dmitry Shmidtb5e8f062012-08-08 10:56:33 -0700962#ifdef ANDROID_P2P
963void p2p_sd_service_update(struct p2p_data *p2p, int action)
964#else
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700965void p2p_sd_service_update(struct p2p_data *p2p)
Dmitry Shmidtb5e8f062012-08-08 10:56:33 -0700966#endif
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700967{
968 p2p->srv_update_indic++;
Dmitry Shmidtb5e8f062012-08-08 10:56:33 -0700969#ifdef ANDROID_P2P
970 if(action == SRV_FLUSH)
971 p2p->srv_count = 0;
972 else if (action == SRV_DEL)
973 p2p->srv_count--;
974 else if (action == SRV_ADD)
975 p2p->srv_count++;
976
977 if(p2p->cfg->sd_request) {
978 if (p2p->srv_count == 1) {
979 /* First Service Registered. Enable SD capability */
980 p2p->dev_capab |= P2P_DEV_CAPAB_SERVICE_DISCOVERY;
981 } else if (p2p->srv_count == 0 && !p2p->sd_queries) {
982 /* No services remaining + No queries registered .
983 * Remove the SD Capability
984 */
985 p2p->dev_capab &= ~P2P_DEV_CAPAB_SERVICE_DISCOVERY;
986 }
987 }
988#endif
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700989}
990
991
992int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
993{
994 if (p2p_unlink_sd_query(p2p, req)) {
995 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
996 "P2P: Cancel pending SD query %p", req);
Irfan Sherifff44b9c42012-06-13 11:09:23 -0700997#ifdef ANDROID_P2P
Dmitry Shmidt04949592012-07-19 12:16:46 -0700998 p2p->sd_dev_list = NULL;
Irfan Sherifff44b9c42012-06-13 11:09:23 -0700999#endif
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -07001000 p2p_free_sd_query(req);
1001 return 0;
1002 }
1003 return -1;
1004}