]>
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'; | |
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 | ||
4bd7e161 TB |
269 | int 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; | |
4debeb42 | 274 | unsigned int len, subelem; |
4bd7e161 TB |
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 | ||
bd43e67e | 295 | if (len > (unsigned int) (end - pos)) |
4bd7e161 TB |
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 | ||
9675ce35 JM |
323 | int 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 | } | |
b125c48f DS |
340 | |
341 | ||
342 | char * 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); | |
49d13df6 JM |
361 | if (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN + elen > buflen) |
362 | break; /* truncated subelement */ | |
b125c48f DS |
363 | |
364 | if (buf[i] == id) { | |
d3fa2bbb JM |
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; | |
b125c48f DS |
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 | } |