]>
Commit | Line | Data |
---|---|---|
b22128ef JM |
1 | /* |
2 | * Wi-Fi Direct - P2P service discovery | |
3 | * Copyright (c) 2009, Atheros Communications | |
4 | * | |
5 | * This program is free software; you can redistribute it and/or modify | |
6 | * it under the terms of the GNU General Public License version 2 as | |
7 | * published by the Free Software Foundation. | |
8 | * | |
9 | * Alternatively, this software may be distributed under the terms of BSD | |
10 | * license. | |
11 | * | |
12 | * See README and COPYING for more details. | |
13 | */ | |
14 | ||
15 | #include "includes.h" | |
16 | ||
17 | #include "common.h" | |
18 | #include "common/ieee802_11_defs.h" | |
19 | #include "p2p_i.h" | |
20 | #include "p2p.h" | |
21 | ||
22 | ||
23 | struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p, | |
24 | struct p2p_device *dev) | |
25 | { | |
26 | struct p2p_sd_query *q; | |
27 | ||
28 | if (!(dev->dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY)) | |
29 | return 0; /* peer does not support SD */ | |
30 | ||
31 | for (q = p2p->sd_queries; q; q = q->next) { | |
32 | if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO)) | |
33 | return q; | |
34 | if (!q->for_all_peers && | |
35 | os_memcmp(q->peer, dev->p2p_device_addr, ETH_ALEN) == 0) | |
36 | return q; | |
37 | } | |
38 | ||
39 | return NULL; | |
40 | } | |
41 | ||
42 | ||
43 | static int p2p_unlink_sd_query(struct p2p_data *p2p, | |
44 | struct p2p_sd_query *query) | |
45 | { | |
46 | struct p2p_sd_query *q, *prev; | |
47 | q = p2p->sd_queries; | |
48 | prev = NULL; | |
49 | while (q) { | |
50 | if (q == query) { | |
51 | if (prev) | |
52 | prev->next = q->next; | |
53 | else | |
54 | p2p->sd_queries = q->next; | |
55 | if (p2p->sd_query == query) | |
56 | p2p->sd_query = NULL; | |
57 | return 1; | |
58 | } | |
59 | prev = q; | |
60 | q = q->next; | |
61 | } | |
62 | return 0; | |
63 | } | |
64 | ||
65 | ||
66 | static void p2p_free_sd_query(struct p2p_sd_query *q) | |
67 | { | |
68 | if (q == NULL) | |
69 | return; | |
70 | wpabuf_free(q->tlvs); | |
71 | os_free(q); | |
72 | } | |
73 | ||
74 | ||
75 | void p2p_free_sd_queries(struct p2p_data *p2p) | |
76 | { | |
77 | struct p2p_sd_query *q, *prev; | |
78 | q = p2p->sd_queries; | |
79 | p2p->sd_queries = NULL; | |
80 | while (q) { | |
81 | prev = q; | |
82 | q = q->next; | |
83 | p2p_free_sd_query(prev); | |
84 | } | |
85 | } | |
86 | ||
87 | ||
88 | static struct wpabuf * p2p_build_sd_query(u16 update_indic, | |
89 | struct wpabuf *tlvs) | |
90 | { | |
91 | struct wpabuf *buf; | |
92 | u8 *len_pos, *len_pos2; | |
93 | ||
94 | buf = wpabuf_alloc(1000 + wpabuf_len(tlvs)); | |
95 | if (buf == NULL) | |
96 | return NULL; | |
97 | ||
98 | wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC); | |
99 | wpabuf_put_u8(buf, WLAN_PA_GAS_INITIAL_REQ); | |
100 | wpabuf_put_u8(buf, 0); /* Dialog Token */ | |
101 | ||
102 | /* Advertisement Protocol IE */ | |
103 | wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO); | |
104 | wpabuf_put_u8(buf, 2); /* Length */ | |
105 | wpabuf_put_u8(buf, 0); /* QueryRespLenLimit | PAME-BI */ | |
106 | wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */ | |
107 | ||
108 | /* Query Request */ | |
109 | len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */ | |
110 | ||
111 | /* NQP Query Request Frame */ | |
112 | wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */ | |
113 | len_pos2 = wpabuf_put(buf, 2); /* Length (to be filled) */ | |
114 | wpabuf_put_be24(buf, OUI_WFA); | |
115 | wpabuf_put_u8(buf, P2P_OUI_TYPE); | |
116 | wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */ | |
117 | wpabuf_put_buf(buf, tlvs); | |
118 | ||
119 | WPA_PUT_LE16(len_pos2, (u8 *) wpabuf_put(buf, 0) - len_pos2 - 2); | |
120 | WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2); | |
121 | ||
122 | return buf; | |
123 | } | |
124 | ||
125 | ||
126 | static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code, | |
127 | u16 update_indic, | |
128 | const struct wpabuf *tlvs) | |
129 | { | |
130 | struct wpabuf *buf; | |
131 | u8 *len_pos, *len_pos2; | |
132 | ||
133 | buf = wpabuf_alloc(1000 + wpabuf_len(tlvs)); | |
134 | if (buf == NULL) | |
135 | return NULL; | |
136 | ||
137 | wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC); | |
138 | wpabuf_put_u8(buf, WLAN_PA_GAS_INITIAL_RESP); | |
139 | wpabuf_put_u8(buf, dialog_token); | |
140 | wpabuf_put_le16(buf, status_code); | |
141 | wpabuf_put_le16(buf, 0); /* GAS Comeback Delay */ | |
142 | ||
143 | /* Advertisement Protocol IE */ | |
144 | wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO); | |
145 | wpabuf_put_u8(buf, 2); /* Length */ | |
146 | wpabuf_put_u8(buf, 0x7f); /* QueryRespLenLimit | PAME-BI */ | |
147 | wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */ | |
148 | ||
149 | /* Query Response */ | |
150 | len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */ | |
151 | ||
152 | /* NQP Query Response Frame */ | |
153 | wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */ | |
154 | len_pos2 = wpabuf_put(buf, 2); /* Length (to be filled) */ | |
155 | wpabuf_put_be24(buf, OUI_WFA); | |
156 | wpabuf_put_u8(buf, P2P_OUI_TYPE); | |
157 | wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */ | |
158 | wpabuf_put_buf(buf, tlvs); | |
159 | ||
160 | WPA_PUT_LE16(len_pos2, (u8 *) wpabuf_put(buf, 0) - len_pos2 - 2); | |
161 | WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2); | |
162 | ||
163 | return buf; | |
164 | } | |
165 | ||
166 | ||
167 | int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev) | |
168 | { | |
169 | struct wpabuf *req; | |
170 | int ret = 0; | |
171 | struct p2p_sd_query *query; | |
172 | int freq; | |
173 | ||
174 | freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq; | |
175 | if (freq <= 0) { | |
176 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
177 | "P2P: No Listen/Operating frequency known for the " | |
178 | "peer " MACSTR " to send SD Request", | |
179 | MAC2STR(dev->p2p_device_addr)); | |
180 | return -1; | |
181 | } | |
182 | ||
183 | query = p2p_pending_sd_req(p2p, dev); | |
184 | if (query == NULL) | |
185 | return -1; | |
186 | ||
187 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
188 | "P2P: Start Service Discovery with " MACSTR, | |
189 | MAC2STR(dev->p2p_device_addr)); | |
190 | ||
191 | req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs); | |
192 | if (req == NULL) | |
193 | return -1; | |
194 | ||
195 | p2p->sd_peer = dev; | |
196 | p2p->sd_query = query; | |
197 | p2p->pending_action_state = P2P_PENDING_SD; | |
198 | ||
199 | if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, | |
200 | dev->p2p_device_addr, p2p->cfg->dev_addr, | |
201 | dev->p2p_device_addr, | |
202 | wpabuf_head(req), wpabuf_len(req), 5000) < 0) | |
203 | { | |
204 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
205 | "P2P: Failed to send Action frame"); | |
206 | ret = -1; | |
207 | } | |
208 | ||
209 | wpabuf_free(req); | |
210 | ||
211 | return ret; | |
212 | } | |
213 | ||
214 | ||
215 | void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa, | |
216 | const u8 *data, size_t len, int rx_freq) | |
217 | { | |
218 | const u8 *pos = data; | |
219 | const u8 *end = data + len; | |
220 | const u8 *next; | |
221 | u8 dialog_token; | |
222 | u16 slen; | |
223 | int freq; | |
224 | u16 update_indic; | |
225 | ||
226 | ||
227 | if (p2p->cfg->sd_request == NULL) | |
228 | return; | |
229 | ||
230 | if (rx_freq > 0) | |
231 | freq = rx_freq; | |
232 | else | |
233 | freq = p2p_channel_to_freq(p2p->cfg->country, | |
234 | p2p->cfg->reg_class, | |
235 | p2p->cfg->channel); | |
236 | if (freq < 0) | |
237 | return; | |
238 | ||
239 | if (len < 1 + 2) | |
240 | return; | |
241 | ||
242 | dialog_token = *pos++; | |
243 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
244 | "P2P: GAS Initial Request from " MACSTR " (dialog token %u, " | |
245 | "freq %d)", | |
246 | MAC2STR(sa), dialog_token, rx_freq); | |
247 | ||
248 | if (*pos != WLAN_EID_ADV_PROTO) { | |
249 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
250 | "P2P: Unexpected IE in GAS Initial Request: %u", *pos); | |
251 | return; | |
252 | } | |
253 | pos++; | |
254 | ||
255 | slen = *pos++; | |
256 | next = pos + slen; | |
257 | if (next > end || slen < 2) { | |
258 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
259 | "P2P: Invalid IE in GAS Initial Request"); | |
260 | return; | |
261 | } | |
262 | pos++; /* skip QueryRespLenLimit and PAME-BI */ | |
263 | ||
264 | if (*pos != NATIVE_QUERY_PROTOCOL) { | |
265 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
266 | "P2P: Unsupported GAS advertisement protocol id %u", | |
267 | *pos); | |
268 | return; | |
269 | } | |
270 | ||
271 | pos = next; | |
272 | /* Query Request */ | |
273 | if (pos + 2 > end) | |
274 | return; | |
275 | slen = WPA_GET_LE16(pos); | |
276 | pos += 2; | |
277 | if (pos + slen > end) | |
278 | return; | |
279 | end = pos + slen; | |
280 | ||
281 | /* NQP Query Request */ | |
282 | if (pos + 4 > end) | |
283 | return; | |
284 | if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) { | |
285 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
286 | "P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos)); | |
287 | return; | |
288 | } | |
289 | pos += 2; | |
290 | ||
291 | slen = WPA_GET_LE16(pos); | |
292 | pos += 2; | |
293 | if (pos + slen > end || slen < 3 + 1) { | |
294 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
295 | "P2P: Invalid NQP Query Request length"); | |
296 | return; | |
297 | } | |
298 | ||
299 | if (WPA_GET_BE24(pos) != OUI_WFA) { | |
300 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
301 | "P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos)); | |
302 | return; | |
303 | } | |
304 | pos += 3; | |
305 | ||
306 | if (*pos != P2P_OUI_TYPE) { | |
307 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
308 | "P2P: Unsupported NQP vendor type %u", *pos); | |
309 | return; | |
310 | } | |
311 | pos++; | |
312 | ||
313 | if (pos + 2 > end) | |
314 | return; | |
315 | update_indic = WPA_GET_LE16(pos); | |
316 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
317 | "P2P: Service Update Indicator: %u", update_indic); | |
318 | pos += 2; | |
319 | ||
320 | p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token, | |
321 | update_indic, pos, end - pos); | |
322 | /* the response will be indicated with a call to p2p_sd_response() */ | |
323 | } | |
324 | ||
325 | ||
326 | void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst, | |
327 | u8 dialog_token, const struct wpabuf *resp_tlvs) | |
328 | { | |
329 | struct wpabuf *resp; | |
330 | resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS, | |
331 | p2p->srv_update_indic, resp_tlvs); | |
332 | if (resp == NULL) | |
333 | return; | |
334 | ||
335 | p2p->pending_action_state = P2P_NO_PENDING_ACTION; | |
336 | if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, | |
337 | dst, p2p->cfg->dev_addr, p2p->cfg->dev_addr, | |
338 | wpabuf_head(resp), wpabuf_len(resp), 200) < | |
339 | 0) | |
340 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
341 | "P2P: Failed to send Action frame"); | |
342 | ||
343 | wpabuf_free(resp); | |
344 | } | |
345 | ||
346 | ||
347 | void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa, | |
348 | const u8 *data, size_t len) | |
349 | { | |
350 | const u8 *pos = data; | |
351 | const u8 *end = data + len; | |
352 | const u8 *next; | |
353 | u8 dialog_token; | |
354 | u16 status_code; | |
355 | u16 comeback_delay; | |
356 | u16 slen; | |
357 | u16 update_indic; | |
358 | ||
359 | if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || | |
360 | os_memcmp(sa, p2p->sd_peer->p2p_device_addr, ETH_ALEN) != 0) { | |
361 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
362 | "P2P: Ignore unexpected GAS Initial Response from " | |
363 | MACSTR, MAC2STR(sa)); | |
364 | return; | |
365 | } | |
366 | p2p->cfg->send_action_done(p2p->cfg->cb_ctx); | |
367 | p2p_clear_timeout(p2p); | |
368 | ||
369 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
370 | "P2P: Received GAS Initial Response from " MACSTR, | |
371 | MAC2STR(sa)); | |
372 | ||
373 | if (len < 5 + 2) | |
374 | return; | |
375 | ||
376 | dialog_token = *pos++; | |
377 | /* TODO: check dialog_token match */ | |
378 | status_code = WPA_GET_LE16(pos); | |
379 | pos += 2; | |
380 | comeback_delay = WPA_GET_LE16(pos); | |
381 | pos += 2; | |
382 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
383 | "P2P: dialog_token=%u status_code=%u comeback_delay=%u", | |
384 | dialog_token, status_code, comeback_delay); | |
385 | ||
386 | if (*pos != WLAN_EID_ADV_PROTO) { | |
387 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
388 | "P2P: Unexpected IE in GAS Initial Response: %u", | |
389 | *pos); | |
390 | return; | |
391 | } | |
392 | pos++; | |
393 | ||
394 | slen = *pos++; | |
395 | next = pos + slen; | |
396 | if (next > end || slen < 2) { | |
397 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
398 | "P2P: Invalid IE in GAS Initial Response"); | |
399 | return; | |
400 | } | |
401 | pos++; /* skip QueryRespLenLimit and PAME-BI */ | |
402 | ||
403 | if (*pos != NATIVE_QUERY_PROTOCOL) { | |
404 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
405 | "P2P: Unsupported GAS advertisement protocol id %u", | |
406 | *pos); | |
407 | return; | |
408 | } | |
409 | ||
410 | pos = next; | |
411 | /* Query Response */ | |
412 | if (pos + 2 > end) | |
413 | return; | |
414 | slen = WPA_GET_LE16(pos); | |
415 | pos += 2; | |
416 | if (pos + slen > end) | |
417 | return; | |
418 | end = pos + slen; | |
419 | ||
420 | /* NQP Query Response */ | |
421 | if (pos + 4 > end) | |
422 | return; | |
423 | if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) { | |
424 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
425 | "P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos)); | |
426 | return; | |
427 | } | |
428 | pos += 2; | |
429 | ||
430 | slen = WPA_GET_LE16(pos); | |
431 | pos += 2; | |
432 | if (pos + slen > end || slen < 3 + 1) { | |
433 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
434 | "P2P: Invalid NQP Query Response length"); | |
435 | return; | |
436 | } | |
437 | ||
438 | if (WPA_GET_BE24(pos) != OUI_WFA) { | |
439 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
440 | "P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos)); | |
441 | return; | |
442 | } | |
443 | pos += 3; | |
444 | ||
445 | if (*pos != P2P_OUI_TYPE) { | |
446 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
447 | "P2P: Unsupported NQP vendor type %u", *pos); | |
448 | return; | |
449 | } | |
450 | pos++; | |
451 | ||
452 | if (pos + 2 > end) | |
453 | return; | |
454 | update_indic = WPA_GET_LE16(pos); | |
455 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
456 | "P2P: Service Update Indicator: %u", update_indic); | |
457 | pos += 2; | |
458 | ||
459 | p2p->sd_peer->flags |= P2P_DEV_SD_INFO; | |
460 | p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE; | |
461 | p2p->sd_peer = NULL; | |
462 | ||
463 | if (p2p->sd_query) { | |
464 | if (!p2p->sd_query->for_all_peers) { | |
465 | struct p2p_sd_query *q; | |
466 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
467 | "P2P: Remove completed SD query %p", | |
468 | p2p->sd_query); | |
469 | q = p2p->sd_query; | |
470 | p2p_unlink_sd_query(p2p, p2p->sd_query); | |
471 | p2p_free_sd_query(q); | |
472 | } | |
473 | p2p->sd_query = NULL; | |
474 | } | |
475 | ||
476 | if (p2p->cfg->sd_response) | |
477 | p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic, | |
478 | pos, end - pos); | |
479 | p2p_continue_find(p2p); | |
480 | } | |
481 | ||
482 | ||
483 | void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst, | |
484 | const struct wpabuf *tlvs) | |
485 | { | |
486 | struct p2p_sd_query *q; | |
487 | ||
488 | q = os_zalloc(sizeof(*q)); | |
489 | if (q == NULL) | |
490 | return NULL; | |
491 | ||
492 | if (dst) | |
493 | os_memcpy(q->peer, dst, ETH_ALEN); | |
494 | else | |
495 | q->for_all_peers = 1; | |
496 | ||
497 | q->tlvs = wpabuf_dup(tlvs); | |
498 | if (q->tlvs == NULL) { | |
499 | p2p_free_sd_query(q); | |
500 | return NULL; | |
501 | } | |
502 | ||
503 | q->next = p2p->sd_queries; | |
504 | p2p->sd_queries = q; | |
505 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p", q); | |
506 | ||
507 | return q; | |
508 | } | |
509 | ||
510 | ||
511 | void p2p_sd_service_update(struct p2p_data *p2p) | |
512 | { | |
513 | p2p->srv_update_indic++; | |
514 | } | |
515 | ||
516 | ||
517 | int p2p_sd_cancel_request(struct p2p_data *p2p, void *req) | |
518 | { | |
519 | if (p2p_unlink_sd_query(p2p, req)) { | |
520 | wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, | |
521 | "P2P: Cancel pending SD query %p", req); | |
522 | p2p_free_sd_query(req); | |
523 | return 0; | |
524 | } | |
525 | return -1; | |
526 | } |