Line data Source code
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 :
19 : #define WIFI_DISPLAY_SUBELEM_HEADER_LEN 3
20 :
21 :
22 49 : int wifi_display_init(struct wpa_global *global)
23 : {
24 49 : global->wifi_display = 1;
25 49 : return 0;
26 : }
27 :
28 :
29 51 : void wifi_display_deinit(struct wpa_global *global)
30 : {
31 : int i;
32 561 : for (i = 0; i < MAX_WFD_SUBELEMS; i++) {
33 510 : wpabuf_free(global->wfd_subelem[i]);
34 510 : global->wfd_subelem[i] = NULL;
35 : }
36 51 : }
37 :
38 :
39 8 : 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 8 : if (global->p2p == NULL)
46 0 : return NULL;
47 :
48 8 : len = 0;
49 88 : for (i = 0; i < MAX_WFD_SUBELEMS; i++) {
50 80 : if (global->wfd_subelem[i])
51 28 : len += wpabuf_len(global->wfd_subelem[i]);
52 : }
53 :
54 8 : ie = wpabuf_alloc(len);
55 8 : if (ie == NULL)
56 0 : return NULL;
57 :
58 88 : for (i = 0; i < MAX_WFD_SUBELEMS; i++) {
59 80 : if (global->wfd_subelem[i])
60 28 : wpabuf_put_buf(ie, global->wfd_subelem[i]);
61 : }
62 :
63 8 : return ie;
64 : }
65 :
66 :
67 62 : static int wifi_display_update_wfd_ie(struct wpa_global *global)
68 : {
69 : struct wpabuf *ie, *buf;
70 : size_t len, plen;
71 :
72 62 : if (global->p2p == NULL)
73 0 : return 0;
74 :
75 62 : wpa_printf(MSG_DEBUG, "WFD: Update WFD IE");
76 :
77 62 : if (!global->wifi_display) {
78 17 : wpa_printf(MSG_DEBUG, "WFD: Wi-Fi Display disabled - do not "
79 : "include WFD IE");
80 17 : p2p_set_wfd_ie_beacon(global->p2p, NULL);
81 17 : p2p_set_wfd_ie_probe_req(global->p2p, NULL);
82 17 : p2p_set_wfd_ie_probe_resp(global->p2p, NULL);
83 17 : p2p_set_wfd_ie_assoc_req(global->p2p, NULL);
84 17 : p2p_set_wfd_ie_invitation(global->p2p, NULL);
85 17 : p2p_set_wfd_ie_prov_disc_req(global->p2p, NULL);
86 17 : p2p_set_wfd_ie_prov_disc_resp(global->p2p, NULL);
87 17 : p2p_set_wfd_ie_go_neg(global->p2p, NULL);
88 17 : p2p_set_wfd_dev_info(global->p2p, NULL);
89 17 : p2p_set_wfd_assoc_bssid(global->p2p, NULL);
90 17 : p2p_set_wfd_coupled_sink_info(global->p2p, NULL);
91 17 : return 0;
92 : }
93 :
94 45 : p2p_set_wfd_dev_info(global->p2p,
95 45 : global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]);
96 45 : p2p_set_wfd_assoc_bssid(
97 : global->p2p,
98 45 : global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID]);
99 45 : p2p_set_wfd_coupled_sink_info(
100 45 : 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 45 : len = 0;
133 45 : if (global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO])
134 29 : len += wpabuf_len(global->wfd_subelem[
135 : WFD_SUBELEM_DEVICE_INFO]);
136 45 : if (global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID])
137 15 : len += wpabuf_len(global->wfd_subelem[
138 : WFD_SUBELEM_ASSOCIATED_BSSID]);
139 45 : if (global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK])
140 9 : len += wpabuf_len(global->wfd_subelem[
141 : WFD_SUBELEM_COUPLED_SINK]);
142 45 : if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO])
143 11 : len += wpabuf_len(global->wfd_subelem[
144 : WFD_SUBELEM_SESSION_INFO]);
145 45 : if (global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB])
146 10 : len += wpabuf_len(global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]);
147 45 : buf = wpabuf_alloc(len);
148 45 : if (buf == NULL)
149 0 : return -1;
150 :
151 45 : if (global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO])
152 29 : wpabuf_put_buf(buf,
153 29 : global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]);
154 45 : if (global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID])
155 15 : wpabuf_put_buf(buf, global->wfd_subelem[
156 : WFD_SUBELEM_ASSOCIATED_BSSID]);
157 45 : if (global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK])
158 9 : wpabuf_put_buf(buf,
159 9 : global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]);
160 :
161 45 : ie = wifi_display_encaps(buf);
162 45 : wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Beacon", ie);
163 45 : p2p_set_wfd_ie_beacon(global->p2p, ie);
164 :
165 45 : ie = wifi_display_encaps(buf);
166 45 : wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for (Re)Association Request",
167 : ie);
168 45 : p2p_set_wfd_ie_assoc_req(global->p2p, ie);
169 :
170 45 : ie = wifi_display_encaps(buf);
171 45 : wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for GO Negotiation", ie);
172 45 : p2p_set_wfd_ie_go_neg(global->p2p, ie);
173 :
174 45 : ie = wifi_display_encaps(buf);
175 45 : wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Provision Discovery "
176 : "Request", ie);
177 45 : p2p_set_wfd_ie_prov_disc_req(global->p2p, ie);
178 :
179 45 : plen = buf->used;
180 45 : if (global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB])
181 10 : wpabuf_put_buf(buf,
182 10 : global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]);
183 :
184 45 : ie = wifi_display_encaps(buf);
185 45 : wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Probe Request", ie);
186 45 : p2p_set_wfd_ie_probe_req(global->p2p, ie);
187 :
188 45 : if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO])
189 11 : wpabuf_put_buf(buf,
190 11 : global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]);
191 45 : ie = wifi_display_encaps(buf);
192 45 : wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Probe Response", ie);
193 45 : p2p_set_wfd_ie_probe_resp(global->p2p, ie);
194 :
195 : /* Remove WFD Extended Capability from buffer */
196 45 : buf->used = plen;
197 45 : if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO])
198 11 : wpabuf_put_buf(buf,
199 11 : global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]);
200 :
201 45 : ie = wifi_display_encaps(buf);
202 45 : wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for P2P Invitation", ie);
203 45 : p2p_set_wfd_ie_invitation(global->p2p, ie);
204 :
205 45 : ie = wifi_display_encaps(buf);
206 45 : wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Provision Discovery "
207 : "Response", ie);
208 45 : p2p_set_wfd_ie_prov_disc_resp(global->p2p, ie);
209 :
210 45 : wpabuf_free(buf);
211 :
212 45 : return 0;
213 : }
214 :
215 :
216 31 : void wifi_display_enable(struct wpa_global *global, int enabled)
217 : {
218 31 : wpa_printf(MSG_DEBUG, "WFD: Wi-Fi Display %s",
219 : enabled ? "enabled" : "disabled");
220 31 : global->wifi_display = enabled;
221 31 : wifi_display_update_wfd_ie(global);
222 31 : }
223 :
224 :
225 37 : 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 37 : pos = os_strchr(cmd, ' ');
233 37 : if (pos == NULL)
234 2 : return -1;
235 35 : *pos++ = '\0';
236 :
237 35 : len = os_strlen(pos);
238 35 : if (len & 1)
239 1 : return -1;
240 34 : len /= 2;
241 :
242 34 : if (os_strcmp(cmd, "all") == 0) {
243 : int res;
244 :
245 6 : e = wpabuf_alloc(len);
246 6 : if (e == NULL)
247 0 : return -1;
248 6 : if (hexstr2bin(pos, wpabuf_put(e, len), len) < 0) {
249 1 : wpabuf_free(e);
250 1 : return -1;
251 : }
252 5 : res = wifi_display_subelem_set_from_ies(global, e);
253 5 : wpabuf_free(e);
254 5 : return res;
255 : }
256 :
257 28 : subelem = atoi(cmd);
258 28 : if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS)
259 2 : return -1;
260 :
261 26 : if (len == 0) {
262 : /* Clear subelement */
263 1 : e = NULL;
264 1 : wpa_printf(MSG_DEBUG, "WFD: Clear subelement %d", subelem);
265 : } else {
266 25 : e = wpabuf_alloc(1 + len);
267 25 : if (e == NULL)
268 0 : return -1;
269 25 : wpabuf_put_u8(e, subelem);
270 25 : if (hexstr2bin(pos, wpabuf_put(e, len), len) < 0) {
271 1 : wpabuf_free(e);
272 1 : return -1;
273 : }
274 24 : wpa_printf(MSG_DEBUG, "WFD: Set subelement %d", subelem);
275 : }
276 :
277 25 : wpabuf_free(global->wfd_subelem[subelem]);
278 25 : global->wfd_subelem[subelem] = e;
279 25 : wifi_display_update_wfd_ie(global);
280 :
281 25 : return 0;
282 : }
283 :
284 :
285 8 : int wifi_display_subelem_set_from_ies(struct wpa_global *global,
286 : struct wpabuf *ie)
287 : {
288 8 : int subelements[MAX_WFD_SUBELEMS] = {};
289 : const u8 *pos, *end;
290 : unsigned int len, subelem;
291 : struct wpabuf *e;
292 :
293 8 : wpa_printf(MSG_DEBUG, "WFD IEs set: %p - %lu",
294 : ie, ie ? (unsigned long) wpabuf_len(ie) : 0);
295 :
296 8 : if (ie == NULL || wpabuf_len(ie) < 6)
297 2 : return -1;
298 :
299 6 : pos = wpabuf_head(ie);
300 6 : end = pos + wpabuf_len(ie);
301 :
302 25 : while (end > pos) {
303 15 : if (pos + 3 > end)
304 1 : break;
305 :
306 14 : len = WPA_GET_BE16(pos + 1) + 3;
307 :
308 28 : wpa_printf(MSG_DEBUG, "WFD Sub-Element ID %d - len %d",
309 14 : *pos, len - 3);
310 :
311 14 : if (len > (unsigned int) (end - pos))
312 1 : break;
313 :
314 13 : subelem = *pos;
315 13 : if (subelem < MAX_WFD_SUBELEMS && subelements[subelem] == 0) {
316 11 : e = wpabuf_alloc_copy(pos, len);
317 11 : if (e == NULL)
318 0 : return -1;
319 :
320 11 : wpabuf_free(global->wfd_subelem[subelem]);
321 11 : global->wfd_subelem[subelem] = e;
322 11 : subelements[subelem] = 1;
323 : }
324 :
325 13 : pos += len;
326 : }
327 :
328 66 : for (subelem = 0; subelem < MAX_WFD_SUBELEMS; subelem++) {
329 60 : if (subelements[subelem] == 0) {
330 49 : wpabuf_free(global->wfd_subelem[subelem]);
331 49 : global->wfd_subelem[subelem] = NULL;
332 : }
333 : }
334 :
335 6 : return wifi_display_update_wfd_ie(global);
336 : }
337 :
338 :
339 16 : int wifi_display_subelem_get(struct wpa_global *global, char *cmd,
340 : char *buf, size_t buflen)
341 : {
342 : int subelem;
343 :
344 16 : if (os_strcmp(cmd, "all") == 0) {
345 : struct wpabuf *ie;
346 : int res;
347 :
348 4 : ie = wifi_display_get_wfd_ie(global);
349 4 : if (ie == NULL)
350 0 : return 0;
351 4 : res = wpa_snprintf_hex(buf, buflen, wpabuf_head(ie),
352 : wpabuf_len(ie));
353 4 : wpabuf_free(ie);
354 4 : return res;
355 : }
356 :
357 12 : subelem = atoi(cmd);
358 12 : if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS)
359 2 : return -1;
360 :
361 10 : if (global->wfd_subelem[subelem] == NULL)
362 2 : return 0;
363 :
364 16 : return wpa_snprintf_hex(buf, buflen,
365 8 : wpabuf_head_u8(global->wfd_subelem[subelem]) +
366 : 1,
367 8 : wpabuf_len(global->wfd_subelem[subelem]) - 1);
368 : }
369 :
370 :
371 592 : char * wifi_display_subelem_hex(const struct wpabuf *wfd_subelems, u8 id)
372 : {
373 592 : char *subelem = NULL;
374 : const u8 *buf;
375 : size_t buflen;
376 592 : size_t i = 0;
377 : u16 elen;
378 :
379 592 : if (!wfd_subelems)
380 570 : return NULL;
381 :
382 22 : buf = wpabuf_head_u8(wfd_subelems);
383 22 : if (!buf)
384 0 : return NULL;
385 :
386 22 : buflen = wpabuf_len(wfd_subelems);
387 :
388 44 : while (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN < buflen) {
389 22 : elen = WPA_GET_BE16(buf + i + 1);
390 22 : if (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN + elen > buflen)
391 1 : break; /* truncated subelement */
392 :
393 21 : if (buf[i] == id) {
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 21 : if (elen > 1000)
403 0 : break;
404 21 : subelem = os_zalloc(2 * elen + 1);
405 21 : if (!subelem)
406 0 : return NULL;
407 42 : wpa_snprintf_hex(subelem, 2 * elen + 1,
408 21 : buf + i +
409 : WIFI_DISPLAY_SUBELEM_HEADER_LEN,
410 : elen);
411 21 : break;
412 : }
413 :
414 0 : i += elen + WIFI_DISPLAY_SUBELEM_HEADER_LEN;
415 : }
416 :
417 22 : return subelem;
418 : }
|
