1 /* 2 * Generic advertisement service (GAS) server 3 * Copyright (c) 2017, Qualcomm Atheros, Inc. 4 * 5 * This software may be distributed under the terms of the BSD license. 6 * See README for more details. 7 */ 8 9 #include "includes.h" 10 11 #include "utils/common.h" 12 #include "utils/list.h" 13 #include "utils/eloop.h" 14 #include "ieee802_11_defs.h" 15 #include "gas.h" 16 #include "gas_server.h" 17 18 19 #define MAX_ADV_PROTO_ID_LEN 10 20 #define GAS_QUERY_TIMEOUT 10 21 22 struct gas_server_handler { 23 struct dl_list list; 24 u8 adv_proto_id[MAX_ADV_PROTO_ID_LEN]; 25 u8 adv_proto_id_len; 26 struct wpabuf * (*req_cb)(void *ctx, const u8 *sa, 27 const u8 *query, size_t query_len); 28 void (*status_cb)(void *ctx, struct wpabuf *resp, int ok); 29 void *ctx; 30 struct gas_server *gas; 31 }; 32 33 struct gas_server_response { 34 struct dl_list list; 35 size_t offset; 36 u8 frag_id; 37 struct wpabuf *resp; 38 int freq; 39 u8 dst[ETH_ALEN]; 40 u8 dialog_token; 41 struct gas_server_handler *handler; 42 }; 43 44 struct gas_server { 45 struct dl_list handlers; /* struct gas_server_handler::list */ 46 struct dl_list responses; /* struct gas_server_response::list */ 47 void (*tx)(void *ctx, int freq, const u8 *da, struct wpabuf *resp, 48 unsigned int wait_time); 49 void *ctx; 50 }; 51 52 static void gas_server_free_response(struct gas_server_response *response); 53 54 55 static void gas_server_response_timeout(void *eloop_ctx, void *user_ctx) 56 { 57 struct gas_server_response *response = eloop_ctx; 58 59 wpa_printf(MSG_DEBUG, "GAS: Response @%p timeout for " MACSTR 60 " (dialog_token=%u freq=%d frag_id=%u sent=%lu/%lu) - drop pending data", 61 response, MAC2STR(response->dst), response->dialog_token, 62 response->freq, response->frag_id, 63 (unsigned long) response->offset, 64 (unsigned long) wpabuf_len(response->resp)); 65 response->handler->status_cb(response->handler->ctx, 66 response->resp, 0); 67 response->resp = NULL; 68 dl_list_del(&response->list); 69 gas_server_free_response(response); 70 } 71 72 73 static void gas_server_free_response(struct gas_server_response *response) 74 { 75 if (!response) 76 return; 77 wpa_printf(MSG_DEBUG, "DPP: Free GAS response @%p", response); 78 eloop_cancel_timeout(gas_server_response_timeout, response, NULL); 79 wpabuf_free(response->resp); 80 os_free(response); 81 } 82 83 84 static void 85 gas_server_send_resp(struct gas_server *gas, struct gas_server_handler *handler, 86 const u8 *da, int freq, u8 dialog_token, 87 struct wpabuf *query_resp) 88 { 89 size_t max_len = (freq > 56160) ? 928 : 1400; 90 size_t hdr_len = 24 + 2 + 5 + 3 + handler->adv_proto_id_len + 2; 91 size_t resp_frag_len; 92 struct wpabuf *resp; 93 u16 comeback_delay; 94 struct gas_server_response *response; 95 96 if (!query_resp) 97 return; 98 99 response = os_zalloc(sizeof(*response)); 100 if (!response) { 101 wpabuf_free(query_resp); 102 return; 103 } 104 wpa_printf(MSG_DEBUG, "DPP: Allocated GAS response @%p", response); 105 response->freq = freq; 106 response->handler = handler; 107 os_memcpy(response->dst, da, ETH_ALEN); 108 response->dialog_token = dialog_token; 109 if (hdr_len + wpabuf_len(query_resp) > max_len) { 110 /* Need to use comeback to initiate fragmentation */ 111 comeback_delay = 1; 112 resp_frag_len = 0; 113 } else { 114 /* Full response fits into the initial response */ 115 comeback_delay = 0; 116 resp_frag_len = wpabuf_len(query_resp); 117 } 118 119 resp = gas_build_initial_resp(dialog_token, WLAN_STATUS_SUCCESS, 120 comeback_delay, 121 handler->adv_proto_id_len + 122 resp_frag_len); 123 if (!resp) { 124 wpabuf_free(query_resp); 125 gas_server_free_response(response); 126 return; 127 } 128 129 /* Advertisement Protocol element */ 130 wpabuf_put_u8(resp, WLAN_EID_ADV_PROTO); 131 wpabuf_put_u8(resp, 1 + handler->adv_proto_id_len); /* Length */ 132 wpabuf_put_u8(resp, 0x7f); 133 /* Advertisement Protocol ID */ 134 wpabuf_put_data(resp, handler->adv_proto_id, handler->adv_proto_id_len); 135 136 /* Query Response Length */ 137 wpabuf_put_le16(resp, resp_frag_len); 138 if (!comeback_delay) 139 wpabuf_put_buf(resp, query_resp); 140 141 if (comeback_delay) { 142 wpa_printf(MSG_DEBUG, 143 "GAS: Need to fragment query response"); 144 } else { 145 wpa_printf(MSG_DEBUG, 146 "GAS: Full query response fits in the GAS Initial Response frame"); 147 } 148 response->offset = resp_frag_len; 149 response->resp = query_resp; 150 dl_list_add(&gas->responses, &response->list); 151 gas->tx(gas->ctx, freq, da, resp, comeback_delay ? 2000 : 0); 152 wpabuf_free(resp); 153 eloop_register_timeout(GAS_QUERY_TIMEOUT, 0, 154 gas_server_response_timeout, response, NULL); 155 } 156 157 158 static int 159 gas_server_rx_initial_req(struct gas_server *gas, const u8 *da, const u8 *sa, 160 const u8 *bssid, int freq, u8 dialog_token, 161 const u8 *data, size_t len) 162 { 163 const u8 *pos, *end, *adv_proto, *query_req; 164 u8 adv_proto_len; 165 u16 query_req_len; 166 struct gas_server_handler *handler; 167 struct wpabuf *resp; 168 169 wpa_hexdump(MSG_MSGDUMP, "GAS: Received GAS Initial Request frame", 170 data, len); 171 pos = data; 172 end = data + len; 173 174 if (end - pos < 2 || pos[0] != WLAN_EID_ADV_PROTO) { 175 wpa_printf(MSG_DEBUG, 176 "GAS: No Advertisement Protocol element found"); 177 return -1; 178 } 179 pos++; 180 adv_proto_len = *pos++; 181 if (end - pos < adv_proto_len || adv_proto_len < 2) { 182 wpa_printf(MSG_DEBUG, 183 "GAS: Truncated Advertisement Protocol element"); 184 return -1; 185 } 186 187 adv_proto = pos; 188 pos += adv_proto_len; 189 wpa_hexdump(MSG_MSGDUMP, "GAS: Advertisement Protocol element", 190 adv_proto, adv_proto_len); 191 192 if (end - pos < 2) { 193 wpa_printf(MSG_DEBUG, "GAS: No Query Request Length field"); 194 return -1; 195 } 196 query_req_len = WPA_GET_LE16(pos); 197 pos += 2; 198 if (end - pos < query_req_len) { 199 wpa_printf(MSG_DEBUG, "GAS: Truncated Query Request field"); 200 return -1; 201 } 202 query_req = pos; 203 pos += query_req_len; 204 wpa_hexdump(MSG_MSGDUMP, "GAS: Query Request", 205 query_req, query_req_len); 206 207 if (pos < end) { 208 wpa_hexdump(MSG_MSGDUMP, 209 "GAS: Ignored extra data after Query Request field", 210 pos, end - pos); 211 } 212 213 dl_list_for_each(handler, &gas->handlers, struct gas_server_handler, 214 list) { 215 if (adv_proto_len < 1 + handler->adv_proto_id_len || 216 os_memcmp(adv_proto + 1, handler->adv_proto_id, 217 handler->adv_proto_id_len) != 0) 218 continue; 219 220 wpa_printf(MSG_DEBUG, 221 "GAS: Calling handler for the requested Advertisement Protocol ID"); 222 resp = handler->req_cb(handler->ctx, sa, query_req, 223 query_req_len); 224 wpa_hexdump_buf(MSG_MSGDUMP, "GAS: Response from the handler", 225 resp); 226 gas_server_send_resp(gas, handler, sa, freq, dialog_token, 227 resp); 228 return 0; 229 } 230 231 wpa_printf(MSG_DEBUG, 232 "GAS: No registered handler for the requested Advertisement Protocol ID"); 233 return -1; 234 } 235 236 237 static void 238 gas_server_handle_rx_comeback_req(struct gas_server_response *response) 239 { 240 struct gas_server_handler *handler = response->handler; 241 struct gas_server *gas = handler->gas; 242 size_t max_len = (response->freq > 56160) ? 928 : 1400; 243 size_t hdr_len = 24 + 2 + 6 + 3 + handler->adv_proto_id_len + 2; 244 size_t remaining, resp_frag_len; 245 struct wpabuf *resp; 246 247 remaining = wpabuf_len(response->resp) - response->offset; 248 if (hdr_len + remaining > max_len) 249 resp_frag_len = max_len - hdr_len; 250 else 251 resp_frag_len = remaining; 252 wpa_printf(MSG_DEBUG, 253 "GAS: Sending out %u/%u remaining Query Response octets", 254 (unsigned int) resp_frag_len, (unsigned int) remaining); 255 256 resp = gas_build_comeback_resp(response->dialog_token, 257 WLAN_STATUS_SUCCESS, 258 response->frag_id++, 259 resp_frag_len < remaining, 0, 260 handler->adv_proto_id_len + 261 resp_frag_len); 262 if (!resp) { 263 dl_list_del(&response->list); 264 gas_server_free_response(response); 265 return; 266 } 267 268 /* Advertisement Protocol element */ 269 wpabuf_put_u8(resp, WLAN_EID_ADV_PROTO); 270 wpabuf_put_u8(resp, 1 + handler->adv_proto_id_len); /* Length */ 271 wpabuf_put_u8(resp, 0x7f); 272 /* Advertisement Protocol ID */ 273 wpabuf_put_data(resp, handler->adv_proto_id, handler->adv_proto_id_len); 274 275 /* Query Response Length */ 276 wpabuf_put_le16(resp, resp_frag_len); 277 wpabuf_put_data(resp, wpabuf_head_u8(response->resp) + response->offset, 278 resp_frag_len); 279 280 response->offset += resp_frag_len; 281 282 gas->tx(gas->ctx, response->freq, response->dst, resp, 283 remaining > resp_frag_len ? 2000 : 0); 284 wpabuf_free(resp); 285 } 286 287 288 static int 289 gas_server_rx_comeback_req(struct gas_server *gas, const u8 *da, const u8 *sa, 290 const u8 *bssid, int freq, u8 dialog_token) 291 { 292 struct gas_server_response *response; 293 294 dl_list_for_each(response, &gas->responses, struct gas_server_response, 295 list) { 296 if (response->dialog_token != dialog_token || 297 os_memcmp(sa, response->dst, ETH_ALEN) != 0) 298 continue; 299 gas_server_handle_rx_comeback_req(response); 300 return 0; 301 } 302 303 wpa_printf(MSG_DEBUG, "GAS: No pending GAS response for " MACSTR 304 " (dialog token %u)", MAC2STR(sa), dialog_token); 305 return -1; 306 } 307 308 309 /** 310 * gas_query_rx - Indicate reception of a Public Action or Protected Dual frame 311 * @gas: GAS query data from gas_server_init() 312 * @da: Destination MAC address of the Action frame 313 * @sa: Source MAC address of the Action frame 314 * @bssid: BSSID of the Action frame 315 * @categ: Category of the Action frame 316 * @data: Payload of the Action frame 317 * @len: Length of @data 318 * @freq: Frequency (in MHz) on which the frame was received 319 * Returns: 0 if the Public Action frame was a GAS request frame or -1 if not 320 */ 321 int gas_server_rx(struct gas_server *gas, const u8 *da, const u8 *sa, 322 const u8 *bssid, u8 categ, const u8 *data, size_t len, 323 int freq) 324 { 325 u8 action, dialog_token; 326 const u8 *pos, *end; 327 328 if (!gas || len < 2) 329 return -1; 330 331 if (categ == WLAN_ACTION_PROTECTED_DUAL) 332 return -1; /* Not supported for now */ 333 334 pos = data; 335 end = data + len; 336 action = *pos++; 337 dialog_token = *pos++; 338 339 if (action != WLAN_PA_GAS_INITIAL_REQ && 340 action != WLAN_PA_GAS_COMEBACK_REQ) 341 return -1; /* Not a GAS request */ 342 343 wpa_printf(MSG_DEBUG, "GAS: Received GAS %s Request frame DA=" MACSTR 344 " SA=" MACSTR " BSSID=" MACSTR 345 " freq=%d dialog_token=%u len=%u", 346 action == WLAN_PA_GAS_INITIAL_REQ ? "Initial" : "Comeback", 347 MAC2STR(da), MAC2STR(sa), MAC2STR(bssid), freq, dialog_token, 348 (unsigned int) len); 349 350 if (action == WLAN_PA_GAS_INITIAL_REQ) 351 return gas_server_rx_initial_req(gas, da, sa, bssid, 352 freq, dialog_token, 353 pos, end - pos); 354 return gas_server_rx_comeback_req(gas, da, sa, bssid, 355 freq, dialog_token); 356 } 357 358 359 static void gas_server_handle_tx_status(struct gas_server_response *response, 360 int ack) 361 { 362 if (ack && response->offset < wpabuf_len(response->resp)) { 363 wpa_printf(MSG_DEBUG, 364 "GAS: More fragments remaining - keep pending entry"); 365 return; 366 } 367 368 if (!ack) 369 wpa_printf(MSG_DEBUG, 370 "GAS: No ACK received - drop pending entry"); 371 else 372 wpa_printf(MSG_DEBUG, 373 "GAS: Last fragment of the response sent out - drop pending entry"); 374 375 response->handler->status_cb(response->handler->ctx, 376 response->resp, ack); 377 response->resp = NULL; 378 dl_list_del(&response->list); 379 gas_server_free_response(response); 380 } 381 382 383 void gas_server_tx_status(struct gas_server *gas, const u8 *dst, const u8 *data, 384 size_t data_len, int ack) 385 { 386 const u8 *pos; 387 u8 action, code, dialog_token; 388 struct gas_server_response *response; 389 390 if (data_len < 24 + 3) 391 return; 392 pos = data + 24; 393 action = *pos++; 394 code = *pos++; 395 dialog_token = *pos++; 396 if (action != WLAN_ACTION_PUBLIC || 397 (code != WLAN_PA_GAS_INITIAL_RESP && 398 code != WLAN_PA_GAS_COMEBACK_RESP)) 399 return; 400 wpa_printf(MSG_DEBUG, "GAS: TX status dst=" MACSTR 401 " ack=%d %s dialog_token=%u", 402 MAC2STR(dst), ack, 403 code == WLAN_PA_GAS_INITIAL_RESP ? "initial" : "comeback", 404 dialog_token); 405 dl_list_for_each(response, &gas->responses, struct gas_server_response, 406 list) { 407 if (response->dialog_token != dialog_token || 408 os_memcmp(dst, response->dst, ETH_ALEN) != 0) 409 continue; 410 gas_server_handle_tx_status(response, ack); 411 return; 412 } 413 414 wpa_printf(MSG_DEBUG, "GAS: No pending response matches TX status"); 415 } 416 417 418 struct gas_server * gas_server_init(void *ctx, 419 void (*tx)(void *ctx, int freq, 420 const u8 *da, 421 struct wpabuf *buf, 422 unsigned int wait_time)) 423 { 424 struct gas_server *gas; 425 426 gas = os_zalloc(sizeof(*gas)); 427 if (!gas) 428 return NULL; 429 gas->ctx = ctx; 430 gas->tx = tx; 431 dl_list_init(&gas->handlers); 432 dl_list_init(&gas->responses); 433 return gas; 434 } 435 436 437 void gas_server_deinit(struct gas_server *gas) 438 { 439 struct gas_server_handler *handler, *tmp; 440 struct gas_server_response *response, *tmp_r; 441 442 if (!gas) 443 return; 444 445 dl_list_for_each_safe(handler, tmp, &gas->handlers, 446 struct gas_server_handler, list) { 447 dl_list_del(&handler->list); 448 os_free(handler); 449 } 450 451 dl_list_for_each_safe(response, tmp_r, &gas->responses, 452 struct gas_server_response, list) { 453 dl_list_del(&response->list); 454 gas_server_free_response(response); 455 } 456 457 os_free(gas); 458 } 459 460 461 int gas_server_register(struct gas_server *gas, 462 const u8 *adv_proto_id, u8 adv_proto_id_len, 463 struct wpabuf * 464 (*req_cb)(void *ctx, const u8 *sa, 465 const u8 *query, size_t query_len), 466 void (*status_cb)(void *ctx, struct wpabuf *resp, 467 int ok), 468 void *ctx) 469 { 470 struct gas_server_handler *handler; 471 472 if (!gas || adv_proto_id_len > MAX_ADV_PROTO_ID_LEN) 473 return -1; 474 handler = os_zalloc(sizeof(*handler)); 475 if (!handler) 476 return -1; 477 478 os_memcpy(handler->adv_proto_id, adv_proto_id, adv_proto_id_len); 479 handler->adv_proto_id_len = adv_proto_id_len; 480 handler->req_cb = req_cb; 481 handler->status_cb = status_cb; 482 handler->ctx = ctx; 483 handler->gas = gas; 484 dl_list_add(&gas->handlers, &handler->list); 485 486 return 0; 487 } 488