]>
Commit | Line | Data |
---|---|---|
9675ce35 JM |
1 | /* |
2 | * wpa_supplicant - Wi-Fi Display | |
3 | * Copyright (c) 2011, Atheros Communications, Inc. | |
4 | * Copyright (c) 2011-2012, Qualcomm Atheros, Inc. | |
5 | * | |
6 | * This software may be distributed under the terms of the BSD license. | |
7 | * See README for more details. | |
8 | */ | |
9 | ||
10 | #include "includes.h" | |
11 | ||
12 | #include "common.h" | |
13 | #include "p2p/p2p.h" | |
14 | #include "common/ieee802_11_defs.h" | |
15 | #include "wpa_supplicant_i.h" | |
16 | #include "wifi_display.h" | |
17 | ||
18 | ||
b125c48f DS |
19 | #define WIFI_DISPLAY_SUBELEM_HEADER_LEN 3 |
20 | ||
21 | ||
9675ce35 JM |
22 | int wifi_display_init(struct wpa_global *global) |
23 | { | |
24 | global->wifi_display = 1; | |
25 | return 0; | |
26 | } | |
27 | ||
28 | ||
29 | void wifi_display_deinit(struct wpa_global *global) | |
30 | { | |
31 | int i; | |
32 | for (i = 0; i < MAX_WFD_SUBELEMS; i++) { | |
33 | wpabuf_free(global->wfd_subelem[i]); | |
34 | global->wfd_subelem[i] = NULL; | |
35 | } | |
36 | } | |
37 | ||
38 | ||
d4177443 TB |
39 | struct wpabuf * wifi_display_get_wfd_ie(struct wpa_global *global) |
40 | { | |
41 | struct wpabuf *ie; | |
42 | size_t len; | |
43 | int i; | |
44 | ||
45 | if (global->p2p == NULL) | |
46 | return NULL; | |
47 | ||
48 | len = 0; | |
49 | for (i = 0; i < MAX_WFD_SUBELEMS; i++) { | |
50 | if (global->wfd_subelem[i]) | |
51 | len += wpabuf_len(global->wfd_subelem[i]); | |
52 | } | |
53 | ||
54 | ie = wpabuf_alloc(len); | |
55 | if (ie == NULL) | |
56 | return NULL; | |
57 | ||
58 | for (i = 0; i < MAX_WFD_SUBELEMS; i++) { | |
59 | if (global->wfd_subelem[i]) | |
60 | wpabuf_put_buf(ie, global->wfd_subelem[i]); | |
61 | } | |
62 | ||
63 | return ie; | |
64 | } | |
65 | ||
66 | ||
9675ce35 JM |
67 | static int wifi_display_update_wfd_ie(struct wpa_global *global) |
68 | { | |
69 | struct wpabuf *ie, *buf; | |
70 | size_t len, plen; | |
71 | ||
bab6677a JM |
72 | if (global->p2p == NULL) |
73 | return 0; | |
74 | ||
9675ce35 JM |
75 | wpa_printf(MSG_DEBUG, "WFD: Update WFD IE"); |
76 | ||
77 | if (!global->wifi_display) { | |
78 | wpa_printf(MSG_DEBUG, "WFD: Wi-Fi Display disabled - do not " | |
79 | "include WFD IE"); | |
80 | p2p_set_wfd_ie_beacon(global->p2p, NULL); | |
81 | p2p_set_wfd_ie_probe_req(global->p2p, NULL); | |
82 | p2p_set_wfd_ie_probe_resp(global->p2p, NULL); | |
83 | p2p_set_wfd_ie_assoc_req(global->p2p, NULL); | |
84 | p2p_set_wfd_ie_invitation(global->p2p, NULL); | |
85 | p2p_set_wfd_ie_prov_disc_req(global->p2p, NULL); | |
86 | p2p_set_wfd_ie_prov_disc_resp(global->p2p, NULL); | |
87 | p2p_set_wfd_ie_go_neg(global->p2p, NULL); | |
88 | p2p_set_wfd_dev_info(global->p2p, NULL); | |
e9518ae7 | 89 | p2p_set_wfd_r2_dev_info(global->p2p, NULL); |
9675ce35 JM |
90 | p2p_set_wfd_assoc_bssid(global->p2p, NULL); |
91 | p2p_set_wfd_coupled_sink_info(global->p2p, NULL); | |
92 | return 0; | |
93 | } | |
94 | ||
95 | p2p_set_wfd_dev_info(global->p2p, | |
96 | global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]); | |
e9518ae7 AHS |
97 | p2p_set_wfd_r2_dev_info( |
98 | global->p2p, global->wfd_subelem[WFD_SUBELEM_R2_DEVICE_INFO]); | |
9675ce35 JM |
99 | p2p_set_wfd_assoc_bssid( |
100 | global->p2p, | |
101 | global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID]); | |
102 | p2p_set_wfd_coupled_sink_info( | |
103 | global->p2p, global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]); | |
104 | ||
105 | /* | |
106 | * WFD IE is included in number of management frames. Two different | |
107 | * sets of subelements are included depending on the frame: | |
108 | * | |
109 | * Beacon, (Re)Association Request, GO Negotiation Req/Resp/Conf, | |
110 | * Provision Discovery Req: | |
111 | * WFD Device Info | |
112 | * [Associated BSSID] | |
113 | * [Coupled Sink Info] | |
114 | * | |
115 | * Probe Request: | |
116 | * WFD Device Info | |
117 | * [Associated BSSID] | |
118 | * [Coupled Sink Info] | |
119 | * [WFD Extended Capability] | |
120 | * | |
121 | * Probe Response: | |
122 | * WFD Device Info | |
123 | * [Associated BSSID] | |
124 | * [Coupled Sink Info] | |
125 | * [WFD Extended Capability] | |
126 | * [WFD Session Info] | |
127 | * | |
128 | * (Re)Association Response, P2P Invitation Req/Resp, | |
129 | * Provision Discovery Resp: | |
130 | * WFD Device Info | |
131 | * [Associated BSSID] | |
132 | * [Coupled Sink Info] | |
133 | * [WFD Session Info] | |
134 | */ | |
135 | len = 0; | |
136 | if (global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]) | |
137 | len += wpabuf_len(global->wfd_subelem[ | |
138 | WFD_SUBELEM_DEVICE_INFO]); | |
e9518ae7 AHS |
139 | |
140 | if (global->wfd_subelem[WFD_SUBELEM_R2_DEVICE_INFO]) | |
141 | len += wpabuf_len(global->wfd_subelem[ | |
142 | WFD_SUBELEM_R2_DEVICE_INFO]); | |
143 | ||
9675ce35 JM |
144 | if (global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID]) |
145 | len += wpabuf_len(global->wfd_subelem[ | |
146 | WFD_SUBELEM_ASSOCIATED_BSSID]); | |
147 | if (global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]) | |
148 | len += wpabuf_len(global->wfd_subelem[ | |
149 | WFD_SUBELEM_COUPLED_SINK]); | |
150 | if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]) | |
151 | len += wpabuf_len(global->wfd_subelem[ | |
152 | WFD_SUBELEM_SESSION_INFO]); | |
153 | if (global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]) | |
154 | len += wpabuf_len(global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]); | |
155 | buf = wpabuf_alloc(len); | |
156 | if (buf == NULL) | |
157 | return -1; | |
158 | ||
159 | if (global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]) | |
160 | wpabuf_put_buf(buf, | |
161 | global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]); | |
e9518ae7 AHS |
162 | |
163 | if (global->wfd_subelem[WFD_SUBELEM_R2_DEVICE_INFO]) | |
164 | wpabuf_put_buf(buf, | |
165 | global->wfd_subelem[WFD_SUBELEM_R2_DEVICE_INFO]); | |
166 | ||
9675ce35 JM |
167 | if (global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID]) |
168 | wpabuf_put_buf(buf, global->wfd_subelem[ | |
169 | WFD_SUBELEM_ASSOCIATED_BSSID]); | |
170 | if (global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]) | |
171 | wpabuf_put_buf(buf, | |
172 | global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]); | |
173 | ||
174 | ie = wifi_display_encaps(buf); | |
175 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Beacon", ie); | |
176 | p2p_set_wfd_ie_beacon(global->p2p, ie); | |
177 | ||
178 | ie = wifi_display_encaps(buf); | |
179 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for (Re)Association Request", | |
180 | ie); | |
181 | p2p_set_wfd_ie_assoc_req(global->p2p, ie); | |
182 | ||
183 | ie = wifi_display_encaps(buf); | |
184 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for GO Negotiation", ie); | |
185 | p2p_set_wfd_ie_go_neg(global->p2p, ie); | |
186 | ||
187 | ie = wifi_display_encaps(buf); | |
188 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Provision Discovery " | |
189 | "Request", ie); | |
190 | p2p_set_wfd_ie_prov_disc_req(global->p2p, ie); | |
191 | ||
192 | plen = buf->used; | |
193 | if (global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]) | |
194 | wpabuf_put_buf(buf, | |
195 | global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]); | |
196 | ||
197 | ie = wifi_display_encaps(buf); | |
198 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Probe Request", ie); | |
199 | p2p_set_wfd_ie_probe_req(global->p2p, ie); | |
200 | ||
201 | if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]) | |
202 | wpabuf_put_buf(buf, | |
203 | global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]); | |
204 | ie = wifi_display_encaps(buf); | |
205 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Probe Response", ie); | |
206 | p2p_set_wfd_ie_probe_resp(global->p2p, ie); | |
207 | ||
208 | /* Remove WFD Extended Capability from buffer */ | |
209 | buf->used = plen; | |
210 | if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]) | |
211 | wpabuf_put_buf(buf, | |
212 | global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]); | |
213 | ||
214 | ie = wifi_display_encaps(buf); | |
215 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for P2P Invitation", ie); | |
216 | p2p_set_wfd_ie_invitation(global->p2p, ie); | |
217 | ||
218 | ie = wifi_display_encaps(buf); | |
219 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Provision Discovery " | |
220 | "Response", ie); | |
221 | p2p_set_wfd_ie_prov_disc_resp(global->p2p, ie); | |
222 | ||
223 | wpabuf_free(buf); | |
224 | ||
225 | return 0; | |
226 | } | |
227 | ||
228 | ||
229 | void wifi_display_enable(struct wpa_global *global, int enabled) | |
230 | { | |
231 | wpa_printf(MSG_DEBUG, "WFD: Wi-Fi Display %s", | |
232 | enabled ? "enabled" : "disabled"); | |
233 | global->wifi_display = enabled; | |
234 | wifi_display_update_wfd_ie(global); | |
235 | } | |
236 | ||
237 | ||
238 | int wifi_display_subelem_set(struct wpa_global *global, char *cmd) | |
239 | { | |
240 | char *pos; | |
241 | int subelem; | |
242 | size_t len; | |
243 | struct wpabuf *e; | |
244 | ||
245 | pos = os_strchr(cmd, ' '); | |
246 | if (pos == NULL) | |
247 | return -1; | |
248 | *pos++ = '\0'; | |
9675ce35 JM |
249 | |
250 | len = os_strlen(pos); | |
251 | if (len & 1) | |
252 | return -1; | |
253 | len /= 2; | |
254 | ||
bc0ba01d JM |
255 | if (os_strcmp(cmd, "all") == 0) { |
256 | int res; | |
257 | ||
258 | e = wpabuf_alloc(len); | |
259 | if (e == NULL) | |
260 | return -1; | |
261 | if (hexstr2bin(pos, wpabuf_put(e, len), len) < 0) { | |
262 | wpabuf_free(e); | |
263 | return -1; | |
264 | } | |
265 | res = wifi_display_subelem_set_from_ies(global, e); | |
266 | wpabuf_free(e); | |
267 | return res; | |
268 | } | |
269 | ||
270 | subelem = atoi(cmd); | |
271 | if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS) | |
272 | return -1; | |
273 | ||
9675ce35 JM |
274 | if (len == 0) { |
275 | /* Clear subelement */ | |
276 | e = NULL; | |
277 | wpa_printf(MSG_DEBUG, "WFD: Clear subelement %d", subelem); | |
278 | } else { | |
279 | e = wpabuf_alloc(1 + len); | |
280 | if (e == NULL) | |
281 | return -1; | |
282 | wpabuf_put_u8(e, subelem); | |
283 | if (hexstr2bin(pos, wpabuf_put(e, len), len) < 0) { | |
284 | wpabuf_free(e); | |
285 | return -1; | |
286 | } | |
287 | wpa_printf(MSG_DEBUG, "WFD: Set subelement %d", subelem); | |
288 | } | |
289 | ||
290 | wpabuf_free(global->wfd_subelem[subelem]); | |
291 | global->wfd_subelem[subelem] = e; | |
292 | wifi_display_update_wfd_ie(global); | |
293 | ||
294 | return 0; | |
295 | } | |
296 | ||
297 | ||
4bd7e161 TB |
298 | int wifi_display_subelem_set_from_ies(struct wpa_global *global, |
299 | struct wpabuf *ie) | |
300 | { | |
301 | int subelements[MAX_WFD_SUBELEMS] = {}; | |
302 | const u8 *pos, *end; | |
4debeb42 | 303 | unsigned int len, subelem; |
4bd7e161 TB |
304 | struct wpabuf *e; |
305 | ||
306 | wpa_printf(MSG_DEBUG, "WFD IEs set: %p - %lu", | |
307 | ie, ie ? (unsigned long) wpabuf_len(ie) : 0); | |
308 | ||
309 | if (ie == NULL || wpabuf_len(ie) < 6) | |
310 | return -1; | |
311 | ||
312 | pos = wpabuf_head(ie); | |
313 | end = pos + wpabuf_len(ie); | |
314 | ||
315 | while (end > pos) { | |
316 | if (pos + 3 > end) | |
317 | break; | |
318 | ||
319 | len = WPA_GET_BE16(pos + 1) + 3; | |
320 | ||
321 | wpa_printf(MSG_DEBUG, "WFD Sub-Element ID %d - len %d", | |
322 | *pos, len - 3); | |
323 | ||
bd43e67e | 324 | if (len > (unsigned int) (end - pos)) |
4bd7e161 TB |
325 | break; |
326 | ||
327 | subelem = *pos; | |
328 | if (subelem < MAX_WFD_SUBELEMS && subelements[subelem] == 0) { | |
329 | e = wpabuf_alloc_copy(pos, len); | |
330 | if (e == NULL) | |
331 | return -1; | |
332 | ||
333 | wpabuf_free(global->wfd_subelem[subelem]); | |
334 | global->wfd_subelem[subelem] = e; | |
335 | subelements[subelem] = 1; | |
336 | } | |
337 | ||
338 | pos += len; | |
339 | } | |
340 | ||
341 | for (subelem = 0; subelem < MAX_WFD_SUBELEMS; subelem++) { | |
342 | if (subelements[subelem] == 0) { | |
343 | wpabuf_free(global->wfd_subelem[subelem]); | |
344 | global->wfd_subelem[subelem] = NULL; | |
345 | } | |
346 | } | |
347 | ||
348 | return wifi_display_update_wfd_ie(global); | |
349 | } | |
350 | ||
351 | ||
9675ce35 JM |
352 | int wifi_display_subelem_get(struct wpa_global *global, char *cmd, |
353 | char *buf, size_t buflen) | |
354 | { | |
355 | int subelem; | |
356 | ||
bc0ba01d JM |
357 | if (os_strcmp(cmd, "all") == 0) { |
358 | struct wpabuf *ie; | |
359 | int res; | |
360 | ||
361 | ie = wifi_display_get_wfd_ie(global); | |
362 | if (ie == NULL) | |
363 | return 0; | |
364 | res = wpa_snprintf_hex(buf, buflen, wpabuf_head(ie), | |
365 | wpabuf_len(ie)); | |
366 | wpabuf_free(ie); | |
367 | return res; | |
368 | } | |
369 | ||
9675ce35 JM |
370 | subelem = atoi(cmd); |
371 | if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS) | |
372 | return -1; | |
373 | ||
374 | if (global->wfd_subelem[subelem] == NULL) | |
375 | return 0; | |
376 | ||
377 | return wpa_snprintf_hex(buf, buflen, | |
378 | wpabuf_head_u8(global->wfd_subelem[subelem]) + | |
379 | 1, | |
380 | wpabuf_len(global->wfd_subelem[subelem]) - 1); | |
381 | } | |
b125c48f DS |
382 | |
383 | ||
384 | char * wifi_display_subelem_hex(const struct wpabuf *wfd_subelems, u8 id) | |
385 | { | |
386 | char *subelem = NULL; | |
387 | const u8 *buf; | |
388 | size_t buflen; | |
389 | size_t i = 0; | |
390 | u16 elen; | |
391 | ||
392 | if (!wfd_subelems) | |
393 | return NULL; | |
394 | ||
395 | buf = wpabuf_head_u8(wfd_subelems); | |
396 | if (!buf) | |
397 | return NULL; | |
398 | ||
399 | buflen = wpabuf_len(wfd_subelems); | |
400 | ||
401 | while (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN < buflen) { | |
402 | elen = WPA_GET_BE16(buf + i + 1); | |
49d13df6 JM |
403 | if (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN + elen > buflen) |
404 | break; /* truncated subelement */ | |
b125c48f DS |
405 | |
406 | if (buf[i] == id) { | |
d3fa2bbb JM |
407 | /* |
408 | * Limit explicitly to an arbitrary length to avoid | |
409 | * unnecessarily large allocations. In practice, this | |
410 | * is limited to maximum frame length anyway, so the | |
411 | * maximum memory allocation here is not really that | |
412 | * large. Anyway, the Wi-Fi Display subelements that | |
413 | * are fetched with this function are even shorter. | |
414 | */ | |
415 | if (elen > 1000) | |
416 | break; | |
b125c48f DS |
417 | subelem = os_zalloc(2 * elen + 1); |
418 | if (!subelem) | |
419 | return NULL; | |
420 | wpa_snprintf_hex(subelem, 2 * elen + 1, | |
421 | buf + i + | |
422 | WIFI_DISPLAY_SUBELEM_HEADER_LEN, | |
423 | elen); | |
424 | break; | |
425 | } | |
426 | ||
427 | i += elen + WIFI_DISPLAY_SUBELEM_HEADER_LEN; | |
428 | } | |
429 | ||
430 | return subelem; | |
431 | } |