]>
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); | |
89 | p2p_set_wfd_assoc_bssid(global->p2p, NULL); | |
90 | p2p_set_wfd_coupled_sink_info(global->p2p, NULL); | |
91 | return 0; | |
92 | } | |
93 | ||
94 | p2p_set_wfd_dev_info(global->p2p, | |
95 | global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]); | |
96 | p2p_set_wfd_assoc_bssid( | |
97 | global->p2p, | |
98 | global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID]); | |
99 | p2p_set_wfd_coupled_sink_info( | |
100 | global->p2p, global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]); | |
101 | ||
102 | /* | |
103 | * WFD IE is included in number of management frames. Two different | |
104 | * sets of subelements are included depending on the frame: | |
105 | * | |
106 | * Beacon, (Re)Association Request, GO Negotiation Req/Resp/Conf, | |
107 | * Provision Discovery Req: | |
108 | * WFD Device Info | |
109 | * [Associated BSSID] | |
110 | * [Coupled Sink Info] | |
111 | * | |
112 | * Probe Request: | |
113 | * WFD Device Info | |
114 | * [Associated BSSID] | |
115 | * [Coupled Sink Info] | |
116 | * [WFD Extended Capability] | |
117 | * | |
118 | * Probe Response: | |
119 | * WFD Device Info | |
120 | * [Associated BSSID] | |
121 | * [Coupled Sink Info] | |
122 | * [WFD Extended Capability] | |
123 | * [WFD Session Info] | |
124 | * | |
125 | * (Re)Association Response, P2P Invitation Req/Resp, | |
126 | * Provision Discovery Resp: | |
127 | * WFD Device Info | |
128 | * [Associated BSSID] | |
129 | * [Coupled Sink Info] | |
130 | * [WFD Session Info] | |
131 | */ | |
132 | len = 0; | |
133 | if (global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]) | |
134 | len += wpabuf_len(global->wfd_subelem[ | |
135 | WFD_SUBELEM_DEVICE_INFO]); | |
136 | if (global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID]) | |
137 | len += wpabuf_len(global->wfd_subelem[ | |
138 | WFD_SUBELEM_ASSOCIATED_BSSID]); | |
139 | if (global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]) | |
140 | len += wpabuf_len(global->wfd_subelem[ | |
141 | WFD_SUBELEM_COUPLED_SINK]); | |
142 | if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]) | |
143 | len += wpabuf_len(global->wfd_subelem[ | |
144 | WFD_SUBELEM_SESSION_INFO]); | |
145 | if (global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]) | |
146 | len += wpabuf_len(global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]); | |
147 | buf = wpabuf_alloc(len); | |
148 | if (buf == NULL) | |
149 | return -1; | |
150 | ||
151 | if (global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]) | |
152 | wpabuf_put_buf(buf, | |
153 | global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]); | |
154 | if (global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID]) | |
155 | wpabuf_put_buf(buf, global->wfd_subelem[ | |
156 | WFD_SUBELEM_ASSOCIATED_BSSID]); | |
157 | if (global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]) | |
158 | wpabuf_put_buf(buf, | |
159 | global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]); | |
160 | ||
161 | ie = wifi_display_encaps(buf); | |
162 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Beacon", ie); | |
163 | p2p_set_wfd_ie_beacon(global->p2p, ie); | |
164 | ||
165 | ie = wifi_display_encaps(buf); | |
166 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for (Re)Association Request", | |
167 | ie); | |
168 | p2p_set_wfd_ie_assoc_req(global->p2p, ie); | |
169 | ||
170 | ie = wifi_display_encaps(buf); | |
171 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for GO Negotiation", ie); | |
172 | p2p_set_wfd_ie_go_neg(global->p2p, ie); | |
173 | ||
174 | ie = wifi_display_encaps(buf); | |
175 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Provision Discovery " | |
176 | "Request", ie); | |
177 | p2p_set_wfd_ie_prov_disc_req(global->p2p, ie); | |
178 | ||
179 | plen = buf->used; | |
180 | if (global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]) | |
181 | wpabuf_put_buf(buf, | |
182 | global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]); | |
183 | ||
184 | ie = wifi_display_encaps(buf); | |
185 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Probe Request", ie); | |
186 | p2p_set_wfd_ie_probe_req(global->p2p, ie); | |
187 | ||
188 | if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]) | |
189 | wpabuf_put_buf(buf, | |
190 | global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]); | |
191 | ie = wifi_display_encaps(buf); | |
192 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Probe Response", ie); | |
193 | p2p_set_wfd_ie_probe_resp(global->p2p, ie); | |
194 | ||
195 | /* Remove WFD Extended Capability from buffer */ | |
196 | buf->used = plen; | |
197 | if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]) | |
198 | wpabuf_put_buf(buf, | |
199 | global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]); | |
200 | ||
201 | ie = wifi_display_encaps(buf); | |
202 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for P2P Invitation", ie); | |
203 | p2p_set_wfd_ie_invitation(global->p2p, ie); | |
204 | ||
205 | ie = wifi_display_encaps(buf); | |
206 | wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Provision Discovery " | |
207 | "Response", ie); | |
208 | p2p_set_wfd_ie_prov_disc_resp(global->p2p, ie); | |
209 | ||
210 | wpabuf_free(buf); | |
211 | ||
212 | return 0; | |
213 | } | |
214 | ||
215 | ||
216 | void wifi_display_enable(struct wpa_global *global, int enabled) | |
217 | { | |
218 | wpa_printf(MSG_DEBUG, "WFD: Wi-Fi Display %s", | |
219 | enabled ? "enabled" : "disabled"); | |
220 | global->wifi_display = enabled; | |
221 | wifi_display_update_wfd_ie(global); | |
222 | } | |
223 | ||
224 | ||
225 | int wifi_display_subelem_set(struct wpa_global *global, char *cmd) | |
226 | { | |
227 | char *pos; | |
228 | int subelem; | |
229 | size_t len; | |
230 | struct wpabuf *e; | |
231 | ||
232 | pos = os_strchr(cmd, ' '); | |
233 | if (pos == NULL) | |
234 | return -1; | |
235 | *pos++ = '\0'; | |
9675ce35 JM |
236 | |
237 | len = os_strlen(pos); | |
238 | if (len & 1) | |
239 | return -1; | |
240 | len /= 2; | |
241 | ||
bc0ba01d JM |
242 | if (os_strcmp(cmd, "all") == 0) { |
243 | int res; | |
244 | ||
245 | e = wpabuf_alloc(len); | |
246 | if (e == NULL) | |
247 | return -1; | |
248 | if (hexstr2bin(pos, wpabuf_put(e, len), len) < 0) { | |
249 | wpabuf_free(e); | |
250 | return -1; | |
251 | } | |
252 | res = wifi_display_subelem_set_from_ies(global, e); | |
253 | wpabuf_free(e); | |
254 | return res; | |
255 | } | |
256 | ||
257 | subelem = atoi(cmd); | |
258 | if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS) | |
259 | return -1; | |
260 | ||
9675ce35 JM |
261 | if (len == 0) { |
262 | /* Clear subelement */ | |
263 | e = NULL; | |
264 | wpa_printf(MSG_DEBUG, "WFD: Clear subelement %d", subelem); | |
265 | } else { | |
266 | e = wpabuf_alloc(1 + len); | |
267 | if (e == NULL) | |
268 | return -1; | |
269 | wpabuf_put_u8(e, subelem); | |
270 | if (hexstr2bin(pos, wpabuf_put(e, len), len) < 0) { | |
271 | wpabuf_free(e); | |
272 | return -1; | |
273 | } | |
274 | wpa_printf(MSG_DEBUG, "WFD: Set subelement %d", subelem); | |
275 | } | |
276 | ||
277 | wpabuf_free(global->wfd_subelem[subelem]); | |
278 | global->wfd_subelem[subelem] = e; | |
279 | wifi_display_update_wfd_ie(global); | |
280 | ||
281 | return 0; | |
282 | } | |
283 | ||
284 | ||
4bd7e161 TB |
285 | int wifi_display_subelem_set_from_ies(struct wpa_global *global, |
286 | struct wpabuf *ie) | |
287 | { | |
288 | int subelements[MAX_WFD_SUBELEMS] = {}; | |
289 | const u8 *pos, *end; | |
4debeb42 | 290 | unsigned int len, subelem; |
4bd7e161 TB |
291 | struct wpabuf *e; |
292 | ||
293 | wpa_printf(MSG_DEBUG, "WFD IEs set: %p - %lu", | |
294 | ie, ie ? (unsigned long) wpabuf_len(ie) : 0); | |
295 | ||
296 | if (ie == NULL || wpabuf_len(ie) < 6) | |
297 | return -1; | |
298 | ||
299 | pos = wpabuf_head(ie); | |
300 | end = pos + wpabuf_len(ie); | |
301 | ||
302 | while (end > pos) { | |
303 | if (pos + 3 > end) | |
304 | break; | |
305 | ||
306 | len = WPA_GET_BE16(pos + 1) + 3; | |
307 | ||
308 | wpa_printf(MSG_DEBUG, "WFD Sub-Element ID %d - len %d", | |
309 | *pos, len - 3); | |
310 | ||
bd43e67e | 311 | if (len > (unsigned int) (end - pos)) |
4bd7e161 TB |
312 | break; |
313 | ||
314 | subelem = *pos; | |
315 | if (subelem < MAX_WFD_SUBELEMS && subelements[subelem] == 0) { | |
316 | e = wpabuf_alloc_copy(pos, len); | |
317 | if (e == NULL) | |
318 | return -1; | |
319 | ||
320 | wpabuf_free(global->wfd_subelem[subelem]); | |
321 | global->wfd_subelem[subelem] = e; | |
322 | subelements[subelem] = 1; | |
323 | } | |
324 | ||
325 | pos += len; | |
326 | } | |
327 | ||
328 | for (subelem = 0; subelem < MAX_WFD_SUBELEMS; subelem++) { | |
329 | if (subelements[subelem] == 0) { | |
330 | wpabuf_free(global->wfd_subelem[subelem]); | |
331 | global->wfd_subelem[subelem] = NULL; | |
332 | } | |
333 | } | |
334 | ||
335 | return wifi_display_update_wfd_ie(global); | |
336 | } | |
337 | ||
338 | ||
9675ce35 JM |
339 | int wifi_display_subelem_get(struct wpa_global *global, char *cmd, |
340 | char *buf, size_t buflen) | |
341 | { | |
342 | int subelem; | |
343 | ||
bc0ba01d JM |
344 | if (os_strcmp(cmd, "all") == 0) { |
345 | struct wpabuf *ie; | |
346 | int res; | |
347 | ||
348 | ie = wifi_display_get_wfd_ie(global); | |
349 | if (ie == NULL) | |
350 | return 0; | |
351 | res = wpa_snprintf_hex(buf, buflen, wpabuf_head(ie), | |
352 | wpabuf_len(ie)); | |
353 | wpabuf_free(ie); | |
354 | return res; | |
355 | } | |
356 | ||
9675ce35 JM |
357 | subelem = atoi(cmd); |
358 | if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS) | |
359 | return -1; | |
360 | ||
361 | if (global->wfd_subelem[subelem] == NULL) | |
362 | return 0; | |
363 | ||
364 | return wpa_snprintf_hex(buf, buflen, | |
365 | wpabuf_head_u8(global->wfd_subelem[subelem]) + | |
366 | 1, | |
367 | wpabuf_len(global->wfd_subelem[subelem]) - 1); | |
368 | } | |
b125c48f DS |
369 | |
370 | ||
371 | char * wifi_display_subelem_hex(const struct wpabuf *wfd_subelems, u8 id) | |
372 | { | |
373 | char *subelem = NULL; | |
374 | const u8 *buf; | |
375 | size_t buflen; | |
376 | size_t i = 0; | |
377 | u16 elen; | |
378 | ||
379 | if (!wfd_subelems) | |
380 | return NULL; | |
381 | ||
382 | buf = wpabuf_head_u8(wfd_subelems); | |
383 | if (!buf) | |
384 | return NULL; | |
385 | ||
386 | buflen = wpabuf_len(wfd_subelems); | |
387 | ||
388 | while (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN < buflen) { | |
389 | elen = WPA_GET_BE16(buf + i + 1); | |
49d13df6 JM |
390 | if (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN + elen > buflen) |
391 | break; /* truncated subelement */ | |
b125c48f DS |
392 | |
393 | if (buf[i] == id) { | |
d3fa2bbb JM |
394 | /* |
395 | * Limit explicitly to an arbitrary length to avoid | |
396 | * unnecessarily large allocations. In practice, this | |
397 | * is limited to maximum frame length anyway, so the | |
398 | * maximum memory allocation here is not really that | |
399 | * large. Anyway, the Wi-Fi Display subelements that | |
400 | * are fetched with this function are even shorter. | |
401 | */ | |
402 | if (elen > 1000) | |
403 | break; | |
b125c48f DS |
404 | subelem = os_zalloc(2 * elen + 1); |
405 | if (!subelem) | |
406 | return NULL; | |
407 | wpa_snprintf_hex(subelem, 2 * elen + 1, | |
408 | buf + i + | |
409 | WIFI_DISPLAY_SUBELEM_HEADER_LEN, | |
410 | elen); | |
411 | break; | |
412 | } | |
413 | ||
414 | i += elen + WIFI_DISPLAY_SUBELEM_HEADER_LEN; | |
415 | } | |
416 | ||
417 | return subelem; | |
418 | } |