wifi_display.c revision 346981 
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
22int wifi_display_init(struct wpa_global *global)
23{
24	global->wifi_display = 1;
25	return 0;
26}
27
28
29void 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
39struct 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
67static 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
229void 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
238int 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
298int 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
352int 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
384char * 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