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