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_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 }