gas_query.c 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481
  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. #include "includes.h"
  16. #include "common.h"
  17. #include "utils/eloop.h"
  18. #include "common/ieee802_11_defs.h"
  19. #include "common/gas.h"
  20. #include "wpa_supplicant_i.h"
  21. #include "driver_i.h"
  22. #include "offchannel.h"
  23. #include "gas_query.h"
  24. #define GAS_QUERY_TIMEOUT 5
  25. struct gas_query_pending {
  26. struct dl_list list;
  27. u8 addr[ETH_ALEN];
  28. u8 dialog_token;
  29. u8 next_frag_id;
  30. unsigned int wait_comeback:1;
  31. unsigned int offchannel_tx_started:1;
  32. int freq;
  33. u16 status_code;
  34. struct wpabuf *adv_proto;
  35. struct wpabuf *resp;
  36. void (*cb)(void *ctx, const u8 *dst, u8 dialog_token,
  37. enum gas_query_result result,
  38. const struct wpabuf *adv_proto,
  39. const struct wpabuf *resp, u16 status_code);
  40. void *ctx;
  41. };
  42. struct gas_query {
  43. struct wpa_supplicant *wpa_s;
  44. struct dl_list pending; /* struct gas_query_pending */
  45. };
  46. static void gas_query_tx_comeback_timeout(void *eloop_data, void *user_ctx);
  47. static void gas_query_timeout(void *eloop_data, void *user_ctx);
  48. struct gas_query * gas_query_init(struct wpa_supplicant *wpa_s)
  49. {
  50. struct gas_query *gas;
  51. gas = os_zalloc(sizeof(*gas));
  52. if (gas == NULL)
  53. return NULL;
  54. gas->wpa_s = wpa_s;
  55. dl_list_init(&gas->pending);
  56. return gas;
  57. }
  58. static void gas_query_done(struct gas_query *gas,
  59. struct gas_query_pending *query,
  60. enum gas_query_result result)
  61. {
  62. if (query->offchannel_tx_started)
  63. offchannel_send_action_done(gas->wpa_s);
  64. eloop_cancel_timeout(gas_query_tx_comeback_timeout, gas, query);
  65. eloop_cancel_timeout(gas_query_timeout, gas, query);
  66. dl_list_del(&query->list);
  67. query->cb(query->ctx, query->addr, query->dialog_token, result,
  68. query->adv_proto, query->resp, query->status_code);
  69. wpabuf_free(query->adv_proto);
  70. wpabuf_free(query->resp);
  71. os_free(query);
  72. }
  73. void gas_query_deinit(struct gas_query *gas)
  74. {
  75. struct gas_query_pending *query, *next;
  76. if (gas == NULL)
  77. return;
  78. dl_list_for_each_safe(query, next, &gas->pending,
  79. struct gas_query_pending, list)
  80. gas_query_done(gas, query, GAS_QUERY_DELETED_AT_DEINIT);
  81. os_free(gas);
  82. }
  83. static struct gas_query_pending *
  84. gas_query_get_pending(struct gas_query *gas, const u8 *addr, u8 dialog_token)
  85. {
  86. struct gas_query_pending *q;
  87. dl_list_for_each(q, &gas->pending, struct gas_query_pending, list) {
  88. if (os_memcmp(q->addr, addr, ETH_ALEN) == 0 &&
  89. q->dialog_token == dialog_token)
  90. return q;
  91. }
  92. return NULL;
  93. }
  94. static int gas_query_append(struct gas_query_pending *query, const u8 *data,
  95. size_t len)
  96. {
  97. if (wpabuf_resize(&query->resp, len) < 0) {
  98. wpa_printf(MSG_DEBUG, "GAS: No memory to store the response");
  99. return -1;
  100. }
  101. wpabuf_put_data(query->resp, data, len);
  102. return 0;
  103. }
  104. static int gas_query_tx(struct gas_query *gas, struct gas_query_pending *query,
  105. struct wpabuf *req)
  106. {
  107. int res;
  108. wpa_printf(MSG_DEBUG, "GAS: Send action frame to " MACSTR " len=%u "
  109. "freq=%d", MAC2STR(query->addr),
  110. (unsigned int) wpabuf_len(req), query->freq);
  111. res = offchannel_send_action(gas->wpa_s, query->freq, query->addr,
  112. gas->wpa_s->own_addr, query->addr,
  113. wpabuf_head(req), wpabuf_len(req), 1000,
  114. NULL);
  115. if (res == 0)
  116. query->offchannel_tx_started = 1;
  117. return res;
  118. }
  119. static void gas_query_tx_comeback_req(struct gas_query *gas,
  120. struct gas_query_pending *query)
  121. {
  122. struct wpabuf *req;
  123. req = gas_build_comeback_req(query->dialog_token);
  124. if (req == NULL) {
  125. gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR);
  126. return;
  127. }
  128. if (gas_query_tx(gas, query, req) < 0) {
  129. wpa_printf(MSG_DEBUG, "GAS: Failed to send Action frame to "
  130. MACSTR, MAC2STR(query->addr));
  131. gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR);
  132. }
  133. wpabuf_free(req);
  134. }
  135. static void gas_query_tx_comeback_timeout(void *eloop_data, void *user_ctx)
  136. {
  137. struct gas_query *gas = eloop_data;
  138. struct gas_query_pending *query = user_ctx;
  139. wpa_printf(MSG_DEBUG, "GAS: Comeback timeout for request to " MACSTR,
  140. MAC2STR(query->addr));
  141. gas_query_tx_comeback_req(gas, query);
  142. }
  143. static void gas_query_tx_comeback_req_delay(struct gas_query *gas,
  144. struct gas_query_pending *query,
  145. u16 comeback_delay)
  146. {
  147. unsigned int secs, usecs;
  148. secs = (comeback_delay * 1024) / 1000000;
  149. usecs = comeback_delay * 1024 - secs * 1000000;
  150. wpa_printf(MSG_DEBUG, "GAS: Send comeback request to " MACSTR
  151. " in %u secs %u usecs", MAC2STR(query->addr), secs, usecs);
  152. eloop_cancel_timeout(gas_query_tx_comeback_timeout, gas, query);
  153. eloop_register_timeout(secs, usecs, gas_query_tx_comeback_timeout,
  154. gas, query);
  155. }
  156. static void gas_query_rx_initial(struct gas_query *gas,
  157. struct gas_query_pending *query,
  158. const u8 *adv_proto, const u8 *resp,
  159. size_t len, u16 comeback_delay)
  160. {
  161. wpa_printf(MSG_DEBUG, "GAS: Received initial response from "
  162. MACSTR " (dialog_token=%u comeback_delay=%u)",
  163. MAC2STR(query->addr), query->dialog_token, comeback_delay);
  164. query->adv_proto = wpabuf_alloc_copy(adv_proto, 2 + adv_proto[1]);
  165. if (query->adv_proto == NULL) {
  166. gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR);
  167. return;
  168. }
  169. if (comeback_delay) {
  170. query->wait_comeback = 1;
  171. gas_query_tx_comeback_req_delay(gas, query, comeback_delay);
  172. return;
  173. }
  174. /* Query was completed without comeback mechanism */
  175. if (gas_query_append(query, resp, len) < 0) {
  176. gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR);
  177. return;
  178. }
  179. gas_query_done(gas, query, GAS_QUERY_SUCCESS);
  180. }
  181. static void gas_query_rx_comeback(struct gas_query *gas,
  182. struct gas_query_pending *query,
  183. const u8 *adv_proto, const u8 *resp,
  184. size_t len, u8 frag_id, u8 more_frags,
  185. u16 comeback_delay)
  186. {
  187. wpa_printf(MSG_DEBUG, "GAS: Received comeback response from "
  188. MACSTR " (dialog_token=%u frag_id=%u more_frags=%u "
  189. "comeback_delay=%u)",
  190. MAC2STR(query->addr), query->dialog_token, frag_id,
  191. more_frags, comeback_delay);
  192. if ((size_t) 2 + adv_proto[1] != wpabuf_len(query->adv_proto) ||
  193. os_memcmp(adv_proto, wpabuf_head(query->adv_proto),
  194. wpabuf_len(query->adv_proto)) != 0) {
  195. wpa_printf(MSG_DEBUG, "GAS: Advertisement Protocol changed "
  196. "between initial and comeback response from "
  197. MACSTR, MAC2STR(query->addr));
  198. gas_query_done(gas, query, GAS_QUERY_PEER_ERROR);
  199. return;
  200. }
  201. if (comeback_delay) {
  202. if (frag_id) {
  203. wpa_printf(MSG_DEBUG, "GAS: Invalid comeback response "
  204. "with non-zero frag_id and comeback_delay "
  205. "from " MACSTR, MAC2STR(query->addr));
  206. gas_query_done(gas, query, GAS_QUERY_PEER_ERROR);
  207. return;
  208. }
  209. gas_query_tx_comeback_req_delay(gas, query, comeback_delay);
  210. return;
  211. }
  212. if (frag_id != query->next_frag_id) {
  213. wpa_printf(MSG_DEBUG, "GAS: Unexpected frag_id in response "
  214. "from " MACSTR, MAC2STR(query->addr));
  215. gas_query_done(gas, query, GAS_QUERY_PEER_ERROR);
  216. return;
  217. }
  218. query->next_frag_id++;
  219. if (gas_query_append(query, resp, len) < 0) {
  220. gas_query_done(gas, query, GAS_QUERY_INTERNAL_ERROR);
  221. return;
  222. }
  223. if (more_frags) {
  224. gas_query_tx_comeback_req(gas, query);
  225. return;
  226. }
  227. gas_query_done(gas, query, GAS_QUERY_SUCCESS);
  228. }
  229. int gas_query_rx(struct gas_query *gas, const u8 *da, const u8 *sa,
  230. const u8 *bssid, const u8 *data, size_t len, int freq)
  231. {
  232. struct gas_query_pending *query;
  233. u8 action, dialog_token, frag_id = 0, more_frags = 0;
  234. u16 comeback_delay, resp_len;
  235. const u8 *pos, *adv_proto;
  236. if (gas == NULL || len < 4)
  237. return -1;
  238. pos = data;
  239. action = *pos++;
  240. dialog_token = *pos++;
  241. if (action != WLAN_PA_GAS_INITIAL_RESP &&
  242. action != WLAN_PA_GAS_COMEBACK_RESP)
  243. return -1; /* Not a GAS response */
  244. query = gas_query_get_pending(gas, sa, dialog_token);
  245. if (query == NULL) {
  246. wpa_printf(MSG_DEBUG, "GAS: No pending query found for " MACSTR
  247. " dialog token %u", MAC2STR(sa), dialog_token);
  248. return -1;
  249. }
  250. if (query->wait_comeback && action == WLAN_PA_GAS_INITIAL_RESP) {
  251. wpa_printf(MSG_DEBUG, "GAS: Unexpected initial response from "
  252. MACSTR " dialog token %u when waiting for comeback "
  253. "response", MAC2STR(sa), dialog_token);
  254. return 0;
  255. }
  256. if (!query->wait_comeback && action == WLAN_PA_GAS_COMEBACK_RESP) {
  257. wpa_printf(MSG_DEBUG, "GAS: Unexpected comeback response from "
  258. MACSTR " dialog token %u when waiting for initial "
  259. "response", MAC2STR(sa), dialog_token);
  260. return 0;
  261. }
  262. query->status_code = WPA_GET_LE16(pos);
  263. pos += 2;
  264. if (query->status_code != WLAN_STATUS_SUCCESS) {
  265. wpa_printf(MSG_DEBUG, "GAS: Query to " MACSTR " dialog token "
  266. "%u failed - status code %u",
  267. MAC2STR(sa), dialog_token, query->status_code);
  268. gas_query_done(gas, query, GAS_QUERY_FAILURE);
  269. return 0;
  270. }
  271. if (action == WLAN_PA_GAS_COMEBACK_RESP) {
  272. if (pos + 1 > data + len)
  273. return 0;
  274. frag_id = *pos & 0x7f;
  275. more_frags = (*pos & 0x80) >> 7;
  276. pos++;
  277. }
  278. /* Comeback Delay */
  279. if (pos + 2 > data + len)
  280. return 0;
  281. comeback_delay = WPA_GET_LE16(pos);
  282. pos += 2;
  283. /* Advertisement Protocol element */
  284. if (pos + 2 > data + len || pos + 2 + pos[1] > data + len) {
  285. wpa_printf(MSG_DEBUG, "GAS: No room for Advertisement "
  286. "Protocol element in the response from " MACSTR,
  287. MAC2STR(sa));
  288. return 0;
  289. }
  290. if (*pos != WLAN_EID_ADV_PROTO) {
  291. wpa_printf(MSG_DEBUG, "GAS: Unexpected Advertisement "
  292. "Protocol element ID %u in response from " MACSTR,
  293. *pos, MAC2STR(sa));
  294. return 0;
  295. }
  296. adv_proto = pos;
  297. pos += 2 + pos[1];
  298. /* Query Response Length */
  299. if (pos + 2 > data + len) {
  300. wpa_printf(MSG_DEBUG, "GAS: No room for GAS Response Length");
  301. return 0;
  302. }
  303. resp_len = WPA_GET_LE16(pos);
  304. pos += 2;
  305. if (pos + resp_len > data + len) {
  306. wpa_printf(MSG_DEBUG, "GAS: Truncated Query Response in "
  307. "response from " MACSTR, MAC2STR(sa));
  308. return 0;
  309. }
  310. if (pos + resp_len < data + len) {
  311. wpa_printf(MSG_DEBUG, "GAS: Ignore %u octets of extra data "
  312. "after Query Response from " MACSTR,
  313. (unsigned int) (data + len - pos - resp_len),
  314. MAC2STR(sa));
  315. }
  316. if (action == WLAN_PA_GAS_COMEBACK_RESP)
  317. gas_query_rx_comeback(gas, query, adv_proto, pos, resp_len,
  318. frag_id, more_frags, comeback_delay);
  319. else
  320. gas_query_rx_initial(gas, query, adv_proto, pos, resp_len,
  321. comeback_delay);
  322. return 0;
  323. }
  324. static void gas_query_timeout(void *eloop_data, void *user_ctx)
  325. {
  326. struct gas_query *gas = eloop_data;
  327. struct gas_query_pending *query = user_ctx;
  328. wpa_printf(MSG_DEBUG, "GAS: No response received for query to " MACSTR,
  329. MAC2STR(query->addr));
  330. gas_query_done(gas, query, GAS_QUERY_TIMEOUT);
  331. }
  332. static int gas_query_dialog_token_available(struct gas_query *gas,
  333. const u8 *dst, u8 dialog_token)
  334. {
  335. struct gas_query_pending *q;
  336. dl_list_for_each(q, &gas->pending, struct gas_query_pending, list) {
  337. if (os_memcmp(dst, q->addr, ETH_ALEN) == 0 &&
  338. dialog_token == q->dialog_token)
  339. return 0;
  340. }
  341. return 1;
  342. }
  343. int gas_query_req(struct gas_query *gas, const u8 *dst, int freq,
  344. struct wpabuf *req,
  345. void (*cb)(void *ctx, const u8 *dst, u8 dialog_token,
  346. enum gas_query_result result,
  347. const struct wpabuf *adv_proto,
  348. const struct wpabuf *resp, u16 status_code),
  349. void *ctx)
  350. {
  351. struct gas_query_pending *query;
  352. int dialog_token;
  353. if (wpabuf_len(req) < 3)
  354. return -1;
  355. for (dialog_token = 0; dialog_token < 256; dialog_token++) {
  356. if (gas_query_dialog_token_available(gas, dst, dialog_token))
  357. break;
  358. }
  359. if (dialog_token == 256)
  360. return -1; /* Too many pending queries */
  361. query = os_zalloc(sizeof(*query));
  362. if (query == NULL)
  363. return -1;
  364. os_memcpy(query->addr, dst, ETH_ALEN);
  365. query->dialog_token = dialog_token;
  366. query->freq = freq;
  367. query->cb = cb;
  368. query->ctx = ctx;
  369. dl_list_add(&gas->pending, &query->list);
  370. *(wpabuf_mhead_u8(req) + 2) = dialog_token;
  371. wpa_printf(MSG_DEBUG, "GAS: Starting request for " MACSTR
  372. " dialog_token %u", MAC2STR(dst), dialog_token);
  373. if (gas_query_tx(gas, query, req) < 0) {
  374. wpa_printf(MSG_DEBUG, "GAS: Failed to send Action frame to "
  375. MACSTR, MAC2STR(query->addr));
  376. os_free(query);
  377. return -1;
  378. }
  379. eloop_register_timeout(GAS_QUERY_TIMEOUT, 0, gas_query_timeout,
  380. gas, query);
  381. return dialog_token;
  382. }
  383. void gas_query_cancel(struct gas_query *gas, const u8 *dst, u8 dialog_token)
  384. {
  385. struct gas_query_pending *query;
  386. query = gas_query_get_pending(gas, dst, dialog_token);
  387. if (query)
  388. gas_query_done(gas, query, GAS_QUERY_CANCELLED);
  389. }