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 <list of roles> <list of subelements>
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 <jouni@qca.qualcomm.com>

diff --git a/src/common/ieee802_11_defs.h b/src/common/ieee802_11_defs.h
index ad4f260..2ab7fbf 100644 (file)
--- 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 b2ecc22..9ea4694 100644 (file)
--- 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 f4646ee..8602333 100644 (file)
--- 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 5cf1cfe..abe1d6b 100644 (file)
--- 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 c71fc1c..6b81397 100644 (file)
--- 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 <list of roles> <list of subelements>
+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 <query identifier>
 
 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 5950daf..d7ce6fe 100644 (file)
--- 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 59ceae7..93acf5f 100644 (file)
--- 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 bb84bc4..1e0e68f 100644 (file)
--- 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,