blob: 050976775354a0b230625190d84eb107142ef8aa [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
18struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p,
19 struct p2p_device *dev)
20{
21 struct p2p_sd_query *q;
22
23 if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY))
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -080024 return NULL; /* peer does not support SD */
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -070025
26 for (q = p2p->sd_queries; q; q = q->next) {
27 if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO))
28 return q;
29 if (!q->for_all_peers &&
30 os_memcmp(q->peer, dev->info.p2p_device_addr, ETH_ALEN) ==
31 0)
32 return q;
33 }
34
35 return NULL;
36}
37
38
39static int p2p_unlink_sd_query(struct p2p_data *p2p,
40 struct p2p_sd_query *query)
41{
42 struct p2p_sd_query *q, *prev;
43 q = p2p->sd_queries;
44 prev = NULL;
45 while (q) {
46 if (q == query) {
47 if (prev)
48 prev->next = q->next;
49 else
50 p2p->sd_queries = q->next;
51 if (p2p->sd_query == query)
52 p2p->sd_query = NULL;
53 return 1;
54 }
55 prev = q;
56 q = q->next;
57 }
58 return 0;
59}
60
61
62static void p2p_free_sd_query(struct p2p_sd_query *q)
63{
64 if (q == NULL)
65 return;
66 wpabuf_free(q->tlvs);
67 os_free(q);
68}
69
70
71void p2p_free_sd_queries(struct p2p_data *p2p)
72{
73 struct p2p_sd_query *q, *prev;
74 q = p2p->sd_queries;
75 p2p->sd_queries = NULL;
76 while (q) {
77 prev = q;
78 q = q->next;
79 p2p_free_sd_query(prev);
80 }
81}
82
83
84static struct wpabuf * p2p_build_sd_query(u16 update_indic,
85 struct wpabuf *tlvs)
86{
87 struct wpabuf *buf;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -080088 u8 *len_pos;
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -070089
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -080090 buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -070091 if (buf == NULL)
92 return NULL;
93
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -080094 /* ANQP Query Request Frame */
95 len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -070096 wpabuf_put_be24(buf, OUI_WFA);
97 wpabuf_put_u8(buf, P2P_OUI_TYPE);
98 wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
99 wpabuf_put_buf(buf, tlvs);
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800100 gas_anqp_set_element_len(buf, len_pos);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700101
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800102 gas_anqp_set_len(buf);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700103
104 return buf;
105}
106
107
108static void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst,
109 u8 dialog_token, int freq)
110{
111 struct wpabuf *req;
112
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800113 req = gas_build_comeback_req(dialog_token);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700114 if (req == NULL)
115 return;
116
117 p2p->pending_action_state = P2P_NO_PENDING_ACTION;
118 if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst,
119 wpabuf_head(req), wpabuf_len(req), 200) < 0)
120 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
121 "P2P: Failed to send Action frame");
122
123 wpabuf_free(req);
124}
125
126
127static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
128 u16 comeback_delay,
129 u16 update_indic,
130 const struct wpabuf *tlvs)
131{
132 struct wpabuf *buf;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800133 u8 *len_pos;
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700134
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800135 buf = gas_anqp_build_initial_resp(dialog_token, status_code,
136 comeback_delay,
137 100 + (tlvs ? wpabuf_len(tlvs) : 0));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700138 if (buf == NULL)
139 return NULL;
140
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700141 if (tlvs) {
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800142 /* ANQP Query Response Frame */
143 len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700144 wpabuf_put_be24(buf, OUI_WFA);
145 wpabuf_put_u8(buf, P2P_OUI_TYPE);
146 /* Service Update Indicator */
147 wpabuf_put_le16(buf, update_indic);
148 wpabuf_put_buf(buf, tlvs);
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800149 gas_anqp_set_element_len(buf, len_pos);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700150 }
151
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800152 gas_anqp_set_len(buf);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700153
154 return buf;
155}
156
157
158static struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token,
159 u16 status_code,
160 u16 update_indic,
161 const u8 *data, size_t len,
162 u8 frag_id, u8 more,
163 u16 total_len)
164{
165 struct wpabuf *buf;
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700166
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800167 buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id,
168 more, 0, 100 + len);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700169 if (buf == NULL)
170 return NULL;
171
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700172 if (frag_id == 0) {
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800173 /* ANQP Query Response Frame */
174 wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700175 wpabuf_put_le16(buf, 3 + 1 + 2 + total_len);
176 wpabuf_put_be24(buf, OUI_WFA);
177 wpabuf_put_u8(buf, P2P_OUI_TYPE);
178 /* Service Update Indicator */
179 wpabuf_put_le16(buf, update_indic);
180 }
181
182 wpabuf_put_data(buf, data, len);
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800183 gas_anqp_set_len(buf);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700184
185 return buf;
186}
187
188
189int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev)
190{
191 struct wpabuf *req;
192 int ret = 0;
193 struct p2p_sd_query *query;
194 int freq;
195
196 freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq;
197 if (freq <= 0) {
198 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
199 "P2P: No Listen/Operating frequency known for the "
200 "peer " MACSTR " to send SD Request",
201 MAC2STR(dev->info.p2p_device_addr));
202 return -1;
203 }
204
205 query = p2p_pending_sd_req(p2p, dev);
206 if (query == NULL)
207 return -1;
208
209 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
210 "P2P: Start Service Discovery with " MACSTR,
211 MAC2STR(dev->info.p2p_device_addr));
212
213 req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs);
214 if (req == NULL)
215 return -1;
216
217 p2p->sd_peer = dev;
218 p2p->sd_query = query;
219 p2p->pending_action_state = P2P_PENDING_SD;
220
221 if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr,
222 p2p->cfg->dev_addr, dev->info.p2p_device_addr,
223 wpabuf_head(req), wpabuf_len(req), 5000) < 0) {
224 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
225 "P2P: Failed to send Action frame");
226 ret = -1;
227 }
228
229 wpabuf_free(req);
230
231 return ret;
232}
233
234
235void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
236 const u8 *data, size_t len, int rx_freq)
237{
238 const u8 *pos = data;
239 const u8 *end = data + len;
240 const u8 *next;
241 u8 dialog_token;
242 u16 slen;
243 int freq;
244 u16 update_indic;
245
246
247 if (p2p->cfg->sd_request == NULL)
248 return;
249
250 if (rx_freq > 0)
251 freq = rx_freq;
252 else
253 freq = p2p_channel_to_freq(p2p->cfg->country,
254 p2p->cfg->reg_class,
255 p2p->cfg->channel);
256 if (freq < 0)
257 return;
258
259 if (len < 1 + 2)
260 return;
261
262 dialog_token = *pos++;
263 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
264 "P2P: GAS Initial Request from " MACSTR " (dialog token %u, "
265 "freq %d)",
266 MAC2STR(sa), dialog_token, rx_freq);
267
268 if (*pos != WLAN_EID_ADV_PROTO) {
269 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
270 "P2P: Unexpected IE in GAS Initial Request: %u", *pos);
271 return;
272 }
273 pos++;
274
275 slen = *pos++;
276 next = pos + slen;
277 if (next > end || slen < 2) {
278 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
279 "P2P: Invalid IE in GAS Initial Request");
280 return;
281 }
282 pos++; /* skip QueryRespLenLimit and PAME-BI */
283
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800284 if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700285 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
286 "P2P: Unsupported GAS advertisement protocol id %u",
287 *pos);
288 return;
289 }
290
291 pos = next;
292 /* Query Request */
293 if (pos + 2 > end)
294 return;
295 slen = WPA_GET_LE16(pos);
296 pos += 2;
297 if (pos + slen > end)
298 return;
299 end = pos + slen;
300
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800301 /* ANQP Query Request */
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700302 if (pos + 4 > end)
303 return;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800304 if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700305 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800306 "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700307 return;
308 }
309 pos += 2;
310
311 slen = WPA_GET_LE16(pos);
312 pos += 2;
313 if (pos + slen > end || slen < 3 + 1) {
314 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800315 "P2P: Invalid ANQP Query Request length");
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700316 return;
317 }
318
319 if (WPA_GET_BE24(pos) != OUI_WFA) {
320 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800321 "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700322 return;
323 }
324 pos += 3;
325
326 if (*pos != P2P_OUI_TYPE) {
327 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800328 "P2P: Unsupported ANQP vendor type %u", *pos);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700329 return;
330 }
331 pos++;
332
333 if (pos + 2 > end)
334 return;
335 update_indic = WPA_GET_LE16(pos);
336 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
337 "P2P: Service Update Indicator: %u", update_indic);
338 pos += 2;
339
340 p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token,
341 update_indic, pos, end - pos);
342 /* the response will be indicated with a call to p2p_sd_response() */
343}
344
345
346void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
347 u8 dialog_token, const struct wpabuf *resp_tlvs)
348{
349 struct wpabuf *resp;
350
351 /* TODO: fix the length limit to match with the maximum frame length */
352 if (wpabuf_len(resp_tlvs) > 1400) {
353 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response long "
354 "enough to require fragmentation");
355 if (p2p->sd_resp) {
356 /*
357 * TODO: Could consider storing the fragmented response
358 * separately for each peer to avoid having to drop old
359 * one if there is more than one pending SD query.
360 * Though, that would eat more memory, so there are
361 * also benefits to just using a single buffer.
362 */
363 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop "
364 "previous SD response");
365 wpabuf_free(p2p->sd_resp);
366 }
Dmitry Shmidt04949592012-07-19 12:16:46 -0700367 p2p->sd_resp = wpabuf_dup(resp_tlvs);
368 if (p2p->sd_resp == NULL) {
369 wpa_msg(p2p->cfg->msg_ctx, MSG_ERROR, "P2P: Failed to "
370 "allocate SD response fragmentation area");
371 return;
372 }
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700373 os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
374 p2p->sd_resp_dialog_token = dialog_token;
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700375 p2p->sd_resp_pos = 0;
376 p2p->sd_frag_id = 0;
377 resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
378 1, p2p->srv_update_indic, NULL);
379 } else {
380 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response fits "
381 "in initial response");
382 resp = p2p_build_sd_response(dialog_token,
383 WLAN_STATUS_SUCCESS, 0,
384 p2p->srv_update_indic, resp_tlvs);
385 }
386 if (resp == NULL)
387 return;
388
389 p2p->pending_action_state = P2P_NO_PENDING_ACTION;
390 if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr,
391 p2p->cfg->dev_addr,
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700392 wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
393 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
394 "P2P: Failed to send Action frame");
395
396 wpabuf_free(resp);
397}
398
399
400void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
401 const u8 *data, size_t len, int rx_freq)
402{
403 const u8 *pos = data;
404 const u8 *end = data + len;
405 const u8 *next;
406 u8 dialog_token;
407 u16 status_code;
408 u16 comeback_delay;
409 u16 slen;
410 u16 update_indic;
411
Dmitry Shmidt04949592012-07-19 12:16:46 -0700412#ifdef ANDROID_P2P
413 if (p2p->state != P2P_SD_DURING_FIND) {
414 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
415 "P2P: #### Not ignoring unexpected GAS Initial Response from "
416 MACSTR " state %d", MAC2STR(sa), p2p->state);
417 }
418 if (p2p->sd_peer == NULL ||
419#else
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700420 if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
Dmitry Shmidt04949592012-07-19 12:16:46 -0700421#endif
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700422 os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
Dmitry Shmidt04949592012-07-19 12:16:46 -0700423 wpa_msg(p2p->cfg->msg_ctx, MSG_INFO,
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700424 "P2P: Ignore unexpected GAS Initial Response from "
425 MACSTR, MAC2STR(sa));
426 return;
427 }
428 p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
429 p2p_clear_timeout(p2p);
430
431 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
432 "P2P: Received GAS Initial Response from " MACSTR " (len=%d)",
433 MAC2STR(sa), (int) len);
434
435 if (len < 5 + 2) {
436 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
437 "P2P: Too short GAS Initial Response frame");
438 return;
439 }
440
441 dialog_token = *pos++;
442 /* TODO: check dialog_token match */
443 status_code = WPA_GET_LE16(pos);
444 pos += 2;
445 comeback_delay = WPA_GET_LE16(pos);
446 pos += 2;
447 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
448 "P2P: dialog_token=%u status_code=%u comeback_delay=%u",
449 dialog_token, status_code, comeback_delay);
450 if (status_code) {
451 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
452 "P2P: Service Discovery failed: status code %u",
453 status_code);
454 return;
455 }
456
457 if (*pos != WLAN_EID_ADV_PROTO) {
458 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
459 "P2P: Unexpected IE in GAS Initial Response: %u",
460 *pos);
461 return;
462 }
463 pos++;
464
465 slen = *pos++;
466 next = pos + slen;
467 if (next > end || slen < 2) {
468 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
469 "P2P: Invalid IE in GAS Initial Response");
470 return;
471 }
472 pos++; /* skip QueryRespLenLimit and PAME-BI */
473
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800474 if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700475 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
476 "P2P: Unsupported GAS advertisement protocol id %u",
477 *pos);
478 return;
479 }
480
481 pos = next;
482 /* Query Response */
483 if (pos + 2 > end) {
484 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
485 "Response");
486 return;
487 }
488 slen = WPA_GET_LE16(pos);
489 pos += 2;
490 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
491 slen);
492 if (pos + slen > end) {
493 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
494 "Response data");
495 return;
496 }
497 end = pos + slen;
498
499 if (comeback_delay) {
500 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Fragmented "
501 "response - request fragments");
502 if (p2p->sd_rx_resp) {
503 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop "
504 "old SD reassembly buffer");
505 wpabuf_free(p2p->sd_rx_resp);
506 p2p->sd_rx_resp = NULL;
507 }
508 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
509 return;
510 }
511
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800512 /* ANQP Query Response */
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700513 if (pos + 4 > end)
514 return;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800515 if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700516 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800517 "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700518 return;
519 }
520 pos += 2;
521
522 slen = WPA_GET_LE16(pos);
523 pos += 2;
524 if (pos + slen > end || slen < 3 + 1) {
525 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800526 "P2P: Invalid ANQP Query Response length");
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700527 return;
528 }
529
530 if (WPA_GET_BE24(pos) != OUI_WFA) {
531 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800532 "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700533 return;
534 }
535 pos += 3;
536
537 if (*pos != P2P_OUI_TYPE) {
538 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800539 "P2P: Unsupported ANQP vendor type %u", *pos);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700540 return;
541 }
542 pos++;
543
544 if (pos + 2 > end)
545 return;
546 update_indic = WPA_GET_LE16(pos);
547 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
548 "P2P: Service Update Indicator: %u", update_indic);
549 pos += 2;
550
551 p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
552 p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
553 p2p->sd_peer = NULL;
554
555 if (p2p->sd_query) {
556 if (!p2p->sd_query->for_all_peers) {
557 struct p2p_sd_query *q;
558 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
559 "P2P: Remove completed SD query %p",
560 p2p->sd_query);
561 q = p2p->sd_query;
562 p2p_unlink_sd_query(p2p, p2p->sd_query);
563 p2p_free_sd_query(q);
564 }
565 p2p->sd_query = NULL;
566 }
567
568 if (p2p->cfg->sd_response)
569 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
570 pos, end - pos);
571 p2p_continue_find(p2p);
572}
573
574
575void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
576 const u8 *data, size_t len, int rx_freq)
577{
578 struct wpabuf *resp;
579 u8 dialog_token;
580 size_t frag_len;
581 int more = 0;
582
583 wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
584 if (len < 1)
585 return;
586 dialog_token = *data;
587 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Dialog Token: %u",
588 dialog_token);
589 if (dialog_token != p2p->sd_resp_dialog_token) {
590 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
591 "response fragment for dialog token %u", dialog_token);
592 return;
593 }
594
595 if (p2p->sd_resp == NULL) {
596 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
597 "response fragment available");
598 return;
599 }
600 if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
601 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD "
602 "response fragment for " MACSTR, MAC2STR(sa));
603 return;
604 }
605
606 frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
607 if (frag_len > 1400) {
608 frag_len = 1400;
609 more = 1;
610 }
611 resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
612 p2p->srv_update_indic,
613 wpabuf_head_u8(p2p->sd_resp) +
614 p2p->sd_resp_pos, frag_len,
615 p2p->sd_frag_id, more,
616 wpabuf_len(p2p->sd_resp));
617 if (resp == NULL)
618 return;
619 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Send GAS Comeback "
620 "Response (frag_id %d more=%d frag_len=%d)",
621 p2p->sd_frag_id, more, (int) frag_len);
622 p2p->sd_frag_id++;
623 p2p->sd_resp_pos += frag_len;
624
625 if (more) {
626 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: %d more bytes "
627 "remain to be sent",
628 (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
629 } else {
630 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: All fragments of "
631 "SD response sent");
632 wpabuf_free(p2p->sd_resp);
633 p2p->sd_resp = NULL;
634 }
635
636 p2p->pending_action_state = P2P_NO_PENDING_ACTION;
637 if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr,
638 p2p->cfg->dev_addr,
639 wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
640 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
641 "P2P: Failed to send Action frame");
642
643 wpabuf_free(resp);
644}
645
646
647void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
648 const u8 *data, size_t len, int rx_freq)
649{
650 const u8 *pos = data;
651 const u8 *end = data + len;
652 const u8 *next;
653 u8 dialog_token;
654 u16 status_code;
655 u8 frag_id;
656 u8 more_frags;
657 u16 comeback_delay;
658 u16 slen;
659
660 wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
661
Dmitry Shmidt04949592012-07-19 12:16:46 -0700662#ifdef ANDROID_P2P
663 if (p2p->state != P2P_SD_DURING_FIND) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700664 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt04949592012-07-19 12:16:46 -0700665 "P2P: #### Not ignoring unexpected GAS Comeback Response from "
666 MACSTR " state %d", MAC2STR(sa), p2p->state);
667 }
668 if (p2p->sd_peer == NULL ||
669#else
670 if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
671#endif
672 os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
673 wpa_msg(p2p->cfg->msg_ctx, MSG_INFO,
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700674 "P2P: Ignore unexpected GAS Comeback Response from "
675 MACSTR, MAC2STR(sa));
676 return;
677 }
678 p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
679 p2p_clear_timeout(p2p);
680
681 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
682 "P2P: Received GAS Comeback Response from " MACSTR " (len=%d)",
683 MAC2STR(sa), (int) len);
684
685 if (len < 6 + 2) {
686 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
687 "P2P: Too short GAS Comeback Response frame");
688 return;
689 }
690
691 dialog_token = *pos++;
692 /* TODO: check dialog_token match */
693 status_code = WPA_GET_LE16(pos);
694 pos += 2;
695 frag_id = *pos & 0x7f;
696 more_frags = (*pos & 0x80) >> 7;
697 pos++;
698 comeback_delay = WPA_GET_LE16(pos);
699 pos += 2;
700 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
701 "P2P: dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
702 "comeback_delay=%u",
703 dialog_token, status_code, frag_id, more_frags,
704 comeback_delay);
705 /* TODO: check frag_id match */
706 if (status_code) {
707 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
708 "P2P: Service Discovery failed: status code %u",
709 status_code);
710 return;
711 }
712
713 if (*pos != WLAN_EID_ADV_PROTO) {
714 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
715 "P2P: Unexpected IE in GAS Comeback Response: %u",
716 *pos);
717 return;
718 }
719 pos++;
720
721 slen = *pos++;
722 next = pos + slen;
723 if (next > end || slen < 2) {
724 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
725 "P2P: Invalid IE in GAS Comeback Response");
726 return;
727 }
728 pos++; /* skip QueryRespLenLimit and PAME-BI */
729
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800730 if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700731 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
732 "P2P: Unsupported GAS advertisement protocol id %u",
733 *pos);
734 return;
735 }
736
737 pos = next;
738 /* Query Response */
739 if (pos + 2 > end) {
740 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query "
741 "Response");
742 return;
743 }
744 slen = WPA_GET_LE16(pos);
745 pos += 2;
746 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d",
747 slen);
748 if (pos + slen > end) {
749 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query "
750 "Response data");
751 return;
752 }
753 if (slen == 0) {
754 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No Query Response "
755 "data");
756 return;
757 }
758 end = pos + slen;
759
760 if (p2p->sd_rx_resp) {
761 /*
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800762 * ANQP header is only included in the first fragment; rest of
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700763 * the fragments start with continue TLVs.
764 */
765 goto skip_nqp_header;
766 }
767
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800768 /* ANQP Query Response */
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700769 if (pos + 4 > end)
770 return;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800771 if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700772 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800773 "P2P: Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700774 return;
775 }
776 pos += 2;
777
778 slen = WPA_GET_LE16(pos);
779 pos += 2;
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800780 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: ANQP Query Response "
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700781 "length: %u", slen);
782 if (slen < 3 + 1) {
783 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800784 "P2P: Invalid ANQP Query Response length");
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700785 return;
786 }
787 if (pos + 4 > end)
788 return;
789
790 if (WPA_GET_BE24(pos) != OUI_WFA) {
791 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800792 "P2P: Unsupported ANQP OUI %06x", WPA_GET_BE24(pos));
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700793 return;
794 }
795 pos += 3;
796
797 if (*pos != P2P_OUI_TYPE) {
798 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
Dmitry Shmidt1f69aa52012-01-24 16:10:04 -0800799 "P2P: Unsupported ANQP vendor type %u", *pos);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700800 return;
801 }
802 pos++;
803
804 if (pos + 2 > end)
805 return;
806 p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
807 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
808 "P2P: Service Update Indicator: %u", p2p->sd_rx_update_indic);
809 pos += 2;
810
811skip_nqp_header:
812 if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
813 return;
814 wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
815 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Current SD reassembly "
816 "buffer length: %u",
817 (unsigned int) wpabuf_len(p2p->sd_rx_resp));
818
819 if (more_frags) {
820 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: More fragments "
821 "remains");
822 /* TODO: what would be a good size limit? */
823 if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
824 wpabuf_free(p2p->sd_rx_resp);
825 p2p->sd_rx_resp = NULL;
826 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too long "
827 "SD response - drop it");
828 return;
829 }
830 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
831 return;
832 }
833
834 p2p->sd_peer->flags |= P2P_DEV_SD_INFO;
835 p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE;
836 p2p->sd_peer = NULL;
837
838 if (p2p->sd_query) {
839 if (!p2p->sd_query->for_all_peers) {
840 struct p2p_sd_query *q;
841 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
842 "P2P: Remove completed SD query %p",
843 p2p->sd_query);
844 q = p2p->sd_query;
845 p2p_unlink_sd_query(p2p, p2p->sd_query);
846 p2p_free_sd_query(q);
847 }
848 p2p->sd_query = NULL;
849 }
850
851 if (p2p->cfg->sd_response)
852 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
853 p2p->sd_rx_update_indic,
854 wpabuf_head(p2p->sd_rx_resp),
855 wpabuf_len(p2p->sd_rx_resp));
856 wpabuf_free(p2p->sd_rx_resp);
857 p2p->sd_rx_resp = NULL;
858
859 p2p_continue_find(p2p);
860}
861
862
863void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
864 const struct wpabuf *tlvs)
865{
866 struct p2p_sd_query *q;
Irfan Sherifff44b9c42012-06-13 11:09:23 -0700867#ifdef ANDROID_P2P
Dmitry Shmidt04949592012-07-19 12:16:46 -0700868 /* Currently, supplicant doesn't support more than one pending broadcast SD request.
Irfan Sherifff44b9c42012-06-13 11:09:23 -0700869 * So reject if application is registering another one before cancelling the existing one.
870 */
871 for (q = p2p->sd_queries; q; q = q->next) {
872 if( (q->for_all_peers == 1) && (!dst)) {
873 wpa_printf(MSG_ERROR, "P2P: Already one pending"
874 " Broadcast request. Please cancel the current one"
875 " before adding a new one");
876 return NULL;
877 }
878 }
879#endif
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700880
881 q = os_zalloc(sizeof(*q));
882 if (q == NULL)
883 return NULL;
884
885 if (dst)
886 os_memcpy(q->peer, dst, ETH_ALEN);
887 else
888 q->for_all_peers = 1;
889
890 q->tlvs = wpabuf_dup(tlvs);
891 if (q->tlvs == NULL) {
892 p2p_free_sd_query(q);
893 return NULL;
894 }
895
896 q->next = p2p->sd_queries;
897 p2p->sd_queries = q;
Dmitry Shmidt04949592012-07-19 12:16:46 -0700898 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p", q);
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700899
Irfan Sheriff96161872012-04-11 18:07:13 -0700900 if (dst == NULL) {
901 struct p2p_device *dev;
902 dl_list_for_each(dev, &p2p->devices, struct p2p_device, list)
903 dev->flags &= ~P2P_DEV_SD_INFO;
904 }
905
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700906 return q;
907}
908
909
910void p2p_sd_service_update(struct p2p_data *p2p)
911{
912 p2p->srv_update_indic++;
913}
914
915
916int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
917{
918 if (p2p_unlink_sd_query(p2p, req)) {
919 wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
920 "P2P: Cancel pending SD query %p", req);
Irfan Sherifff44b9c42012-06-13 11:09:23 -0700921#ifdef ANDROID_P2P
Dmitry Shmidt04949592012-07-19 12:16:46 -0700922 p2p->sd_dev_list = NULL;
Irfan Sherifff44b9c42012-06-13 11:09:23 -0700923#endif
Dmitry Shmidt8d520ff2011-05-09 14:06:53 -0700924 p2p_free_sd_query(req);
925 return 0;
926 }
927 return -1;
928}