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