contrib/wpa/wpa_supplicant/wifi_display.c

   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 }