From 347d6a5b763e5e6bf123d67cb616b68c5f1faafb Mon Sep 17 00:00:00 2001 From: Jouni Malinen Date: Fri, 2 Mar 2012 21:48:57 +0200 Subject: [PATCH] WFD: Add support for sending Wi-Fi Display service discovery requests wpa_cli p2p_serv_disc_req command can now be used to request WSD request to be sent to specified or all peers who support WSD. format: wifi-display examples: p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [source] 2,3,4,5 p2p_serv_disc_req 02:01:02:03:04:05 wifi-display [pri-sink] 3 p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [sec-source] 2 p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [source+sink] 2,3,4,5 p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [source][pri-sink] 2,3,4,5 Signed-hostap: Jouni Malinen --- src/common/ieee802_11_defs.h | 1 + src/p2p/p2p.h | 5 ++ src/p2p/p2p_i.h | 1 + src/p2p/p2p_sd.c | 53 +++++++++++++++++++++ wpa_supplicant/README-P2P | 8 ++++ wpa_supplicant/ctrl_iface.c | 4 ++ wpa_supplicant/p2p_supplicant.c | 82 +++++++++++++++++++++++++++++++++ wpa_supplicant/p2p_supplicant.h | 2 + 8 files changed, 156 insertions(+) diff --git a/src/common/ieee802_11_defs.h b/src/common/ieee802_11_defs.h index ad4f26053..2ab7fbf49 100644 --- a/src/common/ieee802_11_defs.h +++ b/src/common/ieee802_11_defs.h @@ -925,6 +925,7 @@ enum p2p_service_protocol_type { P2P_SERV_BONJOUR = 1, P2P_SERV_UPNP = 2, P2P_SERV_WS_DISCOVERY = 3, + P2P_SERV_WIFI_DISPLAY = 4, P2P_SERV_VENDOR_SPECIFIC = 255 }; diff --git a/src/p2p/p2p.h b/src/p2p/p2p.h index b2ecc2200..9ea4694e7 100644 --- a/src/p2p/p2p.h +++ b/src/p2p/p2p.h @@ -964,6 +964,11 @@ int p2p_prov_disc_req(struct p2p_data *p2p, const u8 *peer_addr, void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst, const struct wpabuf *tlvs); +#ifdef CONFIG_WIFI_DISPLAY +void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst, + const struct wpabuf *tlvs); +#endif /* CONFIG_WIFI_DISPLAY */ + /** * p2p_sd_cancel_request - Cancel a pending service discovery query * @p2p: P2P module context from p2p_init() diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h index f4646ee7e..860233323 100644 --- a/src/p2p/p2p_i.h +++ b/src/p2p/p2p_i.h @@ -109,6 +109,7 @@ struct p2p_sd_query { struct p2p_sd_query *next; u8 peer[ETH_ALEN]; int for_all_peers; + int wsd; /* Wi-Fi Display Service Discovery Request */ struct wpabuf *tlvs; }; diff --git a/src/p2p/p2p_sd.c b/src/p2p/p2p_sd.c index 5cf1cfe6b..abe1d6b58 100644 --- a/src/p2p/p2p_sd.c +++ b/src/p2p/p2p_sd.c @@ -15,15 +15,55 @@ #include "p2p.h" +#ifdef CONFIG_WIFI_DISPLAY +static int wfd_wsd_supported(struct wpabuf *wfd) +{ + const u8 *pos, *end; + u8 subelem; + u16 len; + + if (wfd == NULL) + return 0; + + pos = wpabuf_head(wfd); + end = pos + wpabuf_len(wfd); + + while (pos + 3 <= end) { + subelem = *pos++; + len = WPA_GET_BE16(pos); + pos += 2; + if (pos + len > end) + break; + + if (subelem == WFD_SUBELEM_DEVICE_INFO && len >= 6) { + u16 info = WPA_GET_BE16(pos); + return !!(info & 0x0040); + } + + pos += len; + } + + return 0; +} +#endif /* CONFIG_WIFI_DISPLAY */ + struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p, struct p2p_device *dev) { struct p2p_sd_query *q; + int wsd = 0; if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY)) return NULL; /* peer does not support SD */ +#ifdef CONFIG_WIFI_DISPLAY + if (wfd_wsd_supported(dev->info.wfd_subelems)) + wsd = 1; +#endif /* CONFIG_WIFI_DISPLAY */ for (q = p2p->sd_queries; q; q = q->next) { + /* Use WSD only if the peer indicates support or it */ + if (q->wsd && !wsd) + continue; if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO)) return q; if (!q->for_all_peers && @@ -876,6 +916,19 @@ void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst, } +#ifdef CONFIG_WIFI_DISPLAY +void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst, + const struct wpabuf *tlvs) +{ + struct p2p_sd_query *q; + q = p2p_sd_request(p2p, dst, tlvs); + if (q) + q->wsd = 1; + return q; +} +#endif /* CONFIG_WIFI_DISPLAY */ + + void p2p_sd_service_update(struct p2p_data *p2p) { p2p->srv_update_indic++; diff --git a/wpa_supplicant/README-P2P b/wpa_supplicant/README-P2P index c71fc1cb1..6b81397f7 100644 --- a/wpa_supplicant/README-P2P +++ b/wpa_supplicant/README-P2P @@ -256,6 +256,14 @@ p2p_serv_disc_req 00:00:00:00:00:00 upnp 10 urn:schemas-upnp-org:service:Content p2p_serv_disc_req 00:00:00:00:00:00 upnp 10 uuid:6859dede-8574-59ab-9332-123456789012 p2p_serv_disc_req 00:00:00:00:00:00 upnp 10 urn:schemas-upnp-org:device:InternetGatewayDevice:1 +# Wi-Fi Display examples +# format: wifi-display +p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [source] 2,3,4,5 +p2p_serv_disc_req 02:01:02:03:04:05 wifi-display [pri-sink] 3 +p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [sec-source] 2 +p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [source+sink] 2,3,4,5 +p2p_serv_disc_req 00:00:00:00:00:00 wifi-display [source][pri-sink] 2,3,4,5 + p2p_serv_disc_cancel_req Cancel a pending P2P service discovery request. This command takes a diff --git a/wpa_supplicant/ctrl_iface.c b/wpa_supplicant/ctrl_iface.c index 5950daf67..d7ce6feec 100644 --- a/wpa_supplicant/ctrl_iface.c +++ b/wpa_supplicant/ctrl_iface.c @@ -3252,6 +3252,10 @@ static int p2p_ctrl_serv_disc_req(struct wpa_supplicant *wpa_s, char *cmd, return -1; pos++; ref = wpas_p2p_sd_request_upnp(wpa_s, dst, version, pos); +#ifdef CONFIG_WIFI_DISPLAY + } else if (os_strncmp(pos, "wifi-display ", 13) == 0) { + ref = wpas_p2p_sd_request_wifi_display(wpa_s, dst, pos + 13); +#endif /* CONFIG_WIFI_DISPLAY */ } else { len = os_strlen(pos); if (len & 1) diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c index 59ceae7fe..93acf5f10 100644 --- a/wpa_supplicant/p2p_supplicant.c +++ b/wpa_supplicant/p2p_supplicant.c @@ -1687,6 +1687,88 @@ u64 wpas_p2p_sd_request_upnp(struct wpa_supplicant *wpa_s, const u8 *dst, } +#ifdef CONFIG_WIFI_DISPLAY + +static u64 wpas_p2p_sd_request_wfd(struct wpa_supplicant *wpa_s, const u8 *dst, + const struct wpabuf *tlvs) +{ + if (wpa_s->drv_flags & WPA_DRIVER_FLAGS_P2P_MGMT) + return 0; + if (wpa_s->global->p2p_disabled || wpa_s->global->p2p == NULL) + return 0; + return (uintptr_t) p2p_sd_request_wfd(wpa_s->global->p2p, dst, tlvs); +} + + +#define MAX_WFD_SD_SUBELEMS 20 + +static void wfd_add_sd_req_role(struct wpabuf *tlvs, u8 id, u8 role, + const char *subelems) +{ + u8 *len; + const char *pos; + int val; + int count = 0; + + len = wpabuf_put(tlvs, 2); + wpabuf_put_u8(tlvs, P2P_SERV_WIFI_DISPLAY); /* Service Protocol Type */ + wpabuf_put_u8(tlvs, id); /* Service Transaction ID */ + + wpabuf_put_u8(tlvs, role); + + pos = subelems; + while (*pos) { + val = atoi(pos); + if (val >= 0 && val < 256) { + wpabuf_put_u8(tlvs, val); + count++; + if (count == MAX_WFD_SD_SUBELEMS) + break; + } + pos = os_strchr(pos + 1, ','); + if (pos == NULL) + break; + pos++; + } + + WPA_PUT_LE16(len, (u8 *) wpabuf_put(tlvs, 0) - len - 2); +} + + +u64 wpas_p2p_sd_request_wifi_display(struct wpa_supplicant *wpa_s, + const u8 *dst, const char *role) +{ + struct wpabuf *tlvs; + u64 ret; + const char *subelems; + u8 id = 1; + + subelems = os_strchr(role, ' '); + if (subelems == NULL) + return 0; + subelems++; + + tlvs = wpabuf_alloc(4 * (2 + 1 + 1 + 1 + MAX_WFD_SD_SUBELEMS)); + if (tlvs == NULL) + return 0; + + if (os_strstr(role, "[source]")) + wfd_add_sd_req_role(tlvs, id++, 0x00, subelems); + if (os_strstr(role, "[pri-sink]")) + wfd_add_sd_req_role(tlvs, id++, 0x01, subelems); + if (os_strstr(role, "[sec-sink]")) + wfd_add_sd_req_role(tlvs, id++, 0x02, subelems); + if (os_strstr(role, "[source+sink]")) + wfd_add_sd_req_role(tlvs, id++, 0x03, subelems); + + ret = wpas_p2p_sd_request_wfd(wpa_s, dst, tlvs); + wpabuf_free(tlvs); + return ret; +} + +#endif /* CONFIG_WIFI_DISPLAY */ + + int wpas_p2p_sd_cancel_request(struct wpa_supplicant *wpa_s, u64 req) { if (wpa_s->drv_flags & WPA_DRIVER_FLAGS_P2P_MGMT) diff --git a/wpa_supplicant/p2p_supplicant.h b/wpa_supplicant/p2p_supplicant.h index bb84bc4b0..1e0e68f6a 100644 --- a/wpa_supplicant/p2p_supplicant.h +++ b/wpa_supplicant/p2p_supplicant.h @@ -86,6 +86,8 @@ u64 wpas_p2p_sd_request(struct wpa_supplicant *wpa_s, const u8 *dst, const struct wpabuf *tlvs); u64 wpas_p2p_sd_request_upnp(struct wpa_supplicant *wpa_s, const u8 *dst, u8 version, const char *query); +u64 wpas_p2p_sd_request_wifi_display(struct wpa_supplicant *wpa_s, + const u8 *dst, const char *role); int wpas_p2p_sd_cancel_request(struct wpa_supplicant *wpa_s, u64 req); void wpas_p2p_sd_response(struct wpa_supplicant *wpa_s, int freq, const u8 *dst, u8 dialog_token,