From a1fde16cf8e0a4f476e75a0b186f5f3662e5b080 Mon Sep 17 00:00:00 2001 From: Jouni Malinen Date: Fri, 2 Mar 2012 17:26:01 +0200 Subject: WFD: Add Wi-Fi Display support This commit adds control interface commands and internal storage of Wi-Fi Display related configuration. In addition, WFD IE is now added to various P2P frames, Probe Request/Response, and (Re)Association Request/Response frames. WFD subelements from peers are stored in the P2P peer table. Following control interface commands are now available: SET wifi_display <0/1> GET wifi_display WFD_SUBELEM_SET [hexdump of length+body] WFD_SUBELEM_GET Signed-hostap: Jouni Malinen [vishalm@ti.com: Backported it to curent tree] Signed-off-by: Vishal Mahaveer Change-Id: Ief0cd61903facd9209927d9dc84c8f093f3b6bac --- src/ap/ap_drv_ops.c | 11 ++ src/common/ieee802_11_common.c | 5 + src/common/ieee802_11_common.h | 2 + src/common/ieee802_11_defs.h | 16 +++ src/p2p/p2p.c | 218 ++++++++++++++++++++++++++++++++++++++- src/p2p/p2p.h | 19 ++++ src/p2p/p2p_go_neg.c | 42 +++++++- src/p2p/p2p_group.c | 227 +++++++++++++++++++++++++++++++++++++++-- src/p2p/p2p_i.h | 17 +++ src/p2p/p2p_invitation.c | 55 +++++++++- src/p2p/p2p_parse.c | 11 ++ src/p2p/p2p_pd.c | 52 +++++++++- 12 files changed, 653 insertions(+), 22 deletions(-) (limited to 'src') diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c index fbeaf9c..8488f11 100644 --- a/src/ap/ap_drv_ops.c +++ b/src/ap/ap_drv_ops.c @@ -12,6 +12,7 @@ #include "drivers/driver.h" #include "common/ieee802_11_defs.h" #include "wps/wps.h" +#include "p2p/p2p.h" #include "hostapd.h" #include "ieee802_11.h" #include "sta_info.h" @@ -147,6 +148,16 @@ int hostapd_build_ap_extra_ies(struct hostapd_data *hapd, } #endif /* CONFIG_P2P_MANAGER */ +#ifdef CONFIG_WIFI_DISPLAY + if (hapd->p2p_group) { + struct wpabuf *a; + a = p2p_group_assoc_resp_ie(hapd->p2p_group, P2P_SC_SUCCESS); + if (a && wpabuf_resize(&assocresp, wpabuf_len(a)) == 0) + wpabuf_put_buf(assocresp, a); + wpabuf_free(a); + } +#endif /* CONFIG_WIFI_DISPLAY */ + *beacon_ret = beacon; *proberesp_ret = proberesp; *assocresp_ret = assocresp; diff --git a/src/common/ieee802_11_common.c b/src/common/ieee802_11_common.c index d65675c..f90870a 100644 --- a/src/common/ieee802_11_common.c +++ b/src/common/ieee802_11_common.c @@ -97,6 +97,11 @@ static int ieee802_11_parse_vendor_specific(const u8 *pos, size_t elen, elems->p2p = pos; elems->p2p_len = elen; break; + case WFD_OUI_TYPE: + /* Wi-Fi Alliance - WFD IE */ + elems->wfd = pos; + elems->wfd_len = elen; + break; case HS20_INDICATION_OUI_TYPE: /* Hotspot 2.0 */ elems->hs20 = pos; diff --git a/src/common/ieee802_11_common.h b/src/common/ieee802_11_common.h index d9b2b6c..66c7935 100644 --- a/src/common/ieee802_11_common.h +++ b/src/common/ieee802_11_common.h @@ -35,6 +35,7 @@ struct ieee802_11_elems { const u8 *ht_operation; const u8 *vendor_ht_cap; const u8 *p2p; + const u8 *wfd; const u8 *link_id; const u8 *interworking; const u8 *hs20; @@ -65,6 +66,7 @@ struct ieee802_11_elems { u8 ht_operation_len; u8 vendor_ht_cap_len; u8 p2p_len; + u8 wfd_len; u8 interworking_len; u8 hs20_len; u8 ext_capab_len; diff --git a/src/common/ieee802_11_defs.h b/src/common/ieee802_11_defs.h index b4f9275..8e68c2a 100644 --- a/src/common/ieee802_11_defs.h +++ b/src/common/ieee802_11_defs.h @@ -701,6 +701,8 @@ struct ieee80211_vht_operation { #define WPS_IE_VENDOR_TYPE 0x0050f204 #define OUI_WFA 0x506f9a #define P2P_IE_VENDOR_TYPE 0x506f9a09 +#define WFD_IE_VENDOR_TYPE 0x506f9a0a +#define WFD_OUI_TYPE 10 #define HS20_IE_VENDOR_TYPE 0x506f9a10 #define WMM_OUI_TYPE 2 @@ -934,6 +936,20 @@ enum p2p_sd_status { }; +enum wifi_display_subelem { + WFD_SUBELEM_DEVICE_INFO = 0, + WFD_SUBELEM_ASSOCIATED_BSSID = 1, + WFD_SUBELEM_AUDIO_FORMATS = 2, + WFD_SUBELEM_VIDEO_FORMATS = 3, + WFD_SUBELEM_3D_VIDEO_FORMATS = 4, + WFD_SUBELEM_CONTENT_PROTECTION = 5, + WFD_SUBELEM_COUPLED_SINK = 6, + WFD_SUBELEM_EXT_CAPAB = 7, + WFD_SUBELEM_LOCAL_IP_ADDRESS = 8, + WFD_SUBELEM_SESSION_INFO = 9 +}; + + #define OUI_BROADCOM 0x00904c /* Broadcom (Epigram) */ #define VENDOR_HT_CAPAB_OUI_TYPE 0x33 /* 00-90-4c:0x33 */ diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c index 824a59e..df6697a 100644 --- a/src/p2p/p2p.c +++ b/src/p2p/p2p.c @@ -753,6 +753,11 @@ int p2p_add_device(struct p2p_data *p2p, const u8 *addr, int freq, break; } + if (msg.wfd_subelems) { + wpabuf_free(dev->info.wfd_subelems); + dev->info.wfd_subelems = wpabuf_dup(msg.wfd_subelems); + } + if (scan_res) { p2p_add_group_clients(p2p, p2p_dev_addr, addr, freq, msg.group_info, msg.group_info_len); @@ -810,6 +815,8 @@ static void p2p_device_free(struct p2p_data *p2p, struct p2p_device *dev) dev->info.wps_vendor_ext[i] = NULL; } + wpabuf_free(dev->info.wfd_subelems); + os_free(dev); } @@ -1454,6 +1461,11 @@ void p2p_add_dev_info(struct p2p_data *p2p, const u8 *addr, } } + if (msg->wfd_subelems) { + wpabuf_free(dev->info.wfd_subelems); + dev->info.wfd_subelems = wpabuf_dup(msg->wfd_subelems); + } + if (dev->flags & P2P_DEV_PROBE_REQ_ONLY) { dev->flags &= ~P2P_DEV_PROBE_REQ_ONLY; wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, @@ -1791,6 +1803,11 @@ static void p2p_add_dev_from_probe_req(struct p2p_data *p2p, const u8 *addr, p2p_copy_wps_info(dev, 1, &msg); + if (msg.wfd_subelems) { + wpabuf_free(dev->info.wfd_subelems); + dev->info.wfd_subelems = wpabuf_dup(msg.wfd_subelems); + } + p2p_parse_free(&msg); wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, @@ -1889,8 +1906,14 @@ struct wpabuf * p2p_build_probe_resp_ies(struct p2p_data *p2p) struct wpabuf *buf; u8 *len; int pw_id = -1; + size_t extra = 0; - buf = wpabuf_alloc(1000); +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_probe_resp) + extra = wpabuf_len(p2p->wfd_ie_probe_resp); +#endif /* CONFIG_WIFI_DISPLAY */ + + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -1901,6 +1924,11 @@ struct wpabuf * p2p_build_probe_resp_ies(struct p2p_data *p2p) p2p_build_wps_ie(p2p, buf, pw_id, 1); +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_probe_resp) + wpabuf_put_buf(buf, p2p->wfd_ie_probe_resp); +#endif /* CONFIG_WIFI_DISPLAY */ + /* P2P IE */ len = p2p_buf_add_ie_hdr(buf); p2p_buf_add_capability(buf, p2p->dev_capab & @@ -2185,20 +2213,31 @@ int p2p_assoc_req_ie(struct p2p_data *p2p, const u8 *bssid, u8 *buf, struct p2p_device *peer; size_t tmplen; int res; + size_t extra = 0; if (!p2p_group) return p2p_assoc_req_ie_wlan_ap(p2p, bssid, buf, len, p2p_ie); +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_assoc_req) + extra = wpabuf_len(p2p->wfd_ie_assoc_req); +#endif /* CONFIG_WIFI_DISPLAY */ + /* * (Re)Association Request - P2P IE * P2P Capability attribute (shall be present) * Extended Listen Timing (may be present) * P2P Device Info attribute (shall be present) */ - tmp = wpabuf_alloc(200); + tmp = wpabuf_alloc(200 + extra); if (tmp == NULL) return -1; +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_assoc_req) + wpabuf_put_buf(tmp, p2p->wfd_ie_assoc_req); +#endif /* CONFIG_WIFI_DISPLAY */ + peer = bssid ? p2p_get_device(p2p, bssid) : NULL; lpos = p2p_buf_add_ie_hdr(tmp); @@ -2391,6 +2430,20 @@ struct p2p_data * p2p_init(const struct p2p_config *cfg) void p2p_deinit(struct p2p_data *p2p) { +#ifdef CONFIG_WIFI_DISPLAY + wpabuf_free(p2p->wfd_ie_beacon); + wpabuf_free(p2p->wfd_ie_probe_req); + wpabuf_free(p2p->wfd_ie_probe_resp); + wpabuf_free(p2p->wfd_ie_assoc_req); + wpabuf_free(p2p->wfd_ie_invitation); + wpabuf_free(p2p->wfd_ie_prov_disc_req); + wpabuf_free(p2p->wfd_ie_prov_disc_resp); + wpabuf_free(p2p->wfd_ie_go_neg); + wpabuf_free(p2p->wfd_dev_info); + wpabuf_free(p2p->wfd_assoc_bssid); + wpabuf_free(p2p->wfd_coupled_sink_info); +#endif /* CONFIG_WIFI_DISPLAY */ + eloop_cancel_timeout(p2p_expiration_timeout, p2p, NULL); eloop_cancel_timeout(p2p_ext_listen_timeout, p2p, NULL); eloop_cancel_timeout(p2p_scan_timeout, p2p, NULL); @@ -2797,7 +2850,14 @@ void p2p_scan_res_handled(struct p2p_data *p2p) void p2p_scan_ie(struct p2p_data *p2p, struct wpabuf *ies, const u8 *dev_id) { - u8 *len = p2p_buf_add_ie_hdr(ies); + u8 *len; + +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_probe_req) + wpabuf_put_buf(ies, p2p->wfd_ie_probe_req); +#endif /* CONFIG_WIFI_DISPLAY */ + + len = p2p_buf_add_ie_hdr(ies); p2p_buf_add_capability(ies, p2p->dev_capab & ~P2P_DEV_CAPAB_CLIENT_DISCOVERABILITY, 0); if (dev_id) @@ -2816,7 +2876,14 @@ void p2p_scan_ie(struct p2p_data *p2p, struct wpabuf *ies, const u8 *dev_id) size_t p2p_scan_ie_buf_len(struct p2p_data *p2p) { - return 100; + size_t len = 100; + +#ifdef CONFIG_WIFI_DISPLAY + if (p2p && p2p->wfd_ie_probe_req) + len += wpabuf_len(p2p->wfd_ie_probe_req); +#endif /* CONFIG_WIFI_DISPLAY */ + + return len; } @@ -3492,6 +3559,24 @@ int p2p_get_peer_info_txt(const struct p2p_peer_info *info, pos += res; } +#ifdef CONFIG_WIFI_DISPLAY + if (dev->info.wfd_subelems) { + res = os_snprintf(pos, end - pos, "wfd_subelems="); + if (res < 0 || res >= end - pos) + return pos - buf; + pos += res; + + pos += wpa_snprintf_hex(pos, end - pos, + wpabuf_head(dev->info.wfd_subelems), + wpabuf_len(dev->info.wfd_subelems)); + + res = os_snprintf(pos, end - pos, "\n"); + if (res < 0 || res >= end - pos) + return pos - buf; + pos += res; + } +#endif /* CONFIG_WIFI_DISPLAY */ + return pos - buf; } @@ -4110,3 +4195,128 @@ void p2p_set_config_timeout(struct p2p_data *p2p, u8 go_timeout, p2p->client_timeout = client_timeout; } } + + +#ifdef CONFIG_WIFI_DISPLAY + +static void p2p_update_wfd_ie_groups(struct p2p_data *p2p) +{ + size_t g; + struct p2p_group *group; + + for (g = 0; g < p2p->num_groups; g++) { + group = p2p->groups[g]; + p2p_group_update_ies(group); + } +} + + +int p2p_set_wfd_ie_beacon(struct p2p_data *p2p, struct wpabuf *ie) +{ + wpabuf_free(p2p->wfd_ie_beacon); + p2p->wfd_ie_beacon = ie; + p2p_update_wfd_ie_groups(p2p); + return 0; +} + + +int p2p_set_wfd_ie_probe_req(struct p2p_data *p2p, struct wpabuf *ie) +{ + wpabuf_free(p2p->wfd_ie_probe_req); + p2p->wfd_ie_probe_req = ie; + return 0; +} + + +int p2p_set_wfd_ie_probe_resp(struct p2p_data *p2p, struct wpabuf *ie) +{ + wpabuf_free(p2p->wfd_ie_probe_resp); + p2p->wfd_ie_probe_resp = ie; + p2p_update_wfd_ie_groups(p2p); + return 0; +} + + +int p2p_set_wfd_ie_assoc_req(struct p2p_data *p2p, struct wpabuf *ie) +{ + wpabuf_free(p2p->wfd_ie_assoc_req); + p2p->wfd_ie_assoc_req = ie; + return 0; +} + + +int p2p_set_wfd_ie_invitation(struct p2p_data *p2p, struct wpabuf *ie) +{ + wpabuf_free(p2p->wfd_ie_invitation); + p2p->wfd_ie_invitation = ie; + return 0; +} + + +int p2p_set_wfd_ie_prov_disc_req(struct p2p_data *p2p, struct wpabuf *ie) +{ + wpabuf_free(p2p->wfd_ie_prov_disc_req); + p2p->wfd_ie_prov_disc_req = ie; + return 0; +} + + +int p2p_set_wfd_ie_prov_disc_resp(struct p2p_data *p2p, struct wpabuf *ie) +{ + wpabuf_free(p2p->wfd_ie_prov_disc_resp); + p2p->wfd_ie_prov_disc_resp = ie; + return 0; +} + + +int p2p_set_wfd_ie_go_neg(struct p2p_data *p2p, struct wpabuf *ie) +{ + wpabuf_free(p2p->wfd_ie_go_neg); + p2p->wfd_ie_go_neg = ie; + return 0; +} + + +int p2p_set_wfd_dev_info(struct p2p_data *p2p, const struct wpabuf *elem) +{ + wpabuf_free(p2p->wfd_dev_info); + if (elem) { + p2p->wfd_dev_info = wpabuf_dup(elem); + if (p2p->wfd_dev_info == NULL) + return -1; + } else + p2p->wfd_dev_info = NULL; + + return 0; +} + + +int p2p_set_wfd_assoc_bssid(struct p2p_data *p2p, const struct wpabuf *elem) +{ + wpabuf_free(p2p->wfd_assoc_bssid); + if (elem) { + p2p->wfd_assoc_bssid = wpabuf_dup(elem); + if (p2p->wfd_assoc_bssid == NULL) + return -1; + } else + p2p->wfd_assoc_bssid = NULL; + + return 0; +} + + +int p2p_set_wfd_coupled_sink_info(struct p2p_data *p2p, + const struct wpabuf *elem) +{ + wpabuf_free(p2p->wfd_coupled_sink_info); + if (elem) { + p2p->wfd_coupled_sink_info = wpabuf_dup(elem); + if (p2p->wfd_coupled_sink_info == NULL) + return -1; + } else + p2p->wfd_coupled_sink_info = NULL; + + return 0; +} + +#endif /* CONFIG_WIFI_DISPLAY */ diff --git a/src/p2p/p2p.h b/src/p2p/p2p.h index a60974f..ff15aee 100644 --- a/src/p2p/p2p.h +++ b/src/p2p/p2p.h @@ -211,6 +211,11 @@ struct p2p_peer_info { size_t wps_sec_dev_type_list_len; struct wpabuf *wps_vendor_ext[P2P_MAX_WPS_VENDOR_EXT]; + + /** + * wfd_subelems - Wi-Fi Display subelements from WFD IE(s) + */ + struct wpabuf *wfd_subelems; }; enum p2p_prov_disc_status { @@ -1731,4 +1736,18 @@ void p2p_set_config_timeout(struct p2p_data *p2p, u8 go_timeout, * @group: P2P group context from p2p_group_init() */ const u8 *p2p_group_get_interface_addr(struct p2p_group *group); +int p2p_set_wfd_ie_beacon(struct p2p_data *p2p, struct wpabuf *ie); +int p2p_set_wfd_ie_probe_req(struct p2p_data *p2p, struct wpabuf *ie); +int p2p_set_wfd_ie_probe_resp(struct p2p_data *p2p, struct wpabuf *ie); +int p2p_set_wfd_ie_assoc_req(struct p2p_data *p2p, struct wpabuf *ie); +int p2p_set_wfd_ie_invitation(struct p2p_data *p2p, struct wpabuf *ie); +int p2p_set_wfd_ie_prov_disc_req(struct p2p_data *p2p, struct wpabuf *ie); +int p2p_set_wfd_ie_prov_disc_resp(struct p2p_data *p2p, struct wpabuf *ie); +int p2p_set_wfd_ie_go_neg(struct p2p_data *p2p, struct wpabuf *ie); +int p2p_set_wfd_dev_info(struct p2p_data *p2p, const struct wpabuf *elem); +int p2p_set_wfd_assoc_bssid(struct p2p_data *p2p, const struct wpabuf *elem); +int p2p_set_wfd_coupled_sink_info(struct p2p_data *p2p, + const struct wpabuf *elem); +struct wpabuf * wifi_display_encaps(struct wpabuf *subelems); + #endif /* P2P_H */ diff --git a/src/p2p/p2p_go_neg.c b/src/p2p/p2p_go_neg.c index 60c19c6..3573fb7 100644 --- a/src/p2p/p2p_go_neg.c +++ b/src/p2p/p2p_go_neg.c @@ -134,8 +134,14 @@ static struct wpabuf * p2p_build_go_neg_req(struct p2p_data *p2p, struct wpabuf *buf; u8 *len; u8 group_capab; + size_t extra = 0; - buf = wpabuf_alloc(1000); +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_go_neg) + extra = wpabuf_len(p2p->wfd_ie_go_neg); +#endif /* CONFIG_WIFI_DISPLAY */ + + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -177,6 +183,11 @@ static struct wpabuf * p2p_build_go_neg_req(struct p2p_data *p2p, /* WPS IE with Device Password ID attribute */ p2p_build_wps_ie(p2p, buf, p2p_wps_method_pw_id(peer->wps_method), 0); +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_go_neg) + wpabuf_put_buf(buf, p2p->wfd_ie_go_neg); +#endif /* CONFIG_WIFI_DISPLAY */ + return buf; } @@ -246,10 +257,17 @@ static struct wpabuf * p2p_build_go_neg_resp(struct p2p_data *p2p, struct wpabuf *buf; u8 *len; u8 group_capab; + size_t extra = 0; wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Building GO Negotiation Response"); - buf = wpabuf_alloc(1000); + +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_go_neg) + extra = wpabuf_len(p2p->wfd_ie_go_neg); +#endif /* CONFIG_WIFI_DISPLAY */ + + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -308,6 +326,12 @@ static struct wpabuf * p2p_build_go_neg_resp(struct p2p_data *p2p, p2p_wps_method_pw_id(peer ? peer->wps_method : WPS_NOT_READY), 0); +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_go_neg) + wpabuf_put_buf(buf, p2p->wfd_ie_go_neg); +#endif /* CONFIG_WIFI_DISPLAY */ + + return buf; } @@ -748,10 +772,17 @@ static struct wpabuf * p2p_build_go_neg_conf(struct p2p_data *p2p, u8 *len; struct p2p_channels res; u8 group_capab; + size_t extra = 0; wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Building GO Negotiation Confirm"); - buf = wpabuf_alloc(1000); + +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_go_neg) + extra = wpabuf_len(p2p->wfd_ie_go_neg); +#endif /* CONFIG_WIFI_DISPLAY */ + + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -790,6 +821,11 @@ static struct wpabuf * p2p_build_go_neg_conf(struct p2p_data *p2p, } p2p_buf_update_ie_hdr(buf, len); +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_go_neg) + wpabuf_put_buf(buf, p2p->wfd_ie_go_neg); +#endif /* CONFIG_WIFI_DISPLAY */ + return buf; } diff --git a/src/p2p/p2p_group.c b/src/p2p/p2p_group.c index 8d4a3cb..514b5f1 100644 --- a/src/p2p/p2p_group.c +++ b/src/p2p/p2p_group.c @@ -22,6 +22,7 @@ struct p2p_group_member { u8 addr[ETH_ALEN]; /* P2P Interface Address */ u8 dev_addr[ETH_ALEN]; /* P2P Device Address */ struct wpabuf *p2p_ie; + struct wpabuf *wfd_ie; struct wpabuf *client_info; u8 dev_capab; }; @@ -37,12 +38,10 @@ struct p2p_group { int group_formation; int beacon_update; struct wpabuf *noa; + struct wpabuf *wfd_ie; }; -static void p2p_group_update_ies(struct p2p_group *group); - - struct p2p_group * p2p_group_init(struct p2p_data *p2p, struct p2p_group_config *config) { @@ -74,6 +73,7 @@ struct p2p_group * p2p_group_init(struct p2p_data *p2p, static void p2p_group_free_member(struct p2p_group_member *m) { + wpabuf_free(m->wfd_ie); wpabuf_free(m->p2p_ie); wpabuf_free(m->client_info); os_free(m); @@ -118,6 +118,7 @@ void p2p_group_deinit(struct p2p_group *group) p2p_group_free_members(group); os_free(group->cfg); wpabuf_free(group->noa); + wpabuf_free(group->wfd_ie); os_free(group); } @@ -172,11 +173,22 @@ static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group) { struct wpabuf *ie; u8 *len; + size_t extra = 0; + +#ifdef CONFIG_WIFI_DISPLAY + if (group->p2p->wfd_ie_beacon) + extra = wpabuf_len(group->p2p->wfd_ie_beacon); +#endif /* CONFIG_WIFI_DISPLAY */ - ie = wpabuf_alloc(257); + ie = wpabuf_alloc(257 + extra); if (ie == NULL) return NULL; +#ifdef CONFIG_WIFI_DISPLAY + if (group->p2p->wfd_ie_beacon) + wpabuf_put_buf(ie, group->p2p->wfd_ie_beacon); +#endif /* CONFIG_WIFI_DISPLAY */ + len = p2p_buf_add_ie_hdr(ie); p2p_group_add_common_ies(group, ie); p2p_buf_add_device_id(ie, group->p2p->cfg->dev_addr); @@ -187,17 +199,193 @@ static struct wpabuf * p2p_group_build_beacon_ie(struct p2p_group *group) } +#ifdef CONFIG_WIFI_DISPLAY + +struct wpabuf * p2p_group_get_wfd_ie(struct p2p_group *g) +{ + return g->wfd_ie; +} + + +struct wpabuf * wifi_display_encaps(struct wpabuf *subelems) +{ + struct wpabuf *ie; + const u8 *pos, *end; + + if (subelems == NULL) + return NULL; + + ie = wpabuf_alloc(wpabuf_len(subelems) + 100); + if (ie == NULL) + return NULL; + + pos = wpabuf_head(subelems); + end = pos + wpabuf_len(subelems); + + while (end > pos) { + size_t frag_len = end - pos; + if (frag_len > 251) + frag_len = 251; + wpabuf_put_u8(ie, WLAN_EID_VENDOR_SPECIFIC); + wpabuf_put_u8(ie, 4 + frag_len); + wpabuf_put_be32(ie, WFD_IE_VENDOR_TYPE); + wpabuf_put_data(ie, pos, frag_len); + pos += frag_len; + } + + return ie; +} + + +static int wifi_display_add_dev_info_descr(struct wpabuf *buf, + struct p2p_group_member *m) +{ + const u8 *pos, *end; + const u8 *dev_info = NULL; + const u8 *assoc_bssid = NULL; + const u8 *coupled_sink = NULL; + u8 zero_addr[ETH_ALEN]; + + if (m->wfd_ie == NULL) + return 0; + + os_memset(zero_addr, 0, ETH_ALEN); + pos = wpabuf_head_u8(m->wfd_ie); + end = pos + wpabuf_len(m->wfd_ie); + while (pos + 1 < end) { + u8 id; + u16 len; + + id = *pos++; + len = WPA_GET_BE16(pos); + pos += 2; + if (pos + len > end) + break; + + switch (id) { + case WFD_SUBELEM_DEVICE_INFO: + if (len < 6) + break; + dev_info = pos; + break; + case WFD_SUBELEM_ASSOCIATED_BSSID: + if (len < ETH_ALEN) + break; + assoc_bssid = pos; + break; + case WFD_SUBELEM_COUPLED_SINK: + if (len < 1 + ETH_ALEN) + break; + coupled_sink = pos; + break; + } + + pos += len; + } + + if (dev_info == NULL) + return 0; + + wpabuf_put_u8(buf, 23); + wpabuf_put_data(buf, m->dev_addr, ETH_ALEN); + if (assoc_bssid) + wpabuf_put_data(buf, assoc_bssid, ETH_ALEN); + else + wpabuf_put_data(buf, zero_addr, ETH_ALEN); + wpabuf_put_data(buf, dev_info, 2); /* WFD Device Info */ + wpabuf_put_data(buf, dev_info + 4, 2); /* WFD Device Max Throughput */ + if (coupled_sink) { + wpabuf_put_data(buf, coupled_sink, 1 + ETH_ALEN); + } else { + wpabuf_put_u8(buf, 0); + wpabuf_put_data(buf, zero_addr, ETH_ALEN); + } + + return 1; +} + + +static struct wpabuf * +wifi_display_build_go_ie(struct p2p_group *group) +{ + struct wpabuf *wfd_subelems, *wfd_ie; + struct p2p_group_member *m; + u8 *len; + unsigned int count = 0; + + if (!group->p2p->wfd_ie_probe_resp) + return NULL; + + wfd_subelems = wpabuf_alloc(wpabuf_len(group->p2p->wfd_ie_probe_resp) + + group->num_members * 24 + 100); + if (wfd_subelems == NULL) + return NULL; + if (group->p2p->wfd_dev_info) + wpabuf_put_buf(wfd_subelems, group->p2p->wfd_dev_info); + if (group->p2p->wfd_assoc_bssid) + wpabuf_put_buf(wfd_subelems, + group->p2p->wfd_assoc_bssid); + if (group->p2p->wfd_coupled_sink_info) + wpabuf_put_buf(wfd_subelems, + group->p2p->wfd_coupled_sink_info); + + /* Build WFD Session Info */ + wpabuf_put_u8(wfd_subelems, WFD_SUBELEM_SESSION_INFO); + len = wpabuf_put(wfd_subelems, 2); + m = group->members; + while (m) { + if (wifi_display_add_dev_info_descr(wfd_subelems, m)) + count++; + m = m->next; + } + + if (count == 0) { + /* No Wi-Fi Display clients - do not include subelement */ + wfd_subelems->used -= 3; + } else { + WPA_PUT_BE16(len, (u8 *) wpabuf_put(wfd_subelems, 0) - len - + 2); + wpa_printf(MSG_DEBUG, "WFD: WFD Session Info: %u descriptors", + count); + } + + wfd_ie = wifi_display_encaps(wfd_subelems); + wpabuf_free(wfd_subelems); + + return wfd_ie; +} + +static void wifi_display_group_update(struct p2p_group *group) +{ + wpabuf_free(group->wfd_ie); + group->wfd_ie = wifi_display_build_go_ie(group); +} + +#endif /* CONFIG_WIFI_DISPLAY */ + + static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group) { u8 *group_info; struct wpabuf *ie; struct p2p_group_member *m; u8 *len; + size_t extra = 0; + +#ifdef CONFIG_WIFI_DISPLAY + if (group->wfd_ie) + extra += wpabuf_len(group->wfd_ie); +#endif /* CONFIG_WIFI_DISPLAY */ - ie = wpabuf_alloc(257); + ie = wpabuf_alloc(257 + extra); if (ie == NULL) return NULL; +#ifdef CONFIG_WIFI_DISPLAY + if (group->wfd_ie) + wpabuf_put_buf(ie, group->wfd_ie); +#endif /* CONFIG_WIFI_DISPLAY */ + len = p2p_buf_add_ie_hdr(ie); p2p_group_add_common_ies(group, ie); @@ -216,15 +404,20 @@ static struct wpabuf * p2p_group_build_probe_resp_ie(struct p2p_group *group) (u8 *) wpabuf_put(ie, 0) - group_info - 3); p2p_buf_update_ie_hdr(ie, len); + return ie; } -static void p2p_group_update_ies(struct p2p_group *group) +void p2p_group_update_ies(struct p2p_group *group) { struct wpabuf *beacon_ie; struct wpabuf *probe_resp_ie; +#ifdef CONFIG_WIFI_DISPLAY + wifi_display_group_update(group); +#endif /* CONFIG_WIFI_DISPLAY */ + probe_resp_ie = p2p_group_build_probe_resp_ie(group); if (probe_resp_ie == NULL) return; @@ -354,6 +547,9 @@ int p2p_group_notif_assoc(struct p2p_group *group, const u8 *addr, &m->dev_capab, m->dev_addr); } +#ifdef CONFIG_WIFI_DISPLAY + m->wfd_ie = ieee802_11_vendor_ie_concat(ie, len, WFD_IE_VENDOR_TYPE); +#endif /* CONFIG_WIFI_DISPLAY */ p2p_group_remove_member(group, addr); @@ -361,8 +557,9 @@ int p2p_group_notif_assoc(struct p2p_group *group, const u8 *addr, group->members = m; group->num_members++; wpa_msg(group->p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Add client " MACSTR - " to group (p2p=%d client_info=%d); num_members=%u/%u", - MAC2STR(addr), m->p2p_ie ? 1 : 0, m->client_info ? 1 : 0, + " to group (p2p=%d wfd=%d client_info=%d); num_members=%u/%u", + MAC2STR(addr), m->p2p_ie ? 1 : 0, m->wfd_ie ? 1 : 0, + m->client_info ? 1 : 0, group->num_members, group->cfg->max_clients); if (group->num_members == group->cfg->max_clients) group->beacon_update = 1; @@ -378,6 +575,12 @@ struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status) { struct wpabuf *resp; u8 *rlen; + size_t extra = 0; + +#ifdef CONFIG_WIFI_DISPLAY + if (group->wfd_ie) + extra = wpabuf_len(group->wfd_ie); +#endif /* CONFIG_WIFI_DISPLAY */ /* * (Re)Association Response - P2P IE @@ -385,9 +588,15 @@ struct wpabuf * p2p_group_assoc_resp_ie(struct p2p_group *group, u8 status) * denied) * Extended Listen Timing (may be present) */ - resp = wpabuf_alloc(20); + resp = wpabuf_alloc(20 + extra); if (resp == NULL) return NULL; + +#ifdef CONFIG_WIFI_DISPLAY + if (group->wfd_ie) + wpabuf_put_buf(resp, group->wfd_ie); +#endif /* CONFIG_WIFI_DISPLAY */ + rlen = p2p_buf_add_ie_hdr(resp); if (status != P2P_SC_SUCCESS) p2p_buf_add_status(resp, status); diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h index 9e3fd6f..1a15325 100644 --- a/src/p2p/p2p_i.h +++ b/src/p2p/p2p_i.h @@ -435,6 +435,20 @@ struct p2p_data { u8 go_timeout; u8 client_timeout; + +#ifdef CONFIG_WIFI_DISPLAY + struct wpabuf *wfd_ie_beacon; + struct wpabuf *wfd_ie_probe_req; + struct wpabuf *wfd_ie_probe_resp; + struct wpabuf *wfd_ie_assoc_req; + struct wpabuf *wfd_ie_invitation; + struct wpabuf *wfd_ie_prov_disc_req; + struct wpabuf *wfd_ie_prov_disc_resp; + struct wpabuf *wfd_ie_go_neg; + struct wpabuf *wfd_dev_info; + struct wpabuf *wfd_assoc_bssid; + struct wpabuf *wfd_coupled_sink_info; +#endif /* CONFIG_WIFI_DISPLAY */ }; /** @@ -443,6 +457,7 @@ struct p2p_data { struct p2p_message { struct wpabuf *p2p_attributes; struct wpabuf *wps_attributes; + struct wpabuf *wfd_subelems; u8 dialog_token; @@ -562,6 +577,8 @@ u8 p2p_group_presence_req(struct p2p_group *group, const u8 *noa, size_t noa_len); int p2p_group_is_group_id_match(struct p2p_group *group, const u8 *group_id, size_t group_id_len); +void p2p_group_update_ies(struct p2p_group *group); +struct wpabuf * p2p_group_get_wfd_ie(struct p2p_group *g); void p2p_buf_add_action_hdr(struct wpabuf *buf, u8 subtype, u8 dialog_token); diff --git a/src/p2p/p2p_invitation.c b/src/p2p/p2p_invitation.c index c70fc7a..8e3f5d2 100644 --- a/src/p2p/p2p_invitation.c +++ b/src/p2p/p2p_invitation.c @@ -21,8 +21,27 @@ static struct wpabuf * p2p_build_invitation_req(struct p2p_data *p2p, struct wpabuf *buf; u8 *len; const u8 *dev_addr; + size_t extra = 0; + +#ifdef CONFIG_WIFI_DISPLAY + struct wpabuf *wfd_ie = p2p->wfd_ie_invitation; + if (wfd_ie && p2p->inv_role == P2P_INVITE_ROLE_ACTIVE_GO) { + size_t i; + for (i = 0; i < p2p->num_groups; i++) { + struct p2p_group *g = p2p->groups[i]; + struct wpabuf *ie; + ie = p2p_group_get_wfd_ie(g); + if (ie) { + wfd_ie = ie; + break; + } + } + } + if (wfd_ie) + extra = wpabuf_len(wfd_ie); +#endif /* CONFIG_WIFI_DISPLAY */ - buf = wpabuf_alloc(1000); + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -55,6 +74,11 @@ static struct wpabuf * p2p_build_invitation_req(struct p2p_data *p2p, p2p_buf_add_device_info(buf, p2p, peer); p2p_buf_update_ie_hdr(buf, len); +#ifdef CONFIG_WIFI_DISPLAY + if (wfd_ie) + wpabuf_put_buf(buf, wfd_ie); +#endif /* CONFIG_WIFI_DISPLAY */ + return buf; } @@ -68,8 +92,30 @@ static struct wpabuf * p2p_build_invitation_resp(struct p2p_data *p2p, { struct wpabuf *buf; u8 *len; + size_t extra = 0; + +#ifdef CONFIG_WIFI_DISPLAY + struct wpabuf *wfd_ie = p2p->wfd_ie_invitation; + if (wfd_ie && group_bssid) { + size_t i; + for (i = 0; i < p2p->num_groups; i++) { + struct p2p_group *g = p2p->groups[i]; + struct wpabuf *ie; + if (!p2p_group_is_group_id_match(g, group_bssid, + ETH_ALEN)) + continue; + ie = p2p_group_get_wfd_ie(g); + if (ie) { + wfd_ie = ie; + break; + } + } + } + if (wfd_ie) + extra = wpabuf_len(wfd_ie); +#endif /* CONFIG_WIFI_DISPLAY */ - buf = wpabuf_alloc(1000); + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -88,6 +134,11 @@ static struct wpabuf * p2p_build_invitation_resp(struct p2p_data *p2p, p2p_buf_add_channel_list(buf, p2p->cfg->country, channels); p2p_buf_update_ie_hdr(buf, len); +#ifdef CONFIG_WIFI_DISPLAY + if (wfd_ie) + wpabuf_put_buf(buf, wfd_ie); +#endif /* CONFIG_WIFI_DISPLAY */ + return buf; } diff --git a/src/p2p/p2p_parse.c b/src/p2p/p2p_parse.c index a3ec57d..097a31d 100644 --- a/src/p2p/p2p_parse.c +++ b/src/p2p/p2p_parse.c @@ -414,6 +414,13 @@ int p2p_parse_ies(const u8 *data, size_t len, struct p2p_message *msg) return -1; } +#ifdef CONFIG_WIFI_DISPLAY + if (elems.wfd) { + msg->wfd_subelems = ieee802_11_vendor_ie_concat( + data, len, WFD_IE_VENDOR_TYPE); + } +#endif /* CONFIG_WIFI_DISPLAY */ + return 0; } @@ -453,6 +460,10 @@ void p2p_parse_free(struct p2p_message *msg) msg->p2p_attributes = NULL; wpabuf_free(msg->wps_attributes); msg->wps_attributes = NULL; +#ifdef CONFIG_WIFI_DISPLAY + wpabuf_free(msg->wfd_subelems); + msg->wfd_subelems = NULL; +#endif /* CONFIG_WIFI_DISPLAY */ } diff --git a/src/p2p/p2p_pd.c b/src/p2p/p2p_pd.c index 007b643..610f0df 100644 --- a/src/p2p/p2p_pd.c +++ b/src/p2p/p2p_pd.c @@ -46,8 +46,14 @@ static struct wpabuf * p2p_build_prov_disc_req(struct p2p_data *p2p, { struct wpabuf *buf; u8 *len; + size_t extra = 0; - buf = wpabuf_alloc(1000); +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_prov_disc_req) + extra = wpabuf_len(p2p->wfd_ie_prov_disc_req); +#endif /* CONFIG_WIFI_DISPLAY */ + + buf = wpabuf_alloc(1000 + extra); if (buf == NULL) return NULL; @@ -66,17 +72,46 @@ static struct wpabuf * p2p_build_prov_disc_req(struct p2p_data *p2p, /* WPS IE with Config Methods attribute */ p2p_build_wps_ie_config_methods(buf, config_methods); +#ifdef CONFIG_WIFI_DISPLAY + if (p2p->wfd_ie_prov_disc_req) + wpabuf_put_buf(buf, p2p->wfd_ie_prov_disc_req); +#endif /* CONFIG_WIFI_DISPLAY */ + return buf; } static struct wpabuf * p2p_build_prov_disc_resp(struct p2p_data *p2p, u8 dialog_token, - u16 config_methods) + u16 config_methods, + const u8 *group_id, + size_t group_id_len) { struct wpabuf *buf; + size_t extra = 0; + +#ifdef CONFIG_WIFI_DISPLAY + struct wpabuf *wfd_ie = p2p->wfd_ie_prov_disc_resp; + if (wfd_ie && group_id) { + size_t i; + for (i = 0; i < p2p->num_groups; i++) { + struct p2p_group *g = p2p->groups[i]; + struct wpabuf *ie; + if (!p2p_group_is_group_id_match(g, group_id, + group_id_len)) + continue; + ie = p2p_group_get_wfd_ie(g); + if (ie) { + wfd_ie = ie; + break; + } + } + } + if (wfd_ie) + extra = wpabuf_len(wfd_ie); +#endif /* CONFIG_WIFI_DISPLAY */ - buf = wpabuf_alloc(100); + buf = wpabuf_alloc(100 + extra); if (buf == NULL) return NULL; @@ -85,6 +120,11 @@ static struct wpabuf * p2p_build_prov_disc_resp(struct p2p_data *p2p, /* WPS IE with Config Methods attribute */ p2p_build_wps_ie_config_methods(buf, config_methods); +#ifdef CONFIG_WIFI_DISPLAY + if (wfd_ie) + wpabuf_put_buf(buf, wfd_ie); +#endif /* CONFIG_WIFI_DISPLAY */ + return buf; } @@ -119,6 +159,9 @@ void p2p_process_prov_disc_req(struct p2p_data *p2p, const u8 *sa, "P2P: Provision Discovery Request add device " "failed " MACSTR, MAC2STR(sa)); } + } else if (msg.wfd_subelems) { + wpabuf_free(dev->info.wfd_subelems); + dev->info.wfd_subelems = wpabuf_dup(msg.wfd_subelems); } if (!(msg.wps_config_methods & @@ -164,7 +207,8 @@ void p2p_process_prov_disc_req(struct p2p_data *p2p, const u8 *sa, out: resp = p2p_build_prov_disc_resp(p2p, msg.dialog_token, - reject ? 0 : msg.wps_config_methods); + reject ? 0 : msg.wps_config_methods, + msg.group_id, msg.group_id_len); if (resp == NULL) { p2p_parse_free(&msg); return; -- cgit v1.1