wpa_supplicant/gas_query.c

   1 /*
   2  * Generic advertisement service (GAS) query
   3  * Copyright (c) 2009, Atheros Communications
   4  * Copyright (c) 2011, Qualcomm Atheros
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   9  *
  10  * Alternatively, this software may be distributed under the terms of BSD
  11  * license.
  12  *
  13  * See README and COPYING for more details.
  14  */
  15
  16 #include "includes.h"
  17
  18 #include "common.h"
  19 #include "utils/eloop.h"
  20 #include "common/ieee802_11_defs.h"
  21 #include "common/gas.h"
  22 #include "wpa_supplicant_i.h"
  23 #include "driver_i.h"
  24 #include "gas_query.h"
  25
  26
  27 #define GAS_QUERY_TIMEOUT 5
  28
  29
  30 struct gas_query_pending {
  31         struct dl_list list;
  32         u8 addr[ETH_ALEN];
  33         u8 dialog_token;
  34         u8 next_frag_id;
  35         int wait_comeback;
  36         int freq;
  37         u16 status_code;
  38         struct wpabuf *adv_proto;
  39         struct wpabuf *resp;
  40         void (*cb)(void *ctx, const u8 *dst, u8 dialog_token,
  41                    enum gas_query_result result,
  42                    const struct wpabuf *adv_proto,
  43                    const struct wpabuf *resp, u16 status_code);
  44         void *ctx;
  45 };
  46
  47 struct gas_query {
  48         struct wpa_supplicant *wpa_s;
  49         struct dl_list pending; /* struct gas_query_pending */
  50 };
  51
  52
  53 static void gas_query_tx_comeback_timeout(void *eloop_data, void *user_ctx);
  54 static void gas_query_timeout(void *eloop_data, void *user_ctx);
  55
  56
  57 struct gas_query * gas_query_init(struct wpa_supplicant *wpa_s)
  58 {
  59         struct gas_query *gas;
  60
  61         gas = os_zalloc(sizeof(*gas));
  62         if (gas == NULL)
  63                 return NULL;
  64
  65         gas->wpa_s = wpa_s;
  66         dl_list_init(&gas->pending);
  67
  68         return gas;
  69 }
  70
  71
  72 static void gas_query_done(struct gas_query *gas,
  73                            struct gas_query_pending *query,
  74                            enum gas_query_result result)
  75 {
  76         eloop_cancel_timeout(gas_query_tx_comeback_timeout, gas, query);
  77         eloop_cancel_timeout(gas_query_timeout, gas, query);
  78         dl_list_del(&query->list);
  79         query->cb(query->ctx, query->addr, query->dialog_token, result,
  80                   query->adv_proto, query->resp, query->status_code);
  81         wpabuf_free(query->adv_proto);
  82         wpabuf_free(query->resp);
  83         os_free(query);
  84 }
  85
  86
  87 void gas_query_deinit(struct gas_query *gas)
  88 {
  89         struct gas_query_pending *query, *next;
  90
  91         if (gas == NULL)
  92                 return;
  93
  94         dl_list_for_each_safe(query, next, &gas->pending,
  95                               struct gas_query_pending, list)
  96                 gas_query_done(gas, query, GAS_QUERY_DELETED_AT_DEINIT);
  97
  98         os_free(gas);
  99 }
 100
 101
 102 static struct gas_query_pending *
 103 gas_query_get_pending(struct gas_query *gas, const u8 *addr, u8 dialog_token)
 104 {
 105         struct gas_query_pending *q;
 106         dl_list_for_each(q, &gas->pending, struct gas_query_pending, list) {
 107                 if (os_memcmp(q->addr, addr, ETH_ALEN) == 0 &&
 108                     q->dialog_token == dialog_token)
 109                         return q;
 110         }
 111         return NULL;
 112 }
 113
 114
 115 static int gas_query_append(struct gas_query_pending *query, const u8 *data,
 116                             size_t len)
 117 {
 118         if (wpabuf_resize(&query->resp, len) < 0) {
 119                 wpa_printf(MSG_DEBUG, "GAS: No memory to store the response");
 120                 return -1;
 121         }
 122         wpabuf_put_data(query->resp, data, len);
 123         return 0;
 124 }
 125
 126
 127 static int gas_query_tx(struct gas_query *gas, struct gas_query_pending *query,
 128                         struct wpabuf *req)
 129 {
 130         wpa_printf(MSG_DEBUG, "GAS: Send action frame to " MACSTR " len=%u "
 131                    "freq=%d", MAC2STR(query->addr),
 132                    (unsigned int) wpabuf_len(req), query->freq);
 133         return wpa_drv_send_action(gas->wpa_s, query->freq, 0, query->addr,
 134                                    gas->wpa_s->own_addr, query->addr,
 135                                    wpabuf_head(req), wpabuf_len(req));
 136 }
 137
 138
 139 static void gas_query_tx_comeback_req(struct gas_query *gas,
 140                                       struct gas_query_pending *query)
 141 {
 142         struct wpabuf *req;
 143
 144         req = gas_build_comeback_req(query->dialog_token);
 145         if (req == NULL) {
 146                 gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR);
 147                 return;
 148         }
 149
 150         if (gas_query_tx(gas, query, req) < 0) {
 151                 wpa_printf(MSG_DEBUG, "GAS: Failed to send Action frame to "
 152                            MACSTR, MAC2STR(query->addr));
 153                 gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR);
 154         }
 155
 156         wpabuf_free(req);
 157 }
 158
 159
 160 static void gas_query_tx_comeback_timeout(void *eloop_data, void *user_ctx)
 161 {
 162         struct gas_query *gas = eloop_data;
 163         struct gas_query_pending *query = user_ctx;
 164
 165         wpa_printf(MSG_DEBUG, "GAS: Comeback timeout for request to " MACSTR,
 166                    MAC2STR(query->addr));
 167         gas_query_tx_comeback_req(gas, query);
 168 }
 169
 170
 171 static void gas_query_tx_comeback_req_delay(struct gas_query *gas,
 172                                             struct gas_query_pending *query,
 173                                             u16 comeback_delay)
 174 {
 175         unsigned int secs, usecs;
 176
 177         secs = (comeback_delay * 1024) / 1000000;
 178         usecs = comeback_delay * 1024 - secs * 1000000;
 179         wpa_printf(MSG_DEBUG, "GAS: Send comeback request to " MACSTR
 180                    " in %u secs %u usecs", MAC2STR(query->addr), secs, usecs);
 181         eloop_cancel_timeout(gas_query_tx_comeback_timeout, gas, query);
 182         eloop_register_timeout(secs, usecs, gas_query_tx_comeback_timeout,
 183                                gas, query);
 184 }
 185
 186
 187 static void gas_query_rx_initial(struct gas_query *gas,
 188                                  struct gas_query_pending *query,
 189                                  const u8 *adv_proto, const u8 *resp,
 190                                  size_t len, u16 comeback_delay)
 191 {
 192         wpa_printf(MSG_DEBUG, "GAS: Received initial response from "
 193                    MACSTR " (dialog_token=%u comeback_delay=%u)",
 194                    MAC2STR(query->addr), query->dialog_token, comeback_delay);
 195
 196         query->adv_proto = wpabuf_alloc_copy(adv_proto, 2 + adv_proto[1]);
 197         if (query->adv_proto == NULL) {
 198                 gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR);
 199                 return;
 200         }
 201
 202         if (comeback_delay) {
 203                 query->wait_comeback = 1;
 204                 gas_query_tx_comeback_req_delay(gas, query, comeback_delay);
 205                 return;
 206         }
 207
 208         /* Query was completed without comeback mechanism */
 209         if (gas_query_append(query, resp, len) < 0) {
 210                 gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR);
 211                 return;
 212         }
 213
 214         gas_query_done(gas, query, GAS_QUERY_SUCCESS);
 215 }
 216
 217
 218 static void gas_query_rx_comeback(struct gas_query *gas,
 219                                   struct gas_query_pending *query,
 220                                   const u8 *adv_proto, const u8 *resp,
 221                                   size_t len, u8 frag_id, u8 more_frags,
 222                                   u16 comeback_delay)
 223 {
 224         wpa_printf(MSG_DEBUG, "GAS: Received comeback response from "
 225                    MACSTR " (dialog_token=%u frag_id=%u more_frags=%u "
 226                    "comeback_delay=%u)",
 227                    MAC2STR(query->addr), query->dialog_token, frag_id,
 228                    more_frags, comeback_delay);
 229
 230         if ((size_t) 2 + adv_proto[1] != wpabuf_len(query->adv_proto) ||
 231             os_memcmp(adv_proto, wpabuf_head(query->adv_proto),
 232                       wpabuf_len(query->adv_proto)) != 0) {
 233                 wpa_printf(MSG_DEBUG, "GAS: Advertisement Protocol changed "
 234                            "between initial and comeback response from "
 235                            MACSTR, MAC2STR(query->addr));
 236                 gas_query_done(gas, query, GAS_QUERY_PEER_ERROR);
 237                 return;
 238         }
 239
 240         if (comeback_delay) {
 241                 if (frag_id) {
 242                         wpa_printf(MSG_DEBUG, "GAS: Invalid comeback response "
 243                                    "with non-zero frag_id and comeback_delay "
 244                                    "from " MACSTR, MAC2STR(query->addr));
 245                         gas_query_done(gas, query, GAS_QUERY_PEER_ERROR);
 246                         return;
 247                 }
 248                 gas_query_tx_comeback_req_delay(gas, query, comeback_delay);
 249                 return;
 250         }
 251
 252         if (frag_id != query->next_frag_id) {
 253                 wpa_printf(MSG_DEBUG, "GAS: Unexpected frag_id in response "
 254                            "from " MACSTR, MAC2STR(query->addr));
 255                 gas_query_done(gas, query, GAS_QUERY_PEER_ERROR);
 256                 return;
 257         }
 258         query->next_frag_id++;
 259
 260         if (gas_query_append(query, resp, len) < 0) {
 261                 gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR);
 262                 return;
 263         }
 264
 265         if (more_frags) {
 266                 gas_query_tx_comeback_req(gas, query);
 267                 return;
 268         }
 269
 270         gas_query_done(gas, query, GAS_QUERY_SUCCESS);
 271 }
 272
 273
 274 int gas_query_rx(struct gas_query *gas, const u8 *da, const u8 *sa,
 275                  const u8 *bssid, const u8 *data, size_t len, int freq)
 276 {
 277         struct gas_query_pending *query;
 278         u8 action, dialog_token, frag_id = 0, more_frags = 0;
 279         u16 comeback_delay, resp_len;
 280         const u8 *pos, *adv_proto;
 281
 282         if (gas == NULL || len < 4)
 283                 return -1;
 284
 285         pos = data;
 286         action = *pos++;
 287         dialog_token = *pos++;
 288
 289         if (action != WLAN_PA_GAS_INITIAL_RESP &&
 290             action != WLAN_PA_GAS_COMEBACK_RESP)
 291                 return -1; /* Not a GAS response */
 292
 293         query = gas_query_get_pending(gas, sa, dialog_token);
 294         if (query == NULL) {
 295                 wpa_printf(MSG_DEBUG, "GAS: No pending query found for " MACSTR
 296                            " dialog token %u", MAC2STR(sa), dialog_token);
 297                 return -1;
 298         }
 299
 300         if (query->wait_comeback && action == WLAN_PA_GAS_INITIAL_RESP) {
 301                 wpa_printf(MSG_DEBUG, "GAS: Unexpected initial response from "
 302                            MACSTR " dialog token %u when waiting for comeback "
 303                            "response", MAC2STR(sa), dialog_token);
 304                 return 0;
 305         }
 306
 307         if (!query->wait_comeback && action == WLAN_PA_GAS_COMEBACK_RESP) {
 308                 wpa_printf(MSG_DEBUG, "GAS: Unexpected comeback response from "
 309                            MACSTR " dialog token %u when waiting for initial "
 310                            "response", MAC2STR(sa), dialog_token);
 311                 return 0;
 312         }
 313
 314         query->status_code = WPA_GET_LE16(pos);
 315         pos += 2;
 316
 317         if (query->status_code != WLAN_STATUS_SUCCESS) {
 318                 wpa_printf(MSG_DEBUG, "GAS: Query to " MACSTR " dialog token "
 319                            "%u failed - status code %u",
 320                            MAC2STR(sa), dialog_token, query->status_code);
 321                 gas_query_done(gas, query, GAS_QUERY_FAILURE);
 322                 return 0;
 323         }
 324
 325         if (action == WLAN_PA_GAS_COMEBACK_RESP) {
 326                 if (pos + 1 > data + len)
 327                         return 0;
 328                 frag_id = *pos & 0x7f;
 329                 more_frags = (*pos & 0x80) >> 7;
 330                 pos++;
 331         }
 332
 333         /* Comeback Delay */
 334         if (pos + 2 > data + len)
 335                 return 0;
 336         comeback_delay = WPA_GET_LE16(pos);
 337         pos += 2;
 338
 339         /* Advertisement Protocol element */
 340         if (pos + 2 > data + len || pos + 2 + pos[1] > data + len) {
 341                 wpa_printf(MSG_DEBUG, "GAS: No room for Advertisement "
 342                            "Protocol element in the response from " MACSTR,
 343                            MAC2STR(sa));
 344                 return 0;
 345         }
 346
 347         if (*pos != WLAN_EID_ADV_PROTO) {
 348                 wpa_printf(MSG_DEBUG, "GAS: Unexpected Advertisement "
 349                            "Protocol element ID %u in response from " MACSTR,
 350                            *pos, MAC2STR(sa));
 351                 return 0;
 352         }
 353
 354         adv_proto = pos;
 355         pos += 2 + pos[1];
 356
 357         /* Query Response Length */
 358         if (pos + 2 > data + len) {
 359                 wpa_printf(MSG_DEBUG, "GAS: No room for GAS Response Length");
 360                 return 0;
 361         }
 362         resp_len = WPA_GET_LE16(pos);
 363         pos += 2;
 364
 365         if (pos + resp_len > data + len) {
 366                 wpa_printf(MSG_DEBUG, "GAS: Truncated Query Response in "
 367                            "response from " MACSTR, MAC2STR(sa));
 368                 return 0;
 369         }
 370
 371         if (pos + resp_len < data + len) {
 372                 wpa_printf(MSG_DEBUG, "GAS: Ignore %u octets of extra data "
 373                            "after Query Response from " MACSTR,
 374                            (unsigned int) (data + len - pos - resp_len),
 375                            MAC2STR(sa));
 376         }
 377
 378         if (action == WLAN_PA_GAS_COMEBACK_RESP)
 379                 gas_query_rx_comeback(gas, query, adv_proto, pos, resp_len,
 380                                       frag_id, more_frags, comeback_delay);
 381         else
 382                 gas_query_rx_initial(gas, query, adv_proto, pos, resp_len,
 383                                      comeback_delay);
 384
 385         return 0;
 386 }
 387
 388
 389 static void gas_query_timeout(void *eloop_data, void *user_ctx)
 390 {
 391         struct gas_query *gas = eloop_data;
 392         struct gas_query_pending *query = user_ctx;
 393
 394         wpa_printf(MSG_DEBUG, "GAS: No response received for query to " MACSTR,
 395                    MAC2STR(query->addr));
 396         gas_query_done(gas, query, GAS_QUERY_TIMEOUT);
 397 }
 398
 399
 400 static int gas_query_dialog_token_available(struct gas_query *gas,
 401                                             const u8 *dst, u8 dialog_token)
 402 {
 403         struct gas_query_pending *q;
 404         dl_list_for_each(q, &gas->pending, struct gas_query_pending, list) {
 405                 if (os_memcmp(dst, q->addr, ETH_ALEN) == 0 &&
 406                     dialog_token == q->dialog_token)
 407                         return 0;
 408         }
 409
 410         return 1;
 411 }
 412
 413
 414 int gas_query_req(struct gas_query *gas, const u8 *dst, int freq,
 415                   struct wpabuf *req,
 416                   void (*cb)(void *ctx, const u8 *dst, u8 dialog_token,
 417                              enum gas_query_result result,
 418                              const struct wpabuf *adv_proto,
 419                              const struct wpabuf *resp, u16 status_code),
 420                   void *ctx)
 421 {
 422         struct gas_query_pending *query;
 423         int dialog_token;
 424
 425         if (wpabuf_len(req) < 3)
 426                 return -1;
 427
 428         for (dialog_token = 0; dialog_token < 256; dialog_token++) {
 429                 if (gas_query_dialog_token_available(gas, dst, dialog_token))
 430                         break;
 431         }
 432         if (dialog_token == 256)
 433                 return -1; /* Too many pending queries */
 434
 435         query = os_zalloc(sizeof(*query));
 436         if (query == NULL)
 437                 return -1;
 438
 439         os_memcpy(query->addr, dst, ETH_ALEN);
 440         query->dialog_token = dialog_token;
 441         query->freq = freq;
 442         query->cb = cb;
 443         query->ctx = ctx;
 444         dl_list_add(&gas->pending, &query->list);
 445
 446         *(wpabuf_mhead_u8(req) + 2) = dialog_token;
 447
 448         wpa_printf(MSG_DEBUG, "GAS: Starting request for " MACSTR
 449                    " dialog_token %u", MAC2STR(dst), dialog_token);
 450         if (gas_query_tx(gas, query, req) < 0) {
 451                 wpa_printf(MSG_DEBUG, "GAS: Failed to send Action frame to "
 452                            MACSTR, MAC2STR(query->addr));
 453                 os_free(query);
 454                 return -1;
 455         }
 456
 457         eloop_register_timeout(GAS_QUERY_TIMEOUT, 0, gas_query_timeout,
 458                                gas, query);
 459
 460         return dialog_token;
 461 }
 462
 463
 464 void gas_query_cancel(struct gas_query *gas, const u8 *dst, u8 dialog_token)
 465 {
 466         struct gas_query_pending *query;
 467
 468         query = gas_query_get_pending(gas, dst, dialog_token);
 469         if (query)
 470                 gas_query_done(gas, query, GAS_QUERY_CANCELLED);
 471
 472 }