X-Git-Url: http://www.project-moonshot.org/gitweb/?p=mech_eap.git;a=blobdiff_plain;f=libeap%2Fsrc%2Fp2p%2Fp2p_sd.c;h=1a2af04b8004393b362275a791acb78c9452f2ea;hp=873356ae2015c5096fa905750b98f330d0189772;hb=4f319dde67a76fe0aaf33f6d2788968012584ada;hpb=ed09b5e64dd485851310307979d5eed14678087b diff --git a/libeap/src/p2p/p2p_sd.c b/libeap/src/p2p/p2p_sd.c index 873356a..1a2af04 100644 --- a/libeap/src/p2p/p2p_sd.c +++ b/libeap/src/p2p/p2p_sd.c @@ -2,41 +2,115 @@ * Wi-Fi Direct - P2P service discovery * Copyright (c) 2009, Atheros Communications * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 2 as - * published by the Free Software Foundation. - * - * Alternatively, this software may be distributed under the terms of BSD - * license. - * - * See README and COPYING for more details. + * This software may be distributed under the terms of the BSD license. + * See README for more details. */ #include "includes.h" #include "common.h" #include "common/ieee802_11_defs.h" +#include "common/gas.h" #include "p2p_i.h" #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; + int count = 0; - if (!(dev->dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY)) - return 0; /* peer does not support SD */ + 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) { - if (q->for_all_peers && !(dev->flags & P2P_DEV_SD_INFO)) - return q; + /* Use WSD only if the peer indicates support or it */ + if (q->wsd && !wsd) + continue; + /* if the query is a broadcast query */ + if (q->for_all_peers) { + /* + * check if there are any broadcast queries pending for + * this device + */ + if (dev->sd_pending_bcast_queries <= 0) + return NULL; + /* query number that needs to be send to the device */ + if (count == dev->sd_pending_bcast_queries - 1) + goto found; + count++; + } if (!q->for_all_peers && - os_memcmp(q->peer, dev->p2p_device_addr, ETH_ALEN) == 0) - return q; + os_memcmp(q->peer, dev->info.p2p_device_addr, ETH_ALEN) == + 0) + goto found; } return NULL; + +found: + if (dev->sd_reqs > 100) { + p2p_dbg(p2p, "Too many SD request attempts to " MACSTR + " - skip remaining queries", + MAC2STR(dev->info.p2p_device_addr)); + return NULL; + } + return q; +} + + +static void p2p_decrease_sd_bc_queries(struct p2p_data *p2p, int query_number) +{ + struct p2p_device *dev; + + p2p->num_p2p_sd_queries--; + dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) { + if (query_number <= dev->sd_pending_bcast_queries - 1) { + /* + * Query not yet sent to the device and it is to be + * removed, so update the pending count. + */ + dev->sd_pending_bcast_queries--; + } + } } @@ -44,10 +118,16 @@ static int p2p_unlink_sd_query(struct p2p_data *p2p, struct p2p_sd_query *query) { struct p2p_sd_query *q, *prev; + int query_number = 0; + q = p2p->sd_queries; prev = NULL; while (q) { if (q == query) { + /* If the query is a broadcast query, decrease one from + * all the devices */ + if (query->for_all_peers) + p2p_decrease_sd_bc_queries(p2p, query_number); if (prev) prev->next = q->next; else @@ -56,6 +136,8 @@ static int p2p_unlink_sd_query(struct p2p_data *p2p, p2p->sd_query = NULL; return 1; } + if (q->for_all_peers) + query_number++; prev = q; q = q->next; } @@ -82,6 +164,7 @@ void p2p_free_sd_queries(struct p2p_data *p2p) q = q->next; p2p_free_sd_query(prev); } + p2p->num_p2p_sd_queries = 0; } @@ -89,51 +172,20 @@ static struct wpabuf * p2p_build_sd_query(u16 update_indic, struct wpabuf *tlvs) { struct wpabuf *buf; - u8 *len_pos, *len_pos2; + u8 *len_pos; - buf = wpabuf_alloc(1000 + wpabuf_len(tlvs)); + buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs)); if (buf == NULL) return NULL; - wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC); - wpabuf_put_u8(buf, WLAN_PA_GAS_INITIAL_REQ); - wpabuf_put_u8(buf, 0); /* Dialog Token */ - - /* Advertisement Protocol IE */ - wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO); - wpabuf_put_u8(buf, 2); /* Length */ - wpabuf_put_u8(buf, 0); /* QueryRespLenLimit | PAME-BI */ - wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */ - - /* Query Request */ - len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */ - - /* NQP Query Request Frame */ - wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */ - len_pos2 = wpabuf_put(buf, 2); /* Length (to be filled) */ - wpabuf_put_be24(buf, OUI_WFA); - wpabuf_put_u8(buf, P2P_OUI_TYPE); + /* ANQP Query Request Frame */ + len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); + wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */ wpabuf_put_buf(buf, tlvs); + gas_anqp_set_element_len(buf, len_pos); - WPA_PUT_LE16(len_pos2, (u8 *) wpabuf_put(buf, 0) - len_pos2 - 2); - WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2); - - return buf; -} - - -static struct wpabuf * p2p_build_gas_comeback_req(u8 dialog_token) -{ - struct wpabuf *buf; - - buf = wpabuf_alloc(3); - if (buf == NULL) - return NULL; - - wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC); - wpabuf_put_u8(buf, WLAN_PA_GAS_COMEBACK_REQ); - wpabuf_put_u8(buf, dialog_token); + gas_anqp_set_len(buf); return buf; } @@ -144,16 +196,14 @@ static void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst, { struct wpabuf *req; - req = p2p_build_gas_comeback_req(dialog_token); + req = gas_build_comeback_req(dialog_token); if (req == NULL) return; p2p->pending_action_state = P2P_NO_PENDING_ACTION; - if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, - dst, p2p->cfg->dev_addr, dst, - wpabuf_head(req), wpabuf_len(req), 200) < 0) - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Failed to send Action frame"); + if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst, + wpabuf_head(req), wpabuf_len(req), 200) < 0) + p2p_dbg(p2p, "Failed to send Action frame"); wpabuf_free(req); } @@ -165,42 +215,25 @@ static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code, const struct wpabuf *tlvs) { struct wpabuf *buf; - u8 *len_pos, *len_pos2; + u8 *len_pos; - buf = wpabuf_alloc(1000 + (tlvs ? wpabuf_len(tlvs) : 0)); + buf = gas_anqp_build_initial_resp(dialog_token, status_code, + comeback_delay, + 100 + (tlvs ? wpabuf_len(tlvs) : 0)); if (buf == NULL) return NULL; - wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC); - wpabuf_put_u8(buf, WLAN_PA_GAS_INITIAL_RESP); - wpabuf_put_u8(buf, dialog_token); - wpabuf_put_le16(buf, status_code); - wpabuf_put_le16(buf, comeback_delay); - - /* Advertisement Protocol IE */ - wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO); - wpabuf_put_u8(buf, 2); /* Length */ - wpabuf_put_u8(buf, 0x7f); /* QueryRespLenLimit | PAME-BI */ - wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */ - - /* Query Response */ - len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */ - if (tlvs) { - /* NQP Query Response Frame */ - wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */ - len_pos2 = wpabuf_put(buf, 2); /* Length (to be filled) */ - wpabuf_put_be24(buf, OUI_WFA); - wpabuf_put_u8(buf, P2P_OUI_TYPE); + /* ANQP Query Response Frame */ + len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC); + wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); /* Service Update Indicator */ wpabuf_put_le16(buf, update_indic); wpabuf_put_buf(buf, tlvs); - - WPA_PUT_LE16(len_pos2, - (u8 *) wpabuf_put(buf, 0) - len_pos2 - 2); + gas_anqp_set_element_len(buf, len_pos); } - WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2); + gas_anqp_set_len(buf); return buf; } @@ -214,41 +247,23 @@ static struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token, u16 total_len) { struct wpabuf *buf; - u8 *len_pos; - buf = wpabuf_alloc(1000 + len); + buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id, + more, 0, 100 + len); if (buf == NULL) return NULL; - wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC); - wpabuf_put_u8(buf, WLAN_PA_GAS_COMEBACK_RESP); - wpabuf_put_u8(buf, dialog_token); - wpabuf_put_le16(buf, status_code); - wpabuf_put_u8(buf, frag_id | (more ? 0x80 : 0)); - wpabuf_put_le16(buf, 0); /* Comeback Delay */ - - /* Advertisement Protocol IE */ - wpabuf_put_u8(buf, WLAN_EID_ADV_PROTO); - wpabuf_put_u8(buf, 2); /* Length */ - wpabuf_put_u8(buf, 0x7f); /* QueryRespLenLimit | PAME-BI */ - wpabuf_put_u8(buf, NATIVE_QUERY_PROTOCOL); /* Advertisement Protocol */ - - /* Query Response */ - len_pos = wpabuf_put(buf, 2); /* Length (to be filled) */ - if (frag_id == 0) { - /* NQP Query Response Frame */ - wpabuf_put_le16(buf, NQP_VENDOR_SPECIFIC); /* Info ID */ + /* ANQP Query Response Frame */ + wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */ wpabuf_put_le16(buf, 3 + 1 + 2 + total_len); - wpabuf_put_be24(buf, OUI_WFA); - wpabuf_put_u8(buf, P2P_OUI_TYPE); + wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE); /* Service Update Indicator */ wpabuf_put_le16(buf, update_indic); } wpabuf_put_data(buf, data, len); - - WPA_PUT_LE16(len_pos, (u8 *) wpabuf_put(buf, 0) - len_pos - 2); + gas_anqp_set_len(buf); return buf; } @@ -260,13 +275,13 @@ int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev) int ret = 0; struct p2p_sd_query *query; int freq; + unsigned int wait_time; freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq; if (freq <= 0) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: No Listen/Operating frequency known for the " - "peer " MACSTR " to send SD Request", - MAC2STR(dev->p2p_device_addr)); + p2p_dbg(p2p, "No Listen/Operating frequency known for the peer " + MACSTR " to send SD Request", + MAC2STR(dev->info.p2p_device_addr)); return -1; } @@ -274,25 +289,25 @@ int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev) if (query == NULL) return -1; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Start Service Discovery with " MACSTR, - MAC2STR(dev->p2p_device_addr)); + p2p_dbg(p2p, "Start Service Discovery with " MACSTR, + MAC2STR(dev->info.p2p_device_addr)); req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs); if (req == NULL) return -1; + dev->sd_reqs++; p2p->sd_peer = dev; p2p->sd_query = query; p2p->pending_action_state = P2P_PENDING_SD; - if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, - dev->p2p_device_addr, p2p->cfg->dev_addr, - dev->p2p_device_addr, - wpabuf_head(req), wpabuf_len(req), 5000) < 0) - { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Failed to send Action frame"); + wait_time = 5000; + if (p2p->cfg->max_listen && wait_time > p2p->cfg->max_listen) + wait_time = p2p->cfg->max_listen; + if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr, + p2p->cfg->dev_addr, dev->info.p2p_device_addr, + wpabuf_head(req), wpabuf_len(req), wait_time) < 0) { + p2p_dbg(p2p, "Failed to send Action frame"); ret = -1; } @@ -320,8 +335,7 @@ void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa, if (rx_freq > 0) freq = rx_freq; else - freq = p2p_channel_to_freq(p2p->cfg->country, - p2p->cfg->reg_class, + freq = p2p_channel_to_freq(p2p->cfg->reg_class, p2p->cfg->channel); if (freq < 0) return; @@ -330,14 +344,12 @@ void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa, return; dialog_token = *pos++; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: GAS Initial Request from " MACSTR " (dialog token %u, " - "freq %d)", + p2p_dbg(p2p, "GAS Initial Request from " MACSTR + " (dialog token %u, freq %d)", MAC2STR(sa), dialog_token, rx_freq); if (*pos != WLAN_EID_ADV_PROTO) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unexpected IE in GAS Initial Request: %u", *pos); + p2p_dbg(p2p, "Unexpected IE in GAS Initial Request: %u", *pos); return; } pos++; @@ -345,15 +357,13 @@ void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa, slen = *pos++; next = pos + slen; if (next > end || slen < 2) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Invalid IE in GAS Initial Request"); + p2p_dbg(p2p, "Invalid IE in GAS Initial Request"); return; } pos++; /* skip QueryRespLenLimit and PAME-BI */ - if (*pos != NATIVE_QUERY_PROTOCOL) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported GAS advertisement protocol id %u", + if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { + p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u", *pos); return; } @@ -368,12 +378,11 @@ void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa, return; end = pos + slen; - /* NQP Query Request */ + /* ANQP Query Request */ if (pos + 4 > end) return; - if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos)); + if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { + p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); return; } pos += 2; @@ -381,30 +390,21 @@ void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa, slen = WPA_GET_LE16(pos); pos += 2; if (pos + slen > end || slen < 3 + 1) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Invalid NQP Query Request length"); + p2p_dbg(p2p, "Invalid ANQP Query Request length"); return; } - if (WPA_GET_BE24(pos) != OUI_WFA) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos)); + if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) { + p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x", + WPA_GET_BE32(pos)); return; } - pos += 3; - - if (*pos != P2P_OUI_TYPE) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported NQP vendor type %u", *pos); - return; - } - pos++; + pos += 4; if (pos + 2 > end) return; update_indic = WPA_GET_LE16(pos); - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Service Update Indicator: %u", update_indic); + p2p_dbg(p2p, "Service Update Indicator: %u", update_indic); pos += 2; p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token, @@ -420,8 +420,7 @@ void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst, /* TODO: fix the length limit to match with the maximum frame length */ if (wpabuf_len(resp_tlvs) > 1400) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response long " - "enough to require fragmentation"); + p2p_dbg(p2p, "SD response long enough to require fragmentation"); if (p2p->sd_resp) { /* * TODO: Could consider storing the fragmented response @@ -430,20 +429,22 @@ void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst, * Though, that would eat more memory, so there are * also benefits to just using a single buffer. */ - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop " - "previous SD response"); + p2p_dbg(p2p, "Drop previous SD response"); wpabuf_free(p2p->sd_resp); } + p2p->sd_resp = wpabuf_dup(resp_tlvs); + if (p2p->sd_resp == NULL) { + p2p_err(p2p, "Failed to allocate SD response fragmentation area"); + return; + } os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN); p2p->sd_resp_dialog_token = dialog_token; - p2p->sd_resp = wpabuf_dup(resp_tlvs); p2p->sd_resp_pos = 0; p2p->sd_frag_id = 0; resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS, 1, p2p->srv_update_indic, NULL); } else { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: SD response fits " - "in initial response"); + p2p_dbg(p2p, "SD response fits in initial response"); resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS, 0, p2p->srv_update_indic, resp_tlvs); @@ -452,12 +453,10 @@ void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst, return; p2p->pending_action_state = P2P_NO_PENDING_ACTION; - if (p2p->cfg->send_action(p2p->cfg->cb_ctx, freq, - dst, p2p->cfg->dev_addr, p2p->cfg->dev_addr, - wpabuf_head(resp), wpabuf_len(resp), 200) < - 0) - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Failed to send Action frame"); + if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, + p2p->cfg->dev_addr, + wpabuf_head(resp), wpabuf_len(resp), 200) < 0) + p2p_dbg(p2p, "Failed to send Action frame"); wpabuf_free(resp); } @@ -476,22 +475,19 @@ void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa, u16 update_indic; if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || - os_memcmp(sa, p2p->sd_peer->p2p_device_addr, ETH_ALEN) != 0) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Ignore unexpected GAS Initial Response from " + os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { + p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from " MACSTR, MAC2STR(sa)); return; } p2p->cfg->send_action_done(p2p->cfg->cb_ctx); p2p_clear_timeout(p2p); - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Received GAS Initial Response from " MACSTR " (len=%d)", + p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)", MAC2STR(sa), (int) len); if (len < 5 + 2) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Too short GAS Initial Response frame"); + p2p_dbg(p2p, "Too short GAS Initial Response frame"); return; } @@ -501,20 +497,16 @@ void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa, pos += 2; comeback_delay = WPA_GET_LE16(pos); pos += 2; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: dialog_token=%u status_code=%u comeback_delay=%u", + p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u", dialog_token, status_code, comeback_delay); if (status_code) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Service Discovery failed: status code %u", + p2p_dbg(p2p, "Service Discovery failed: status code %u", status_code); return; } if (*pos != WLAN_EID_ADV_PROTO) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unexpected IE in GAS Initial Response: %u", - *pos); + p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos); return; } pos++; @@ -522,15 +514,13 @@ void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa, slen = *pos++; next = pos + slen; if (next > end || slen < 2) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Invalid IE in GAS Initial Response"); + p2p_dbg(p2p, "Invalid IE in GAS Initial Response"); return; } pos++; /* skip QueryRespLenLimit and PAME-BI */ - if (*pos != NATIVE_QUERY_PROTOCOL) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported GAS advertisement protocol id %u", + if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { + p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u", *pos); return; } @@ -538,27 +528,22 @@ void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa, pos = next; /* Query Response */ if (pos + 2 > end) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query " - "Response"); + p2p_dbg(p2p, "Too short Query Response"); return; } slen = WPA_GET_LE16(pos); pos += 2; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d", - slen); + p2p_dbg(p2p, "Query Response Length: %d", slen); if (pos + slen > end) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query " - "Response data"); + p2p_dbg(p2p, "Not enough Query Response data"); return; } end = pos + slen; if (comeback_delay) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Fragmented " - "response - request fragments"); + p2p_dbg(p2p, "Fragmented response - request fragments"); if (p2p->sd_rx_resp) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Drop " - "old SD reassembly buffer"); + p2p_dbg(p2p, "Drop old SD reassembly buffer"); wpabuf_free(p2p->sd_rx_resp); p2p->sd_rx_resp = NULL; } @@ -566,12 +551,11 @@ void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa, return; } - /* NQP Query Response */ + /* ANQP Query Response */ if (pos + 4 > end) return; - if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos)); + if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { + p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); return; } pos += 2; @@ -579,41 +563,29 @@ void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa, slen = WPA_GET_LE16(pos); pos += 2; if (pos + slen > end || slen < 3 + 1) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Invalid NQP Query Response length"); - return; - } - - if (WPA_GET_BE24(pos) != OUI_WFA) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos)); + p2p_dbg(p2p, "Invalid ANQP Query Response length"); return; } - pos += 3; - if (*pos != P2P_OUI_TYPE) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported NQP vendor type %u", *pos); + if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) { + p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x", + WPA_GET_BE32(pos)); return; } - pos++; + pos += 4; if (pos + 2 > end) return; update_indic = WPA_GET_LE16(pos); - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Service Update Indicator: %u", update_indic); + p2p_dbg(p2p, "Service Update Indicator: %u", update_indic); pos += 2; - p2p->sd_peer->flags |= P2P_DEV_SD_INFO; - p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE; p2p->sd_peer = NULL; if (p2p->sd_query) { if (!p2p->sd_query->for_all_peers) { struct p2p_sd_query *q; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Remove completed SD query %p", + p2p_dbg(p2p, "Remove completed SD query %p", p2p->sd_query); q = p2p->sd_query; p2p_unlink_sd_query(p2p, p2p->sd_query); @@ -641,22 +613,20 @@ void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa, if (len < 1) return; dialog_token = *data; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Dialog Token: %u", - dialog_token); + p2p_dbg(p2p, "Dialog Token: %u", dialog_token); if (dialog_token != p2p->sd_resp_dialog_token) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD " - "response fragment for dialog token %u", dialog_token); + p2p_dbg(p2p, "No pending SD response fragment for dialog token %u", + dialog_token); return; } if (p2p->sd_resp == NULL) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD " - "response fragment available"); + p2p_dbg(p2p, "No pending SD response fragment available"); return; } if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No pending SD " - "response fragment for " MACSTR, MAC2STR(sa)); + p2p_dbg(p2p, "No pending SD response fragment for " MACSTR, + MAC2STR(sa)); return; } @@ -673,30 +643,25 @@ void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa, wpabuf_len(p2p->sd_resp)); if (resp == NULL) return; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Send GAS Comeback " - "Response (frag_id %d more=%d frag_len=%d)", + p2p_dbg(p2p, "Send GAS Comeback Response (frag_id %d more=%d frag_len=%d)", p2p->sd_frag_id, more, (int) frag_len); p2p->sd_frag_id++; p2p->sd_resp_pos += frag_len; if (more) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: %d more bytes " - "remain to be sent", + p2p_dbg(p2p, "%d more bytes remain to be sent", (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos)); } else { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: All fragments of " - "SD response sent"); + p2p_dbg(p2p, "All fragments of SD response sent"); wpabuf_free(p2p->sd_resp); p2p->sd_resp = NULL; } p2p->pending_action_state = P2P_NO_PENDING_ACTION; - if (p2p->cfg->send_action(p2p->cfg->cb_ctx, rx_freq, - sa, p2p->cfg->dev_addr, p2p->cfg->dev_addr, - wpabuf_head(resp), wpabuf_len(resp), 200) < - 0) - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Failed to send Action frame"); + if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr, + p2p->cfg->dev_addr, + wpabuf_head(resp), wpabuf_len(resp), 200) < 0) + p2p_dbg(p2p, "Failed to send Action frame"); wpabuf_free(resp); } @@ -718,22 +683,19 @@ void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa, wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len); if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL || - os_memcmp(sa, p2p->sd_peer->p2p_device_addr, ETH_ALEN) != 0) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Ignore unexpected GAS Comeback Response from " + os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) { + p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from " MACSTR, MAC2STR(sa)); return; } p2p->cfg->send_action_done(p2p->cfg->cb_ctx); p2p_clear_timeout(p2p); - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Received GAS Comeback Response from " MACSTR " (len=%d)", + p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)", MAC2STR(sa), (int) len); if (len < 6 + 2) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Too short GAS Comeback Response frame"); + p2p_dbg(p2p, "Too short GAS Comeback Response frame"); return; } @@ -746,22 +708,19 @@ void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa, pos++; comeback_delay = WPA_GET_LE16(pos); pos += 2; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: dialog_token=%u status_code=%u frag_id=%d more_frags=%d " + p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d " "comeback_delay=%u", dialog_token, status_code, frag_id, more_frags, comeback_delay); /* TODO: check frag_id match */ if (status_code) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Service Discovery failed: status code %u", + p2p_dbg(p2p, "Service Discovery failed: status code %u", status_code); return; } if (*pos != WLAN_EID_ADV_PROTO) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unexpected IE in GAS Comeback Response: %u", + p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u", *pos); return; } @@ -770,15 +729,13 @@ void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa, slen = *pos++; next = pos + slen; if (next > end || slen < 2) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Invalid IE in GAS Comeback Response"); + p2p_dbg(p2p, "Invalid IE in GAS Comeback Response"); return; } pos++; /* skip QueryRespLenLimit and PAME-BI */ - if (*pos != NATIVE_QUERY_PROTOCOL) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported GAS advertisement protocol id %u", + if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) { + p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u", *pos); return; } @@ -786,109 +743,88 @@ void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa, pos = next; /* Query Response */ if (pos + 2 > end) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too short Query " - "Response"); + p2p_dbg(p2p, "Too short Query Response"); return; } slen = WPA_GET_LE16(pos); pos += 2; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Query Response Length: %d", - slen); + p2p_dbg(p2p, "Query Response Length: %d", slen); if (pos + slen > end) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Not enough Query " - "Response data"); + p2p_dbg(p2p, "Not enough Query Response data"); return; } if (slen == 0) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: No Query Response " - "data"); + p2p_dbg(p2p, "No Query Response data"); return; } end = pos + slen; if (p2p->sd_rx_resp) { /* - * NQP header is only included in the first fragment; rest of + * ANQP header is only included in the first fragment; rest of * the fragments start with continue TLVs. */ goto skip_nqp_header; } - /* NQP Query Response */ + /* ANQP Query Response */ if (pos + 4 > end) return; - if (WPA_GET_LE16(pos) != NQP_VENDOR_SPECIFIC) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported NQP Info ID %u", WPA_GET_LE16(pos)); + if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) { + p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos)); return; } pos += 2; slen = WPA_GET_LE16(pos); pos += 2; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: NQP Query Response " - "length: %u", slen); + p2p_dbg(p2p, "ANQP Query Response length: %u", slen); if (slen < 3 + 1) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Invalid NQP Query Response length"); + p2p_dbg(p2p, "Invalid ANQP Query Response length"); return; } if (pos + 4 > end) return; - if (WPA_GET_BE24(pos) != OUI_WFA) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported NQP OUI %06x", WPA_GET_BE24(pos)); + if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) { + p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x", + WPA_GET_BE32(pos)); return; } - pos += 3; - - if (*pos != P2P_OUI_TYPE) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Unsupported NQP vendor type %u", *pos); - return; - } - pos++; + pos += 4; if (pos + 2 > end) return; p2p->sd_rx_update_indic = WPA_GET_LE16(pos); - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Service Update Indicator: %u", p2p->sd_rx_update_indic); + p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic); pos += 2; skip_nqp_header: if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0) return; wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos); - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Current SD reassembly " - "buffer length: %u", + p2p_dbg(p2p, "Current SD reassembly buffer length: %u", (unsigned int) wpabuf_len(p2p->sd_rx_resp)); if (more_frags) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: More fragments " - "remains"); + p2p_dbg(p2p, "More fragments remains"); /* TODO: what would be a good size limit? */ if (wpabuf_len(p2p->sd_rx_resp) > 64000) { wpabuf_free(p2p->sd_rx_resp); p2p->sd_rx_resp = NULL; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Too long " - "SD response - drop it"); + p2p_dbg(p2p, "Too long SD response - drop it"); return; } p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq); return; } - p2p->sd_peer->flags |= P2P_DEV_SD_INFO; - p2p->sd_peer->flags &= ~P2P_DEV_SD_SCHEDULE; p2p->sd_peer = NULL; if (p2p->sd_query) { if (!p2p->sd_query->for_all_peers) { struct p2p_sd_query *q; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Remove completed SD query %p", + p2p_dbg(p2p, "Remove completed SD query %p", p2p->sd_query); q = p2p->sd_query; p2p_unlink_sd_query(p2p, p2p->sd_query); @@ -931,10 +867,37 @@ void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst, q->next = p2p->sd_queries; p2p->sd_queries = q; - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p", q); + p2p_dbg(p2p, "Added SD Query %p", q); + + if (dst == NULL) { + struct p2p_device *dev; + p2p->num_p2p_sd_queries++; + + /* Update all the devices for the newly added broadcast query */ + dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) { + if (dev->sd_pending_bcast_queries <= 0) + dev->sd_pending_bcast_queries = 1; + else + dev->sd_pending_bcast_queries++; + } + } + + return q; +} + + +#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) @@ -946,8 +909,7 @@ void p2p_sd_service_update(struct p2p_data *p2p) int p2p_sd_cancel_request(struct p2p_data *p2p, void *req) { if (p2p_unlink_sd_query(p2p, req)) { - wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, - "P2P: Cancel pending SD query %p", req); + p2p_dbg(p2p, "Cancel pending SD query %p", req); p2p_free_sd_query(req); return 0; }