P2PS: Logic to parse GAS requests for ASP services

Add support to parse received GAS requests for ASP services and
prepare GAS responses accordingly.

Signed-off-by: Jouni Malinen <jouni@qca.qualcomm.com>

diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c
index 7190760c4f2aa41dcb22c904af580734bc671fb0..1847cf52b5bbf68b6d8ffafe022aea9112574529 100644 (file)
--- a/src/p2p/p2p.c
+++ b/src/p2p/p2p.c
@@ -153,6 +153,12 @@ const char * p2p_get_state_txt(struct p2p_data *p2p)
 }
 
 
+struct p2ps_advertisement * p2p_get_p2ps_adv_list(struct p2p_data *p2p)
+{
+       return p2p ? p2p->p2ps_adv_list : NULL;
+}
+
+
 u16 p2p_get_provisioning_info(struct p2p_data *p2p, const u8 *addr)
 {
        struct p2p_device *dev = NULL;

diff --git a/src/p2p/p2p.h b/src/p2p/p2p.h
index a6c6110c94a093618ddc3597180ef344b66e2482..bbf9cbd96aa3d26fe86ae87335994109cf123340 100644 (file)
--- a/src/p2p/p2p.h
+++ b/src/p2p/p2p.h
@@ -2095,5 +2095,6 @@ int p2p_service_add_asp(struct p2p_data *p2p, int auto_accept, u32 adv_id,
                        const char *adv_str, u8 svc_state,
                        u16 config_methods, const char *svc_info);
 int p2p_service_del_asp(struct p2p_data *p2p, u32 adv_id);
+struct p2ps_advertisement * p2p_get_p2ps_adv_list(struct p2p_data *p2p);
 
 #endif /* P2P_H */

diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c
index f980c94d995715205e9fe2d23535137740fbe88a..45e2177ab07aa43194a2aa57cab0072a488927fa 100644 (file)
--- a/wpa_supplicant/p2p_supplicant.c
+++ b/wpa_supplicant/p2p_supplicant.c
@@ -2236,8 +2236,8 @@ wpas_p2p_service_get_upnp(struct wpa_supplicant *wpa_s, u8 version,
 }
 
 
-static void wpas_sd_add_proto_not_avail(struct wpabuf *resp, u8 srv_proto,
-                                       u8 srv_trans_id)
+static void wpas_sd_add_empty(struct wpabuf *resp, u8 srv_proto,
+                             u8 srv_trans_id, u8 status)
 {
        u8 *len_pos;
 
@@ -2249,12 +2249,35 @@ static void wpas_sd_add_proto_not_avail(struct wpabuf *resp, u8 srv_proto,
        wpabuf_put_u8(resp, srv_proto);
        wpabuf_put_u8(resp, srv_trans_id);
        /* Status Code */
-       wpabuf_put_u8(resp, P2P_SD_PROTO_NOT_AVAILABLE);
+       wpabuf_put_u8(resp, status);
        /* Response Data: empty */
        WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(resp, 0) - len_pos - 2);
 }
 
 
+static void wpas_sd_add_proto_not_avail(struct wpabuf *resp, u8 srv_proto,
+                                       u8 srv_trans_id)
+{
+       wpas_sd_add_empty(resp, srv_proto, srv_trans_id,
+                         P2P_SD_PROTO_NOT_AVAILABLE);
+}
+
+
+static void wpas_sd_add_bad_request(struct wpabuf *resp, u8 srv_proto,
+                                   u8 srv_trans_id)
+{
+       wpas_sd_add_empty(resp, srv_proto, srv_trans_id, P2P_SD_BAD_REQUEST);
+}
+
+
+static void wpas_sd_add_not_found(struct wpabuf *resp, u8 srv_proto,
+                                 u8 srv_trans_id)
+{
+       wpas_sd_add_empty(resp, srv_proto, srv_trans_id,
+                         P2P_SD_REQUESTED_INFO_NOT_AVAILABLE);
+}
+
+
 static void wpas_sd_all_bonjour(struct wpa_supplicant *wpa_s,
                                struct wpabuf *resp, u8 srv_trans_id)
 {
@@ -2562,6 +2585,148 @@ static void wpas_sd_req_wfd(struct wpa_supplicant *wpa_s,
 #endif /* CONFIG_WIFI_DISPLAY */
 
 
+static int find_p2ps_substr(struct p2ps_advertisement *adv_data,
+                           const u8 *needle, size_t needle_len)
+{
+       const u8 *haystack = (const u8 *) adv_data->svc_info;
+       size_t haystack_len, i;
+
+       /* Allow search term to be empty */
+       if (!needle || !needle_len)
+               return 1;
+
+       if (!haystack)
+               return 0;
+
+       haystack_len = os_strlen(adv_data->svc_info);
+       for (i = 0; i < haystack_len; i++) {
+               if (haystack_len - i < needle_len)
+                       break;
+               if (os_memcmp(haystack + i, needle, needle_len) == 0)
+                       return 1;
+       }
+
+       return 0;
+}
+
+
+static void wpas_sd_req_asp(struct wpa_supplicant *wpa_s,
+                           struct wpabuf *resp, u8 srv_trans_id,
+                           const u8 *query, size_t query_len)
+{
+       struct p2ps_advertisement *adv_data;
+       const u8 *svc = &query[1];
+       const u8 *info = NULL;
+       size_t svc_len = query[0];
+       size_t info_len = 0;
+       int prefix = 0;
+       u8 *count_pos = NULL;
+       u8 *len_pos = NULL;
+
+       wpa_hexdump(MSG_DEBUG, "P2P: SD Request for ASP", query, query_len);
+
+       if (!wpa_s->global->p2p) {
+               wpa_printf(MSG_DEBUG, "P2P: ASP protocol not available");
+               wpas_sd_add_proto_not_avail(resp, P2P_SERV_P2PS, srv_trans_id);
+               return;
+       }
+
+       /* Info block is optional */
+       if (svc_len + 1 < query_len) {
+               info = &svc[svc_len];
+               info_len = *info++;
+       }
+
+       /* Range check length of svc string and info block */
+       if (svc_len + (info_len ? info_len + 2 : 1) > query_len) {
+               wpa_printf(MSG_DEBUG, "P2P: ASP bad request");
+               wpas_sd_add_bad_request(resp, P2P_SERV_P2PS, srv_trans_id);
+               return;
+       }
+
+       /* Detect and correct for prefix search */
+       if (svc_len && svc[svc_len - 1] == '*') {
+               prefix = 1;
+               svc_len--;
+       }
+
+       for (adv_data = p2p_get_p2ps_adv_list(wpa_s->global->p2p);
+            adv_data; adv_data = adv_data->next) {
+               /* If not a prefix match, reject length mismatches */
+               if (!prefix && svc_len != os_strlen(adv_data->svc_name))
+                       continue;
+
+               /* Search each service for request */
+               if (os_memcmp(adv_data->svc_name, svc, svc_len) == 0 &&
+                   find_p2ps_substr(adv_data, info, info_len)) {
+                       size_t len = os_strlen(adv_data->svc_name);
+                       size_t svc_info_len = 0;
+
+                       if (adv_data->svc_info)
+                               svc_info_len = os_strlen(adv_data->svc_info);
+
+                       if (len > 0xff || svc_info_len > 0xffff)
+                               return;
+
+                       /* Length & Count to be filled as we go */
+                       if (!len_pos && !count_pos) {
+                               if (wpabuf_tailroom(resp) <
+                                   len + svc_info_len + 16)
+                                       return;
+
+                               len_pos = wpabuf_put(resp, 2);
+                               wpabuf_put_u8(resp, P2P_SERV_P2PS);
+                               wpabuf_put_u8(resp, srv_trans_id);
+                               /* Status Code */
+                               wpabuf_put_u8(resp, P2P_SD_SUCCESS);
+                               count_pos = wpabuf_put(resp, 1);
+                               *count_pos = 0;
+                       } else if (wpabuf_tailroom(resp) <
+                                  len + svc_info_len + 10)
+                               return;
+
+                       if (svc_info_len) {
+                               wpa_printf(MSG_DEBUG,
+                                          "P2P: Add Svc: %s info: %s",
+                                          adv_data->svc_name,
+                                          adv_data->svc_info);
+                       } else {
+                               wpa_printf(MSG_DEBUG, "P2P: Add Svc: %s",
+                                          adv_data->svc_name);
+                       }
+
+                       /* Advertisement ID */
+                       wpabuf_put_le32(resp, adv_data->id);
+
+                       /* Config Methods */
+                       wpabuf_put_be16(resp, adv_data->config_methods);
+
+                       /* Service Name */
+                       wpabuf_put_u8(resp, (u8) len);
+                       wpabuf_put_data(resp, adv_data->svc_name, len);
+
+                       /* Service State */
+                       wpabuf_put_u8(resp, adv_data->state);
+
+                       /* Service Information */
+                       wpabuf_put_le16(resp, (u16) svc_info_len);
+                       wpabuf_put_data(resp, adv_data->svc_info, svc_info_len);
+
+                       /* Update length and count */
+                       (*count_pos)++;
+                       WPA_PUT_LE16(len_pos,
+                                    (u8 *) wpabuf_put(resp, 0) - len_pos - 2);
+               }
+       }
+
+       /* Return error if no matching svc found */
+       if (count_pos == NULL) {
+               wpa_printf(MSG_DEBUG, "P2P: ASP service not found");
+               wpas_sd_add_not_found(resp, P2P_SERV_P2PS, srv_trans_id);
+       }
+}
+
+
 static void wpas_sd_request(void *ctx, int freq, const u8 *sa, u8 dialog_token,
                            u16 update_indic, const u8 *tlvs, size_t tlvs_len)
 {
@@ -2659,6 +2824,10 @@ static void wpas_sd_request(void *ctx, int freq, const u8 *sa, u8 dialog_token,
                                        pos, tlv_end - pos);
                        break;
 #endif /* CONFIG_WIFI_DISPLAY */
+               case P2P_SERV_P2PS:
+                       wpas_sd_req_asp(wpa_s, resp, srv_trans_id,
+                                       pos, tlv_end - pos);
+                       break;
                default:
                        wpa_printf(MSG_DEBUG, "P2P: Unavailable service "
                                   "protocol %u", srv_proto);