diff --git a/wpa_supplicant/Makefile b/wpa_supplicant/Makefile index f28632b00..fccc4d18f 100644 --- a/wpa_supplicant/Makefile +++ b/wpa_supplicant/Makefile @@ -1285,6 +1285,8 @@ endif ifdef NEED_GAS OBJS += ../src/common/gas.o +OBJS += gas_query.o +CFLAGS += -DCONFIG_GAS endif OBJS_wpa_rm := ctrl_iface.o mlme.o ctrl_iface_unix.o diff --git a/wpa_supplicant/events.c b/wpa_supplicant/events.c index e60a25e1f..736ef66d5 100644 --- a/wpa_supplicant/events.c +++ b/wpa_supplicant/events.c @@ -38,6 +38,7 @@ #include "wps_supplicant.h" #include "ibss_rsn.h" #include "sme.h" +#include "gas_query.h" #include "p2p_supplicant.h" #include "bgscan.h" #include "ap.h" @@ -2026,6 +2027,14 @@ void wpa_supplicant_event(void *ctx, enum wpa_event_type event, } #endif /* CONFIG_SME */ #endif /* CONFIG_IEEE80211W */ +#ifdef CONFIG_GAS + if (data->rx_action.category == WLAN_ACTION_PUBLIC && + gas_query_rx(wpa_s->gas, data->rx_action.da, + data->rx_action.sa, data->rx_action.bssid, + data->rx_action.data, data->rx_action.len, + data->rx_action.freq) == 0) + break; +#endif /* CONFIG_GAS */ #ifdef CONFIG_P2P wpas_p2p_rx_action(wpa_s, data->rx_action.da, data->rx_action.sa, diff --git a/wpa_supplicant/gas_query.c b/wpa_supplicant/gas_query.c new file mode 100644 index 000000000..befe9fff5 --- /dev/null +++ b/wpa_supplicant/gas_query.c @@ -0,0 +1,472 @@ +/* + * Generic advertisement service (GAS) query + * Copyright (c) 2009, Atheros Communications + * Copyright (c) 2011, Qualcomm Atheros + * + * 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. + */ + +#include "includes.h" + +#include "common.h" +#include "utils/eloop.h" +#include "common/ieee802_11_defs.h" +#include "common/gas.h" +#include "wpa_supplicant_i.h" +#include "driver_i.h" +#include "gas_query.h" + + +#define GAS_QUERY_TIMEOUT 5 + + +struct gas_query_pending { + struct dl_list list; + u8 addr[ETH_ALEN]; + u8 dialog_token; + u8 next_frag_id; + int wait_comeback; + int freq; + u16 status_code; + struct wpabuf *adv_proto; + struct wpabuf *resp; + void (*cb)(void *ctx, const u8 *dst, u8 dialog_token, + enum gas_query_result result, + const struct wpabuf *adv_proto, + const struct wpabuf *resp, u16 status_code); + void *ctx; +}; + +struct gas_query { + struct wpa_supplicant *wpa_s; + struct dl_list pending; /* struct gas_query_pending */ +}; + + +static void gas_query_tx_comeback_timeout(void *eloop_data, void *user_ctx); +static void gas_query_timeout(void *eloop_data, void *user_ctx); + + +struct gas_query * gas_query_init(struct wpa_supplicant *wpa_s) +{ + struct gas_query *gas; + + gas = os_zalloc(sizeof(*gas)); + if (gas == NULL) + return NULL; + + gas->wpa_s = wpa_s; + dl_list_init(&gas->pending); + + return gas; +} + + +static void gas_query_done(struct gas_query *gas, + struct gas_query_pending *query, + enum gas_query_result result) +{ + eloop_cancel_timeout(gas_query_tx_comeback_timeout, gas, query); + eloop_cancel_timeout(gas_query_timeout, gas, query); + dl_list_del(&query->list); + query->cb(query->ctx, query->addr, query->dialog_token, result, + query->adv_proto, query->resp, query->status_code); + wpabuf_free(query->adv_proto); + wpabuf_free(query->resp); + os_free(query); +} + + +void gas_query_deinit(struct gas_query *gas) +{ + struct gas_query_pending *query, *next; + + if (gas == NULL) + return; + + dl_list_for_each_safe(query, next, &gas->pending, + struct gas_query_pending, list) + gas_query_done(gas, query, GAS_QUERY_DELETED_AT_DEINIT); + + os_free(gas); +} + + +static struct gas_query_pending * +gas_query_get_pending(struct gas_query *gas, const u8 *addr, u8 dialog_token) +{ + struct gas_query_pending *q; + dl_list_for_each(q, &gas->pending, struct gas_query_pending, list) { + if (os_memcmp(q->addr, addr, ETH_ALEN) == 0 && + q->dialog_token == dialog_token) + return q; + } + return NULL; +} + + +static int gas_query_append(struct gas_query_pending *query, const u8 *data, + size_t len) +{ + if (wpabuf_resize(&query->resp, len) < 0) { + wpa_printf(MSG_DEBUG, "GAS: No memory to store the response"); + return -1; + } + wpabuf_put_data(query->resp, data, len); + return 0; +} + + +static int gas_query_tx(struct gas_query *gas, struct gas_query_pending *query, + struct wpabuf *req) +{ + wpa_printf(MSG_DEBUG, "GAS: Send action frame to " MACSTR " len=%u " + "freq=%d", MAC2STR(query->addr), + (unsigned int) wpabuf_len(req), query->freq); + return wpa_drv_send_action(gas->wpa_s, query->freq, 0, query->addr, + gas->wpa_s->own_addr, query->addr, + wpabuf_head(req), wpabuf_len(req)); +} + + +static void gas_query_tx_comeback_req(struct gas_query *gas, + struct gas_query_pending *query) +{ + struct wpabuf *req; + + req = gas_build_comeback_req(query->dialog_token); + if (req == NULL) { + gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR); + return; + } + + if (gas_query_tx(gas, query, req) < 0) { + wpa_printf(MSG_DEBUG, "GAS: Failed to send Action frame to " + MACSTR, MAC2STR(query->addr)); + gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR); + } + + wpabuf_free(req); +} + + +static void gas_query_tx_comeback_timeout(void *eloop_data, void *user_ctx) +{ + struct gas_query *gas = eloop_data; + struct gas_query_pending *query = user_ctx; + + wpa_printf(MSG_DEBUG, "GAS: Comeback timeout for request to " MACSTR, + MAC2STR(query->addr)); + gas_query_tx_comeback_req(gas, query); +} + + +static void gas_query_tx_comeback_req_delay(struct gas_query *gas, + struct gas_query_pending *query, + u16 comeback_delay) +{ + unsigned int secs, usecs; + + secs = (comeback_delay * 1024) / 1000000; + usecs = comeback_delay * 1024 - secs * 1000000; + wpa_printf(MSG_DEBUG, "GAS: Send comeback request to " MACSTR + " in %u secs %u usecs", MAC2STR(query->addr), secs, usecs); + eloop_cancel_timeout(gas_query_tx_comeback_timeout, gas, query); + eloop_register_timeout(secs, usecs, gas_query_tx_comeback_timeout, + gas, query); +} + + +static void gas_query_rx_initial(struct gas_query *gas, + struct gas_query_pending *query, + const u8 *adv_proto, const u8 *resp, + size_t len, u16 comeback_delay) +{ + wpa_printf(MSG_DEBUG, "GAS: Received initial response from " + MACSTR " (dialog_token=%u comeback_delay=%u)", + MAC2STR(query->addr), query->dialog_token, comeback_delay); + + query->adv_proto = wpabuf_alloc_copy(adv_proto, 2 + adv_proto[1]); + if (query->adv_proto == NULL) { + gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR); + return; + } + + if (comeback_delay) { + query->wait_comeback = 1; + gas_query_tx_comeback_req_delay(gas, query, comeback_delay); + return; + } + + /* Query was completed without comeback mechanism */ + if (gas_query_append(query, resp, len) < 0) { + gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR); + return; + } + + gas_query_done(gas, query, GAS_QUERY_SUCCESS); +} + + +static void gas_query_rx_comeback(struct gas_query *gas, + struct gas_query_pending *query, + const u8 *adv_proto, const u8 *resp, + size_t len, u8 frag_id, u8 more_frags, + u16 comeback_delay) +{ + wpa_printf(MSG_DEBUG, "GAS: Received comeback response from " + MACSTR " (dialog_token=%u frag_id=%u more_frags=%u " + "comeback_delay=%u)", + MAC2STR(query->addr), query->dialog_token, frag_id, + more_frags, comeback_delay); + + if ((size_t) 2 + adv_proto[1] != wpabuf_len(query->adv_proto) || + os_memcmp(adv_proto, wpabuf_head(query->adv_proto), + wpabuf_len(query->adv_proto)) != 0) { + wpa_printf(MSG_DEBUG, "GAS: Advertisement Protocol changed " + "between initial and comeback response from " + MACSTR, MAC2STR(query->addr)); + gas_query_done(gas, query, GAS_QUERY_PEER_ERROR); + return; + } + + if (comeback_delay) { + if (frag_id) { + wpa_printf(MSG_DEBUG, "GAS: Invalid comeback response " + "with non-zero frag_id and comeback_delay " + "from " MACSTR, MAC2STR(query->addr)); + gas_query_done(gas, query, GAS_QUERY_PEER_ERROR); + return; + } + gas_query_tx_comeback_req_delay(gas, query, comeback_delay); + return; + } + + if (frag_id != query->next_frag_id) { + wpa_printf(MSG_DEBUG, "GAS: Unexpected frag_id in response " + "from " MACSTR, MAC2STR(query->addr)); + gas_query_done(gas, query, GAS_QUERY_PEER_ERROR); + return; + } + query->next_frag_id++; + + if (gas_query_append(query, resp, len) < 0) { + gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR); + return; + } + + if (more_frags) { + gas_query_tx_comeback_req(gas, query); + return; + } + + gas_query_done(gas, query, GAS_QUERY_SUCCESS); +} + + +int gas_query_rx(struct gas_query *gas, const u8 *da, const u8 *sa, + const u8 *bssid, const u8 *data, size_t len, int freq) +{ + struct gas_query_pending *query; + u8 action, dialog_token, frag_id = 0, more_frags = 0; + u16 comeback_delay, resp_len; + const u8 *pos, *adv_proto; + + if (gas == NULL || len < 4) + return -1; + + pos = data; + action = *pos++; + dialog_token = *pos++; + + if (action != WLAN_PA_GAS_INITIAL_RESP && + action != WLAN_PA_GAS_COMEBACK_RESP) + return -1; /* Not a GAS response */ + + query = gas_query_get_pending(gas, sa, dialog_token); + if (query == NULL) { + wpa_printf(MSG_DEBUG, "GAS: No pending query found for " MACSTR + " dialog token %u", MAC2STR(sa), dialog_token); + return -1; + } + + if (query->wait_comeback && action == WLAN_PA_GAS_INITIAL_RESP) { + wpa_printf(MSG_DEBUG, "GAS: Unexpected initial response from " + MACSTR " dialog token %u when waiting for comeback " + "response", MAC2STR(sa), dialog_token); + return 0; + } + + if (!query->wait_comeback && action == WLAN_PA_GAS_COMEBACK_RESP) { + wpa_printf(MSG_DEBUG, "GAS: Unexpected comeback response from " + MACSTR " dialog token %u when waiting for initial " + "response", MAC2STR(sa), dialog_token); + return 0; + } + + query->status_code = WPA_GET_LE16(pos); + pos += 2; + + if (query->status_code != WLAN_STATUS_SUCCESS) { + wpa_printf(MSG_DEBUG, "GAS: Query to " MACSTR " dialog token " + "%u failed - status code %u", + MAC2STR(sa), dialog_token, query->status_code); + gas_query_done(gas, query, GAS_QUERY_FAILURE); + return 0; + } + + if (action == WLAN_PA_GAS_COMEBACK_RESP) { + if (pos + 1 > data + len) + return 0; + frag_id = *pos & 0x7f; + more_frags = (*pos & 0x80) >> 7; + pos++; + } + + /* Comeback Delay */ + if (pos + 2 > data + len) + return 0; + comeback_delay = WPA_GET_LE16(pos); + pos += 2; + + /* Advertisement Protocol element */ + if (pos + 2 > data + len || pos + 2 + pos[1] > data + len) { + wpa_printf(MSG_DEBUG, "GAS: No room for Advertisement " + "Protocol element in the response from " MACSTR, + MAC2STR(sa)); + return 0; + } + + if (*pos != WLAN_EID_ADV_PROTO) { + wpa_printf(MSG_DEBUG, "GAS: Unexpected Advertisement " + "Protocol element ID %u in response from " MACSTR, + *pos, MAC2STR(sa)); + return 0; + } + + adv_proto = pos; + pos += 2 + pos[1]; + + /* Query Response Length */ + if (pos + 2 > data + len) { + wpa_printf(MSG_DEBUG, "GAS: No room for GAS Response Length"); + return 0; + } + resp_len = WPA_GET_LE16(pos); + pos += 2; + + if (pos + resp_len > data + len) { + wpa_printf(MSG_DEBUG, "GAS: Truncated Query Response in " + "response from " MACSTR, MAC2STR(sa)); + return 0; + } + + if (pos + resp_len < data + len) { + wpa_printf(MSG_DEBUG, "GAS: Ignore %u octets of extra data " + "after Query Response from " MACSTR, + (unsigned int) (data + len - pos - resp_len), + MAC2STR(sa)); + } + + if (action == WLAN_PA_GAS_COMEBACK_RESP) + gas_query_rx_comeback(gas, query, adv_proto, pos, resp_len, + frag_id, more_frags, comeback_delay); + else + gas_query_rx_initial(gas, query, adv_proto, pos, resp_len, + comeback_delay); + + return 0; +} + + +static void gas_query_timeout(void *eloop_data, void *user_ctx) +{ + struct gas_query *gas = eloop_data; + struct gas_query_pending *query = user_ctx; + + wpa_printf(MSG_DEBUG, "GAS: No response received for query to " MACSTR, + MAC2STR(query->addr)); + gas_query_done(gas, query, GAS_QUERY_TIMEOUT); +} + + +static int gas_query_dialog_token_available(struct gas_query *gas, + const u8 *dst, u8 dialog_token) +{ + struct gas_query_pending *q; + dl_list_for_each(q, &gas->pending, struct gas_query_pending, list) { + if (os_memcmp(dst, q->addr, ETH_ALEN) == 0 && + dialog_token == q->dialog_token) + return 0; + } + + return 1; +} + + +int gas_query_req(struct gas_query *gas, const u8 *dst, int freq, + struct wpabuf *req, + void (*cb)(void *ctx, const u8 *dst, u8 dialog_token, + enum gas_query_result result, + const struct wpabuf *adv_proto, + const struct wpabuf *resp, u16 status_code), + void *ctx) +{ + struct gas_query_pending *query; + int dialog_token; + + if (wpabuf_len(req) < 3) + return -1; + + for (dialog_token = 0; dialog_token < 256; dialog_token++) { + if (gas_query_dialog_token_available(gas, dst, dialog_token)) + break; + } + if (dialog_token == 256) + return -1; /* Too many pending queries */ + + query = os_zalloc(sizeof(*query)); + if (query == NULL) + return -1; + + os_memcpy(query->addr, dst, ETH_ALEN); + query->dialog_token = dialog_token; + query->freq = freq; + query->cb = cb; + query->ctx = ctx; + dl_list_add(&gas->pending, &query->list); + + *(wpabuf_mhead_u8(req) + 2) = dialog_token; + + wpa_printf(MSG_DEBUG, "GAS: Starting request for " MACSTR + " dialog_token %u", MAC2STR(dst), dialog_token); + if (gas_query_tx(gas, query, req) < 0) { + wpa_printf(MSG_DEBUG, "GAS: Failed to send Action frame to " + MACSTR, MAC2STR(query->addr)); + os_free(query); + return -1; + } + + eloop_register_timeout(GAS_QUERY_TIMEOUT, 0, gas_query_timeout, + gas, query); + + return dialog_token; +} + + +void gas_query_cancel(struct gas_query *gas, const u8 *dst, u8 dialog_token) +{ + struct gas_query_pending *query; + + query = gas_query_get_pending(gas, dst, dialog_token); + if (query) + gas_query_done(gas, query, GAS_QUERY_CANCELLED); + +} diff --git a/wpa_supplicant/gas_query.h b/wpa_supplicant/gas_query.h new file mode 100644 index 000000000..64c382546 --- /dev/null +++ b/wpa_supplicant/gas_query.h @@ -0,0 +1,61 @@ +/* + * Generic advertisement service (GAS) query + * Copyright (c) 2009, Atheros Communications + * Copyright (c) 2011, Qualcomm Atheros + * + * 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. + */ + +#ifndef GAS_QUERY_H +#define GAS_QUERY_H + +struct gas_query; + +#ifdef CONFIG_GAS + +struct gas_query * gas_query_init(struct wpa_supplicant *wpa_s); +void gas_query_deinit(struct gas_query *gas); +int gas_query_rx(struct gas_query *gas, const u8 *da, const u8 *sa, + const u8 *bssid, const u8 *data, size_t len, int freq); + +enum gas_query_result { + GAS_QUERY_SUCCESS, + GAS_QUERY_FAILURE, + GAS_QUERY_TIMEOUT, + GAS_QUERY_PEER_ERROR, + GAS_QUERY_INTERNAL_ERROR, + GAS_QUERY_CANCELLED, + GAS_QUERY_DELETED_AT_DEINIT +}; + +int gas_query_req(struct gas_query *gas, const u8 *dst, int freq, + struct wpabuf *req, + void (*cb)(void *ctx, const u8 *dst, u8 dialog_token, + enum gas_query_result result, + const struct wpabuf *adv_proto, + const struct wpabuf *resp, u16 status_code), + void *ctx); +void gas_query_cancel(struct gas_query *gas, const u8 *dst, u8 dialog_token); + +#else /* CONFIG_GAS */ + +static inline struct gas_query * gas_query_init(struct wpa_supplicant *wpa_s) +{ + return (void *) 1; +} + +static inline void gas_query_deinit(struct gas_query *gas) +{ +} + +#endif /* CONFIG_GAS */ + + +#endif /* GAS_QUERY_H */ diff --git a/wpa_supplicant/wpa_supplicant.c b/wpa_supplicant/wpa_supplicant.c index 7389bc3af..7de5b5007 100644 --- a/wpa_supplicant/wpa_supplicant.c +++ b/wpa_supplicant/wpa_supplicant.c @@ -43,6 +43,7 @@ #include "wps_supplicant.h" #include "ibss_rsn.h" #include "sme.h" +#include "gas_query.h" #include "ap.h" #include "p2p_supplicant.h" #include "notify.h" @@ -441,6 +442,9 @@ static void wpa_supplicant_cleanup(struct wpa_supplicant *wpa_s) os_free(wpa_s->next_scan_freqs); wpa_s->next_scan_freqs = NULL; + + gas_query_deinit(wpa_s->gas); + wpa_s->gas = NULL; } @@ -2285,6 +2289,12 @@ next_driver: return -1; } + wpa_s->gas = gas_query_init(wpa_s); + if (wpa_s->gas == NULL) { + wpa_printf(MSG_ERROR, "Failed to initialize GAS query"); + return -1; + } + #ifdef CONFIG_P2P if (wpas_p2p_init(wpa_s->global, wpa_s) < 0) { wpa_msg(wpa_s, MSG_ERROR, "Failed to init P2P"); diff --git a/wpa_supplicant/wpa_supplicant_i.h b/wpa_supplicant/wpa_supplicant_i.h index f0e218cb2..ef1ba11d2 100644 --- a/wpa_supplicant/wpa_supplicant_i.h +++ b/wpa_supplicant/wpa_supplicant_i.h @@ -563,6 +563,8 @@ struct wpa_supplicant { int best_24_freq; int best_5_freq; int best_overall_freq; + + struct gas_query *gas; };