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 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 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; 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 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 } 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); 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
