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_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
216void 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
225int 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';
236	subelem = atoi(cmd);
237	if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS)
238		return -1;
239
240	len = os_strlen(pos);
241	if (len & 1)
242		return -1;
243	len /= 2;
244
245	if (len == 0) {
246		/* Clear subelement */
247		e = NULL;
248		wpa_printf(MSG_DEBUG, "WFD: Clear subelement %d", subelem);
249	} else {
250		e = wpabuf_alloc(1 + len);
251		if (e == NULL)
252			return -1;
253		wpabuf_put_u8(e, subelem);
254		if (hexstr2bin(pos, wpabuf_put(e, len), len) < 0) {
255			wpabuf_free(e);
256			return -1;
257		}
258		wpa_printf(MSG_DEBUG, "WFD: Set subelement %d", subelem);
259	}
260
261	wpabuf_free(global->wfd_subelem[subelem]);
262	global->wfd_subelem[subelem] = e;
263	wifi_display_update_wfd_ie(global);
264
265	return 0;
266}
267
268
269int wifi_display_subelem_set_from_ies(struct wpa_global *global,
270				      struct wpabuf *ie)
271{
272	int subelements[MAX_WFD_SUBELEMS] = {};
273	const u8 *pos, *end;
274	int len, subelem;
275	struct wpabuf *e;
276
277	wpa_printf(MSG_DEBUG, "WFD IEs set: %p - %lu",
278		   ie, ie ? (unsigned long) wpabuf_len(ie) : 0);
279
280	if (ie == NULL || wpabuf_len(ie) < 6)
281		return -1;
282
283	pos = wpabuf_head(ie);
284	end = pos + wpabuf_len(ie);
285
286	while (end > pos) {
287		if (pos + 3 > end)
288			break;
289
290		len = WPA_GET_BE16(pos + 1) + 3;
291
292		wpa_printf(MSG_DEBUG, "WFD Sub-Element ID %d - len %d",
293			   *pos, len - 3);
294
295		if (pos + len > end)
296			break;
297
298		subelem = *pos;
299		if (subelem < MAX_WFD_SUBELEMS && subelements[subelem] == 0) {
300			e = wpabuf_alloc_copy(pos, len);
301			if (e == NULL)
302				return -1;
303
304			wpabuf_free(global->wfd_subelem[subelem]);
305			global->wfd_subelem[subelem] = e;
306			subelements[subelem] = 1;
307		}
308
309		pos += len;
310	}
311
312	for (subelem = 0; subelem < MAX_WFD_SUBELEMS; subelem++) {
313		if (subelements[subelem] == 0) {
314			wpabuf_free(global->wfd_subelem[subelem]);
315			global->wfd_subelem[subelem] = NULL;
316		}
317	}
318
319	return wifi_display_update_wfd_ie(global);
320}
321
322
323int wifi_display_subelem_get(struct wpa_global *global, char *cmd,
324			     char *buf, size_t buflen)
325{
326	int subelem;
327
328	subelem = atoi(cmd);
329	if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS)
330		return -1;
331
332	if (global->wfd_subelem[subelem] == NULL)
333		return 0;
334
335	return wpa_snprintf_hex(buf, buflen,
336				wpabuf_head_u8(global->wfd_subelem[subelem]) +
337				1,
338				wpabuf_len(global->wfd_subelem[subelem]) - 1);
339}
340
341
342char * wifi_display_subelem_hex(const struct wpabuf *wfd_subelems, u8 id)
343{
344	char *subelem = NULL;
345	const u8 *buf;
346	size_t buflen;
347	size_t i = 0;
348	u16 elen;
349
350	if (!wfd_subelems)
351		return NULL;
352
353	buf = wpabuf_head_u8(wfd_subelems);
354	if (!buf)
355		return NULL;
356
357	buflen = wpabuf_len(wfd_subelems);
358
359	while (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN < buflen) {
360		elen = WPA_GET_BE16(buf + i + 1);
361		if (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN + elen > buflen)
362			break; /* truncated subelement */
363
364		if (buf[i] == id) {
365			/*
366			 * Limit explicitly to an arbitrary length to avoid
367			 * unnecessarily large allocations. In practice, this
368			 * is limited to maximum frame length anyway, so the
369			 * maximum memory allocation here is not really that
370			 * large. Anyway, the Wi-Fi Display subelements that
371			 * are fetched with this function are even shorter.
372			 */
373			if (elen > 1000)
374				break;
375			subelem = os_zalloc(2 * elen + 1);
376			if (!subelem)
377				return NULL;
378			wpa_snprintf_hex(subelem, 2 * elen + 1,
379					 buf + i +
380					 WIFI_DISPLAY_SUBELEM_HEADER_LEN,
381					 elen);
382			break;
383		}
384
385		i += elen + WIFI_DISPLAY_SUBELEM_HEADER_LEN;
386	}
387
388	return subelem;
389}
390