--- /dev/null
+/*
+ * wpa_supplicant - Wi-Fi Display
+ * Copyright (c) 2011, Atheros Communications, Inc.
+ * Copyright (c) 2011-2012, Qualcomm Atheros, Inc.
+ *
+ * This software may be distributed under the terms of the BSD license.
+ * See README for more details.
+ */
+
+#include "includes.h"
+
+#include "common.h"
+#include "p2p/p2p.h"
+#include "common/ieee802_11_defs.h"
+#include "wpa_supplicant_i.h"
+#include "wifi_display.h"
+
+
+#define WIFI_DISPLAY_SUBELEM_HEADER_LEN 3
+
+
+int wifi_display_init(struct wpa_global *global)
+{
+ global->wifi_display = 1;
+ return 0;
+}
+
+
+void wifi_display_deinit(struct wpa_global *global)
+{
+ int i;
+ for (i = 0; i < MAX_WFD_SUBELEMS; i++) {
+ wpabuf_free(global->wfd_subelem[i]);
+ global->wfd_subelem[i] = NULL;
+ }
+}
+
+
+struct wpabuf * wifi_display_get_wfd_ie(struct wpa_global *global)
+{
+ struct wpabuf *ie;
+ size_t len;
+ int i;
+
+ if (global->p2p == NULL)
+ return NULL;
+
+ len = 0;
+ for (i = 0; i < MAX_WFD_SUBELEMS; i++) {
+ if (global->wfd_subelem[i])
+ len += wpabuf_len(global->wfd_subelem[i]);
+ }
+
+ ie = wpabuf_alloc(len);
+ if (ie == NULL)
+ return NULL;
+
+ for (i = 0; i < MAX_WFD_SUBELEMS; i++) {
+ if (global->wfd_subelem[i])
+ wpabuf_put_buf(ie, global->wfd_subelem[i]);
+ }
+
+ return ie;
+}
+
+
+static int wifi_display_update_wfd_ie(struct wpa_global *global)
+{
+ struct wpabuf *ie, *buf;
+ size_t len, plen;
+
+ if (global->p2p == NULL)
+ return 0;
+
+ wpa_printf(MSG_DEBUG, "WFD: Update WFD IE");
+
+ if (!global->wifi_display) {
+ wpa_printf(MSG_DEBUG, "WFD: Wi-Fi Display disabled - do not "
+ "include WFD IE");
+ p2p_set_wfd_ie_beacon(global->p2p, NULL);
+ p2p_set_wfd_ie_probe_req(global->p2p, NULL);
+ p2p_set_wfd_ie_probe_resp(global->p2p, NULL);
+ p2p_set_wfd_ie_assoc_req(global->p2p, NULL);
+ p2p_set_wfd_ie_invitation(global->p2p, NULL);
+ p2p_set_wfd_ie_prov_disc_req(global->p2p, NULL);
+ p2p_set_wfd_ie_prov_disc_resp(global->p2p, NULL);
+ p2p_set_wfd_ie_go_neg(global->p2p, NULL);
+ p2p_set_wfd_dev_info(global->p2p, NULL);
+ p2p_set_wfd_assoc_bssid(global->p2p, NULL);
+ p2p_set_wfd_coupled_sink_info(global->p2p, NULL);
+ return 0;
+ }
+
+ p2p_set_wfd_dev_info(global->p2p,
+ global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]);
+ p2p_set_wfd_assoc_bssid(
+ global->p2p,
+ global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID]);
+ p2p_set_wfd_coupled_sink_info(
+ global->p2p, global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]);
+
+ /*
+ * WFD IE is included in number of management frames. Two different
+ * sets of subelements are included depending on the frame:
+ *
+ * Beacon, (Re)Association Request, GO Negotiation Req/Resp/Conf,
+ * Provision Discovery Req:
+ * WFD Device Info
+ * [Associated BSSID]
+ * [Coupled Sink Info]
+ *
+ * Probe Request:
+ * WFD Device Info
+ * [Associated BSSID]
+ * [Coupled Sink Info]
+ * [WFD Extended Capability]
+ *
+ * Probe Response:
+ * WFD Device Info
+ * [Associated BSSID]
+ * [Coupled Sink Info]
+ * [WFD Extended Capability]
+ * [WFD Session Info]
+ *
+ * (Re)Association Response, P2P Invitation Req/Resp,
+ * Provision Discovery Resp:
+ * WFD Device Info
+ * [Associated BSSID]
+ * [Coupled Sink Info]
+ * [WFD Session Info]
+ */
+ len = 0;
+ if (global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO])
+ len += wpabuf_len(global->wfd_subelem[
+ WFD_SUBELEM_DEVICE_INFO]);
+ if (global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID])
+ len += wpabuf_len(global->wfd_subelem[
+ WFD_SUBELEM_ASSOCIATED_BSSID]);
+ if (global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK])
+ len += wpabuf_len(global->wfd_subelem[
+ WFD_SUBELEM_COUPLED_SINK]);
+ if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO])
+ len += wpabuf_len(global->wfd_subelem[
+ WFD_SUBELEM_SESSION_INFO]);
+ if (global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB])
+ len += wpabuf_len(global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]);
+ buf = wpabuf_alloc(len);
+ if (buf == NULL)
+ return -1;
+
+ if (global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO])
+ wpabuf_put_buf(buf,
+ global->wfd_subelem[WFD_SUBELEM_DEVICE_INFO]);
+ if (global->wfd_subelem[WFD_SUBELEM_ASSOCIATED_BSSID])
+ wpabuf_put_buf(buf, global->wfd_subelem[
+ WFD_SUBELEM_ASSOCIATED_BSSID]);
+ if (global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK])
+ wpabuf_put_buf(buf,
+ global->wfd_subelem[WFD_SUBELEM_COUPLED_SINK]);
+
+ ie = wifi_display_encaps(buf);
+ wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Beacon", ie);
+ p2p_set_wfd_ie_beacon(global->p2p, ie);
+
+ ie = wifi_display_encaps(buf);
+ wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for (Re)Association Request",
+ ie);
+ p2p_set_wfd_ie_assoc_req(global->p2p, ie);
+
+ ie = wifi_display_encaps(buf);
+ wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for GO Negotiation", ie);
+ p2p_set_wfd_ie_go_neg(global->p2p, ie);
+
+ ie = wifi_display_encaps(buf);
+ wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Provision Discovery "
+ "Request", ie);
+ p2p_set_wfd_ie_prov_disc_req(global->p2p, ie);
+
+ plen = buf->used;
+ if (global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB])
+ wpabuf_put_buf(buf,
+ global->wfd_subelem[WFD_SUBELEM_EXT_CAPAB]);
+
+ ie = wifi_display_encaps(buf);
+ wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Probe Request", ie);
+ p2p_set_wfd_ie_probe_req(global->p2p, ie);
+
+ if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO])
+ wpabuf_put_buf(buf,
+ global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]);
+ ie = wifi_display_encaps(buf);
+ wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Probe Response", ie);
+ p2p_set_wfd_ie_probe_resp(global->p2p, ie);
+
+ /* Remove WFD Extended Capability from buffer */
+ buf->used = plen;
+ if (global->wfd_subelem[WFD_SUBELEM_SESSION_INFO])
+ wpabuf_put_buf(buf,
+ global->wfd_subelem[WFD_SUBELEM_SESSION_INFO]);
+
+ ie = wifi_display_encaps(buf);
+ wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for P2P Invitation", ie);
+ p2p_set_wfd_ie_invitation(global->p2p, ie);
+
+ ie = wifi_display_encaps(buf);
+ wpa_hexdump_buf(MSG_DEBUG, "WFD: WFD IE for Provision Discovery "
+ "Response", ie);
+ p2p_set_wfd_ie_prov_disc_resp(global->p2p, ie);
+
+ wpabuf_free(buf);
+
+ return 0;
+}
+
+
+void wifi_display_enable(struct wpa_global *global, int enabled)
+{
+ wpa_printf(MSG_DEBUG, "WFD: Wi-Fi Display %s",
+ enabled ? "enabled" : "disabled");
+ global->wifi_display = enabled;
+ wifi_display_update_wfd_ie(global);
+}
+
+
+int wifi_display_subelem_set(struct wpa_global *global, char *cmd)
+{
+ char *pos;
+ int subelem;
+ size_t len;
+ struct wpabuf *e;
+
+ pos = os_strchr(cmd, ' ');
+ if (pos == NULL)
+ return -1;
+ *pos++ = '\0';
+
+ len = os_strlen(pos);
+ if (len & 1)
+ return -1;
+ len /= 2;
+
+ if (os_strcmp(cmd, "all") == 0) {
+ int res;
+
+ e = wpabuf_alloc(len);
+ if (e == NULL)
+ return -1;
+ if (hexstr2bin(pos, wpabuf_put(e, len), len) < 0) {
+ wpabuf_free(e);
+ return -1;
+ }
+ res = wifi_display_subelem_set_from_ies(global, e);
+ wpabuf_free(e);
+ return res;
+ }
+
+ subelem = atoi(cmd);
+ if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS)
+ return -1;
+
+ if (len == 0) {
+ /* Clear subelement */
+ e = NULL;
+ wpa_printf(MSG_DEBUG, "WFD: Clear subelement %d", subelem);
+ } else {
+ e = wpabuf_alloc(1 + len);
+ if (e == NULL)
+ return -1;
+ wpabuf_put_u8(e, subelem);
+ if (hexstr2bin(pos, wpabuf_put(e, len), len) < 0) {
+ wpabuf_free(e);
+ return -1;
+ }
+ wpa_printf(MSG_DEBUG, "WFD: Set subelement %d", subelem);
+ }
+
+ wpabuf_free(global->wfd_subelem[subelem]);
+ global->wfd_subelem[subelem] = e;
+ wifi_display_update_wfd_ie(global);
+
+ return 0;
+}
+
+
+int wifi_display_subelem_set_from_ies(struct wpa_global *global,
+ struct wpabuf *ie)
+{
+ int subelements[MAX_WFD_SUBELEMS] = {};
+ const u8 *pos, *end;
+ unsigned int len, subelem;
+ struct wpabuf *e;
+
+ wpa_printf(MSG_DEBUG, "WFD IEs set: %p - %lu",
+ ie, ie ? (unsigned long) wpabuf_len(ie) : 0);
+
+ if (ie == NULL || wpabuf_len(ie) < 6)
+ return -1;
+
+ pos = wpabuf_head(ie);
+ end = pos + wpabuf_len(ie);
+
+ while (end > pos) {
+ if (pos + 3 > end)
+ break;
+
+ len = WPA_GET_BE16(pos + 1) + 3;
+
+ wpa_printf(MSG_DEBUG, "WFD Sub-Element ID %d - len %d",
+ *pos, len - 3);
+
+ if (len > (unsigned int) (end - pos))
+ break;
+
+ subelem = *pos;
+ if (subelem < MAX_WFD_SUBELEMS && subelements[subelem] == 0) {
+ e = wpabuf_alloc_copy(pos, len);
+ if (e == NULL)
+ return -1;
+
+ wpabuf_free(global->wfd_subelem[subelem]);
+ global->wfd_subelem[subelem] = e;
+ subelements[subelem] = 1;
+ }
+
+ pos += len;
+ }
+
+ for (subelem = 0; subelem < MAX_WFD_SUBELEMS; subelem++) {
+ if (subelements[subelem] == 0) {
+ wpabuf_free(global->wfd_subelem[subelem]);
+ global->wfd_subelem[subelem] = NULL;
+ }
+ }
+
+ return wifi_display_update_wfd_ie(global);
+}
+
+
+int wifi_display_subelem_get(struct wpa_global *global, char *cmd,
+ char *buf, size_t buflen)
+{
+ int subelem;
+
+ if (os_strcmp(cmd, "all") == 0) {
+ struct wpabuf *ie;
+ int res;
+
+ ie = wifi_display_get_wfd_ie(global);
+ if (ie == NULL)
+ return 0;
+ res = wpa_snprintf_hex(buf, buflen, wpabuf_head(ie),
+ wpabuf_len(ie));
+ wpabuf_free(ie);
+ return res;
+ }
+
+ subelem = atoi(cmd);
+ if (subelem < 0 || subelem >= MAX_WFD_SUBELEMS)
+ return -1;
+
+ if (global->wfd_subelem[subelem] == NULL)
+ return 0;
+
+ return wpa_snprintf_hex(buf, buflen,
+ wpabuf_head_u8(global->wfd_subelem[subelem]) +
+ 1,
+ wpabuf_len(global->wfd_subelem[subelem]) - 1);
+}
+
+
+char * wifi_display_subelem_hex(const struct wpabuf *wfd_subelems, u8 id)
+{
+ char *subelem = NULL;
+ const u8 *buf;
+ size_t buflen;
+ size_t i = 0;
+ u16 elen;
+
+ if (!wfd_subelems)
+ return NULL;
+
+ buf = wpabuf_head_u8(wfd_subelems);
+ if (!buf)
+ return NULL;
+
+ buflen = wpabuf_len(wfd_subelems);
+
+ while (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN < buflen) {
+ elen = WPA_GET_BE16(buf + i + 1);
+ if (i + WIFI_DISPLAY_SUBELEM_HEADER_LEN + elen > buflen)
+ break; /* truncated subelement */
+
+ if (buf[i] == id) {
+ /*
+ * Limit explicitly to an arbitrary length to avoid
+ * unnecessarily large allocations. In practice, this
+ * is limited to maximum frame length anyway, so the
+ * maximum memory allocation here is not really that
+ * large. Anyway, the Wi-Fi Display subelements that
+ * are fetched with this function are even shorter.
+ */
+ if (elen > 1000)
+ break;
+ subelem = os_zalloc(2 * elen + 1);
+ if (!subelem)
+ return NULL;
+ wpa_snprintf_hex(subelem, 2 * elen + 1,
+ buf + i +
+ WIFI_DISPLAY_SUBELEM_HEADER_LEN,
+ elen);
+ break;
+ }
+
+ i += elen + WIFI_DISPLAY_SUBELEM_HEADER_LEN;
+ }
+
+ return subelem;
+}
projects / mech_eap.git / blobdiff