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