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 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 
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 
67 static int wifi_display_update_wfd_ie(struct wpa_global *global)
68 {
69 	struct wpabuf *ie, *buf;
70 	size_t len, plen;
71 
72 	if (global->p2p == NULL)
73 		return 0;
74 
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_r2_dev_info(global->p2p, NULL);
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]);
97 	p2p_set_wfd_r2_dev_info(
98 		global->p2p, global->wfd_subelem[WFD_SUBELEM_R2_DEVICE_INFO]);
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]);
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 
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]);
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 
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';
249 
250 	len = os_strlen(pos);
251 	if (len & 1)
252 		return -1;
253 	len /= 2;
254 
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 
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 
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;
303 	unsigned int len, subelem;
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 
324 		if (len > (unsigned int) (end - pos))
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 
352 int wifi_display_subelem_get(struct wpa_global *global, char *cmd,
353 			     char *buf, size_t buflen)
354 {
355 	int subelem;
356 
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 
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 }
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);
403 		if (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN + elen > buflen)
404 			break; /* truncated subelement */
405 
406 		if (buf[i] == id) {
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;
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 }
432