1 /* 2 * NAN Discovery Engine 3 * Copyright (c) 2024, Qualcomm Innovation Center, 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 "utils/includes.h" 10 11 #include "utils/common.h" 12 #include "utils/eloop.h" 13 #include "crypto/crypto.h" 14 #include "crypto/sha256.h" 15 #include "ieee802_11_defs.h" 16 #include "nan.h" 17 #include "nan_de.h" 18 19 static const u8 nan_network_id[ETH_ALEN] = 20 { 0x51, 0x6f, 0x9a, 0x01, 0x00, 0x00 }; 21 static const u8 wildcard_bssid[ETH_ALEN] = 22 { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; 23 24 enum nan_de_service_type { 25 NAN_DE_PUBLISH, 26 NAN_DE_SUBSCRIBE, 27 }; 28 29 struct nan_de_service { 30 int id; 31 enum nan_de_service_type type; 32 char *service_name; 33 u8 service_id[NAN_SERVICE_ID_LEN]; 34 struct nan_publish_params publish; 35 struct nan_subscribe_params subscribe; 36 enum nan_service_protocol_type srv_proto_type; 37 struct wpabuf *ssi; 38 struct wpabuf *elems; 39 struct os_reltime time_started; 40 struct os_reltime end_time; 41 struct os_reltime last_multicast; 42 struct os_reltime first_discovered; 43 struct os_reltime last_followup; 44 bool needs_fsd; 45 unsigned int freq; 46 unsigned int default_freq; 47 int *freq_list; 48 49 /* pauseState information for Publish function */ 50 struct os_reltime pause_state_end; 51 u8 sel_peer_id; 52 u8 sel_peer_addr[ETH_ALEN]; 53 54 /* Publish state - channel iteration */ 55 bool in_multi_chan; 56 bool first_multi_chan; 57 int multi_chan_idx; /* index to freq_list[] */ 58 struct os_reltime next_publish_state; 59 struct os_reltime next_publish_chan; 60 unsigned int next_publish_duration; 61 }; 62 63 struct nan_de { 64 u8 nmi[ETH_ALEN]; 65 bool ap; 66 struct nan_callbacks cb; 67 68 struct nan_de_service *service[NAN_DE_MAX_SERVICE]; 69 unsigned int num_service; 70 71 int next_handle; 72 73 unsigned int ext_listen_freq; 74 unsigned int listen_freq; 75 unsigned int tx_wait_status_freq; 76 unsigned int tx_wait_end_freq; 77 }; 78 79 80 struct nan_de * nan_de_init(const u8 *nmi, bool ap, 81 const struct nan_callbacks *cb) 82 { 83 struct nan_de *de; 84 85 de = os_zalloc(sizeof(*de)); 86 if (!de) 87 return NULL; 88 89 os_memcpy(de->nmi, nmi, ETH_ALEN); 90 de->ap = ap; 91 os_memcpy(&de->cb, cb, sizeof(*cb)); 92 93 return de; 94 } 95 96 97 static void nan_de_service_free(struct nan_de_service *srv) 98 { 99 os_free(srv->service_name); 100 wpabuf_free(srv->ssi); 101 wpabuf_free(srv->elems); 102 os_free(srv->freq_list); 103 os_free(srv); 104 } 105 106 107 static void nan_de_service_deinit(struct nan_de *de, struct nan_de_service *srv, 108 enum nan_de_reason reason) 109 { 110 if (!srv) 111 return; 112 if (srv->type == NAN_DE_PUBLISH && de->cb.publish_terminated) 113 de->cb.publish_terminated(de->cb.ctx, srv->id, reason); 114 if (srv->type == NAN_DE_SUBSCRIBE && de->cb.subscribe_terminated) 115 de->cb.subscribe_terminated(de->cb.ctx, srv->id, reason); 116 nan_de_service_free(srv); 117 } 118 119 120 static void nan_de_clear_pending(struct nan_de *de) 121 { 122 de->listen_freq = 0; 123 de->tx_wait_status_freq = 0; 124 de->tx_wait_end_freq = 0; 125 } 126 127 128 void nan_de_flush(struct nan_de *de) 129 { 130 unsigned int i; 131 132 if (!de) 133 return; 134 135 for (i = 0; i < NAN_DE_MAX_SERVICE; i++) { 136 nan_de_service_deinit(de, de->service[i], 137 NAN_DE_REASON_USER_REQUEST); 138 de->service[i] = NULL; 139 } 140 141 de->num_service = 0; 142 nan_de_clear_pending(de); 143 } 144 145 146 static void nan_de_pause_state(struct nan_de_service *srv, const u8 *peer_addr, 147 u8 peer_id) 148 { 149 wpa_printf(MSG_DEBUG, "NAN: Start pauseState"); 150 os_get_reltime(&srv->pause_state_end); 151 srv->pause_state_end.sec += 60; 152 os_memcpy(srv->sel_peer_addr, peer_addr, ETH_ALEN); 153 srv->sel_peer_id = peer_id; 154 } 155 156 157 static void nan_de_unpause_state(struct nan_de_service *srv) 158 { 159 wpa_printf(MSG_DEBUG, "NAN: Stop pauseState"); 160 srv->pause_state_end.sec = 0; 161 srv->pause_state_end.usec = 0; 162 os_memset(srv->sel_peer_addr, 0, ETH_ALEN); 163 srv->sel_peer_id = 0; 164 } 165 166 167 static struct wpabuf * nan_de_alloc_sdf(size_t len) 168 { 169 struct wpabuf *buf; 170 171 buf = wpabuf_alloc(2 + 4 + len); 172 if (buf) { 173 wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC); 174 wpabuf_put_u8(buf, WLAN_PA_VENDOR_SPECIFIC); 175 wpabuf_put_be32(buf, NAN_SDF_VENDOR_TYPE); 176 } 177 178 return buf; 179 } 180 181 182 static int nan_de_tx(struct nan_de *de, unsigned int freq, 183 unsigned int wait_time, 184 const u8 *dst, const u8 *src, const u8 *bssid, 185 const struct wpabuf *buf) 186 { 187 int res; 188 189 if (!de->cb.tx) 190 return -1; 191 192 res = de->cb.tx(de->cb.ctx, freq, wait_time, dst, src, bssid, buf); 193 if (res < 0) 194 return res; 195 196 de->tx_wait_status_freq = freq; 197 de->tx_wait_end_freq = wait_time ? freq : 0; 198 199 return res; 200 } 201 202 203 static void nan_de_tx_sdf(struct nan_de *de, struct nan_de_service *srv, 204 unsigned int wait_time, 205 enum nan_service_control_type type, 206 const u8 *dst, u8 req_instance_id, 207 const struct wpabuf *ssi) 208 { 209 struct wpabuf *buf; 210 size_t len = 0, sda_len, sdea_len; 211 u8 ctrl = type; 212 u16 sdea_ctrl = 0; 213 214 /* Service Descriptor attribute */ 215 sda_len = NAN_SERVICE_ID_LEN + 1 + 1 + 1; 216 len += NAN_ATTR_HDR_LEN + sda_len; 217 218 /* Service Descriptor Extension attribute */ 219 sdea_len = 1 + 2; 220 if (ssi) 221 sdea_len += 2 + 4 + wpabuf_len(ssi); 222 len += NAN_ATTR_HDR_LEN + sdea_len; 223 224 /* Element Container attribute */ 225 if (srv->elems) 226 len += NAN_ATTR_HDR_LEN + 1 + wpabuf_len(srv->elems); 227 228 buf = nan_de_alloc_sdf(len); 229 if (!buf) 230 return; 231 232 /* Service Descriptor attribute */ 233 wpabuf_put_u8(buf, NAN_ATTR_SDA); 234 wpabuf_put_le16(buf, sda_len); 235 wpabuf_put_data(buf, srv->service_id, NAN_SERVICE_ID_LEN); 236 wpabuf_put_u8(buf, srv->id); /* Instance ID */ 237 wpabuf_put_u8(buf, req_instance_id); /* Requestor Instance ID */ 238 wpabuf_put_u8(buf, ctrl); 239 240 /* Service Descriptor Extension attribute */ 241 if (srv->type == NAN_DE_PUBLISH || ssi) { 242 wpabuf_put_u8(buf, NAN_ATTR_SDEA); 243 wpabuf_put_le16(buf, sdea_len); 244 wpabuf_put_u8(buf, srv->id); /* Instance ID */ 245 if (srv->type == NAN_DE_PUBLISH) { 246 if (srv->publish.fsd) 247 sdea_ctrl |= NAN_SDEA_CTRL_FSD_REQ; 248 if (srv->publish.fsd_gas) 249 sdea_ctrl |= NAN_SDEA_CTRL_FSD_GAS; 250 } 251 wpabuf_put_le16(buf, sdea_ctrl); 252 if (ssi) { 253 wpabuf_put_le16(buf, 4 + wpabuf_len(ssi)); 254 wpabuf_put_be24(buf, OUI_WFA); 255 wpabuf_put_u8(buf, srv->srv_proto_type); 256 wpabuf_put_buf(buf, ssi); 257 } 258 } 259 260 /* Element Container attribute */ 261 if (srv->elems) { 262 wpabuf_put_u8(buf, NAN_ATTR_ELEM_CONTAINER); 263 wpabuf_put_le16(buf, 1 + wpabuf_len(srv->elems)); 264 wpabuf_put_u8(buf, 0); /* Map ID */ 265 wpabuf_put_buf(buf, srv->elems); 266 } 267 268 /* Wi-Fi Aware specification v4.0 uses NAN Cluster ID as A3 for USD, 269 * but there is no synchronization in USD as as such, no NAN Cluster 270 * either. Use Wildcard BSSID instead. */ 271 nan_de_tx(de, srv->freq, wait_time, dst, de->nmi, wildcard_bssid, buf); 272 wpabuf_free(buf); 273 } 274 275 276 static int nan_de_time_to_next_chan_change(struct nan_de_service *srv) 277 { 278 struct os_reltime tmp, diff, now; 279 280 if (os_reltime_before(&srv->next_publish_state, 281 &srv->next_publish_chan)) 282 tmp = srv->next_publish_state; 283 else if (srv->in_multi_chan) 284 tmp = srv->next_publish_chan; 285 else 286 tmp = srv->next_publish_state; 287 288 os_get_reltime(&now); 289 os_reltime_sub(&tmp, &now, &diff); 290 return os_reltime_in_ms(&diff); 291 } 292 293 294 static void nan_de_set_publish_times(struct nan_de_service *srv) 295 { 296 os_get_reltime(&srv->next_publish_state); 297 srv->next_publish_chan = srv->next_publish_state; 298 /* Swap single/multi channel state in N * 100 TU */ 299 os_reltime_add_ms(&srv->next_publish_state, 300 srv->next_publish_duration * 1024 / 1000); 301 302 /* Swap channel in multi channel state after 150 ms */ 303 os_reltime_add_ms(&srv->next_publish_chan, 150); 304 } 305 306 307 static void nan_de_check_chan_change(struct nan_de_service *srv) 308 { 309 if (srv->next_publish_duration) { 310 /* Update end times for the first operation of the publish 311 * iteration */ 312 nan_de_set_publish_times(srv); 313 srv->next_publish_duration = 0; 314 } else if (srv->in_multi_chan) { 315 if (!os_reltime_initialized(&srv->pause_state_end)) { 316 srv->multi_chan_idx++; 317 if (srv->freq_list[srv->multi_chan_idx] == 0) 318 srv->multi_chan_idx = 0; 319 srv->freq = srv->freq_list[srv->multi_chan_idx]; 320 wpa_printf(MSG_DEBUG, 321 "NAN: Publish multi-channel change to %u MHz", 322 srv->freq); 323 } 324 os_get_reltime(&srv->next_publish_chan); 325 os_reltime_add_ms(&srv->next_publish_chan, 150); 326 } 327 } 328 329 330 static void nan_de_tx_multicast(struct nan_de *de, struct nan_de_service *srv, 331 u8 req_instance_id) 332 { 333 enum nan_service_control_type type; 334 unsigned int wait_time = 100; 335 336 if (srv->type == NAN_DE_PUBLISH) { 337 int ms; 338 339 type = NAN_SRV_CTRL_PUBLISH; 340 341 nan_de_check_chan_change(srv); 342 ms = nan_de_time_to_next_chan_change(srv); 343 if (ms < 100) 344 ms = 100; 345 wait_time = ms; 346 } else if (srv->type == NAN_DE_SUBSCRIBE) { 347 type = NAN_SRV_CTRL_SUBSCRIBE; 348 } else { 349 return; 350 } 351 352 nan_de_tx_sdf(de, srv, wait_time, type, nan_network_id, 353 req_instance_id, srv->ssi); 354 os_get_reltime(&srv->last_multicast); 355 } 356 357 358 static void nan_de_add_srv(struct nan_de *de, struct nan_de_service *srv) 359 { 360 int ttl; 361 362 os_get_reltime(&srv->time_started); 363 ttl = srv->type == NAN_DE_PUBLISH ? srv->publish.ttl : 364 srv->subscribe.ttl; 365 if (ttl) { 366 srv->end_time = srv->time_started; 367 srv->end_time.sec += ttl; 368 } 369 370 de->service[srv->id - 1] = srv; 371 de->num_service++; 372 } 373 374 375 static void nan_de_del_srv(struct nan_de *de, struct nan_de_service *srv, 376 enum nan_de_reason reason) 377 { 378 de->service[srv->id - 1] = NULL; 379 nan_de_service_deinit(de, srv, reason); 380 de->num_service--; 381 if (de->num_service == 0) 382 nan_de_clear_pending(de); 383 } 384 385 386 static bool nan_de_srv_expired(struct nan_de_service *srv, 387 struct os_reltime *now) 388 { 389 if (os_reltime_initialized(&srv->end_time)) 390 return os_reltime_before(&srv->end_time, now); 391 392 if (srv->type == NAN_DE_PUBLISH) { 393 /* Time out after one transmission (and wait for FSD) */ 394 if (!os_reltime_initialized(&srv->last_multicast)) 395 return false; 396 if (!srv->publish.fsd) 397 return true; 398 if (os_reltime_initialized(&srv->last_followup) && 399 !os_reltime_expired(now, &srv->last_followup, 1)) 400 return false; 401 if (os_reltime_expired(now, &srv->last_multicast, 1)) 402 return true; 403 } 404 405 if (srv->type == NAN_DE_SUBSCRIBE) { 406 /* Time out after first DiscoveryResult event (and wait for 407 * FSD) */ 408 if (!os_reltime_initialized(&srv->first_discovered)) 409 return false; 410 if (!srv->needs_fsd) 411 return true; 412 if (os_reltime_initialized(&srv->last_followup) && 413 !os_reltime_expired(now, &srv->last_followup, 1)) 414 return false; 415 if (os_reltime_expired(now, &srv->first_discovered, 1)) 416 return true; 417 } 418 419 return false; 420 } 421 422 423 static int nan_de_next_multicast(struct nan_de *de, struct nan_de_service *srv, 424 struct os_reltime *now) 425 { 426 unsigned int period; 427 struct os_reltime next, diff; 428 429 if (srv->type == NAN_DE_PUBLISH && !srv->publish.unsolicited) 430 return -1; 431 if (srv->type == NAN_DE_SUBSCRIBE && !srv->subscribe.active) 432 return -1; 433 434 if (!os_reltime_initialized(&srv->last_multicast)) 435 return 0; 436 437 if (srv->type == NAN_DE_PUBLISH && srv->publish.ttl == 0) 438 return -1; 439 440 if (srv->type == NAN_DE_PUBLISH && 441 os_reltime_initialized(&srv->pause_state_end)) 442 return -1; 443 444 period = srv->type == NAN_DE_PUBLISH ? 445 srv->publish.announcement_period : 446 srv->subscribe.query_period; 447 if (period == 0) 448 period = 100; 449 next = srv->last_multicast; 450 os_reltime_add_ms(&next, period); 451 452 if (srv->type == NAN_DE_PUBLISH) { 453 if (!de->tx_wait_end_freq && srv->publish.unsolicited && 454 os_reltime_before(&next, now)) 455 return 0; 456 next = srv->next_publish_state; 457 } 458 459 if (os_reltime_before(&next, now)) 460 return 0; 461 462 os_reltime_sub(&next, now, &diff); 463 return os_reltime_in_ms(&diff); 464 } 465 466 467 static int nan_de_srv_time_to_next(struct nan_de *de, 468 struct nan_de_service *srv, 469 struct os_reltime *now) 470 { 471 struct os_reltime diff; 472 int next = -1, tmp; 473 474 if (os_reltime_initialized(&srv->end_time)) { 475 os_reltime_sub(&srv->end_time, now, &diff); 476 tmp = os_reltime_in_ms(&diff); 477 if (next == -1 || tmp < next) 478 next = tmp; 479 } 480 481 tmp = nan_de_next_multicast(de, srv, now); 482 if (tmp >= 0 && (next == -1 || tmp < next)) 483 next = tmp; 484 485 if (srv->type == NAN_DE_PUBLISH && 486 os_reltime_initialized(&srv->last_multicast)) { 487 /* Time out after one transmission (and wait for FSD) */ 488 tmp = srv->publish.fsd ? 1000 : 100; 489 if (next == -1 || tmp < next) 490 next = tmp; 491 } 492 493 if (srv->type == NAN_DE_SUBSCRIBE && 494 os_reltime_initialized(&srv->first_discovered)) { 495 /* Time out after first DiscoveryResult event (and wait for 496 * FSD) */ 497 tmp = srv->needs_fsd ? 1000 : 100; 498 if (next == -1 || tmp < next) 499 next = tmp; 500 } 501 502 if (os_reltime_initialized(&srv->next_publish_state)) { 503 os_reltime_sub(&srv->next_publish_state, now, &diff); 504 if (diff.sec < 0 || (diff.sec == 0 && diff.usec < 0)) 505 tmp = 0; 506 else 507 tmp = os_reltime_in_ms(&diff); 508 if (next == -1 || tmp < next) 509 next = tmp; 510 } 511 512 return next; 513 } 514 515 516 static void nan_de_start_new_publish_state(struct nan_de_service *srv, 517 bool force_single) 518 { 519 unsigned int n; 520 521 if (force_single || !srv->freq_list || srv->freq_list[0] == 0) 522 srv->in_multi_chan = false; 523 else 524 srv->in_multi_chan = !srv->in_multi_chan; 525 526 /* Use hardcoded Nmin=5 and Nmax=10 and pick a random N from that range. 527 * Use same values for M. */ 528 n = 5 + os_random() % 5; 529 srv->next_publish_duration = n * 100; 530 531 nan_de_set_publish_times(srv); 532 533 if (os_reltime_initialized(&srv->pause_state_end)) 534 return; 535 536 if (srv->in_multi_chan && srv->freq_list && srv->freq_list[0]) { 537 if (!srv->first_multi_chan) 538 srv->multi_chan_idx++; 539 if (srv->freq_list[srv->multi_chan_idx] == 0) 540 srv->multi_chan_idx = 0; 541 srv->first_multi_chan = false; 542 srv->freq = srv->freq_list[srv->multi_chan_idx]; 543 } else { 544 srv->freq = srv->default_freq; 545 } 546 547 wpa_printf(MSG_DEBUG, 548 "NAN: Publish in %s channel state for %u TU; starting with %u MHz", 549 srv->in_multi_chan ? "multi" : "single", n * 100, srv->freq); 550 } 551 552 553 static void nan_de_timer(void *eloop_ctx, void *timeout_ctx) 554 { 555 struct nan_de *de = eloop_ctx; 556 unsigned int i; 557 int next = -1; 558 bool started = false; 559 struct os_reltime now; 560 561 os_get_reltime(&now); 562 563 for (i = 0; i < NAN_DE_MAX_SERVICE; i++) { 564 struct nan_de_service *srv = de->service[i]; 565 int srv_next; 566 567 if (!srv) 568 continue; 569 570 if (nan_de_srv_expired(srv, &now)) { 571 wpa_printf(MSG_DEBUG, "NAN: Service id %d expired", 572 srv->id); 573 nan_de_del_srv(de, srv, NAN_DE_REASON_TIMEOUT); 574 continue; 575 } 576 577 if (os_reltime_initialized(&srv->next_publish_state) && 578 os_reltime_before(&srv->next_publish_state, &now)) 579 nan_de_start_new_publish_state(srv, false); 580 581 if (srv->type == NAN_DE_PUBLISH && 582 os_reltime_initialized(&srv->pause_state_end) && 583 (os_reltime_before(&srv->pause_state_end, &now) || 584 (srv->publish.fsd && 585 os_reltime_initialized(&srv->last_followup) && 586 os_reltime_expired(&now, &srv->last_followup, 1)))) 587 nan_de_unpause_state(srv); 588 589 srv_next = nan_de_srv_time_to_next(de, srv, &now); 590 if (srv_next >= 0 && (next == -1 || srv_next < next)) 591 next = srv_next; 592 593 if (srv_next == 0 && !started && 594 de->listen_freq == 0 && de->ext_listen_freq == 0 && 595 de->tx_wait_end_freq == 0 && 596 nan_de_next_multicast(de, srv, &now) == 0) { 597 started = true; 598 nan_de_tx_multicast(de, srv, 0); 599 } 600 601 if (!started && de->cb.listen && 602 de->listen_freq == 0 && de->ext_listen_freq == 0 && 603 de->tx_wait_end_freq == 0 && 604 ((srv->type == NAN_DE_PUBLISH && 605 !srv->publish.unsolicited && srv->publish.solicited) || 606 (srv->type == NAN_DE_SUBSCRIBE && 607 !srv->subscribe.active))) { 608 int duration = 1000; 609 610 if (srv->type == NAN_DE_PUBLISH) { 611 nan_de_check_chan_change(srv); 612 duration = nan_de_time_to_next_chan_change(srv); 613 if (duration < 150) 614 duration = 150; 615 } 616 617 started = true; 618 if (de->cb.listen(de->cb.ctx, srv->freq, duration) == 0) 619 de->listen_freq = srv->freq; 620 } 621 622 } 623 624 if (next < 0) 625 return; 626 627 if (next == 0) 628 next = 1; 629 wpa_printf(MSG_DEBUG, "NAN: Next timer in %u ms", next); 630 eloop_register_timeout(next / 1000, (next % 1000) * 1000, nan_de_timer, 631 de, NULL); 632 } 633 634 635 static void nan_de_run_timer(struct nan_de *de) 636 { 637 eloop_cancel_timeout(nan_de_timer, de, NULL); 638 eloop_register_timeout(0, 0, nan_de_timer, de, NULL); 639 } 640 641 642 void nan_de_deinit(struct nan_de *de) 643 { 644 eloop_cancel_timeout(nan_de_timer, de, NULL); 645 nan_de_flush(de); 646 os_free(de); 647 } 648 649 650 void nan_de_listen_started(struct nan_de *de, unsigned int freq, 651 unsigned int duration) 652 { 653 if (freq != de->listen_freq) 654 de->ext_listen_freq = freq; 655 } 656 657 658 void nan_de_listen_ended(struct nan_de *de, unsigned int freq) 659 { 660 if (freq == de->ext_listen_freq) 661 de->ext_listen_freq = 0; 662 663 if (freq == de->listen_freq) { 664 de->listen_freq = 0; 665 nan_de_run_timer(de); 666 } 667 } 668 669 670 void nan_de_tx_status(struct nan_de *de, unsigned int freq, const u8 *dst) 671 { 672 if (freq == de->tx_wait_status_freq) 673 de->tx_wait_status_freq = 0; 674 } 675 676 677 void nan_de_tx_wait_ended(struct nan_de *de) 678 { 679 de->tx_wait_end_freq = 0; 680 nan_de_run_timer(de); 681 } 682 683 684 static const u8 * 685 nan_de_get_attr(const u8 *buf, size_t len, enum nan_attr_id id, 686 unsigned int skip) 687 { 688 const u8 *pos = buf, *end = buf + len; 689 690 while (end - pos >= NAN_ATTR_HDR_LEN) { 691 const u8 *attr = pos; 692 u8 attr_id; 693 u16 attr_len; 694 695 attr_id = *pos++; 696 attr_len = WPA_GET_LE16(pos); 697 pos += 2; 698 if (attr_len > end - pos) { 699 wpa_printf(MSG_DEBUG, 700 "NAN: Truncated attribute %u (len %u; left %zu)", 701 attr_id, attr_len, end - pos); 702 break; 703 } 704 705 if (attr_id == id) { 706 if (skip == 0) 707 return attr; 708 skip--; 709 } 710 711 pos += attr_len; 712 } 713 714 return NULL; 715 } 716 717 718 static void nan_de_get_sdea(const u8 *buf, size_t len, u8 instance_id, 719 u16 *sdea_control, 720 enum nan_service_protocol_type *srv_proto_type, 721 const u8 **ssi, size_t *ssi_len) 722 { 723 unsigned int skip; 724 const u8 *sdea, *end; 725 u16 sdea_len; 726 727 for (skip = 0; ; skip++) { 728 sdea = nan_de_get_attr(buf, len, NAN_ATTR_SDEA, skip); 729 if (!sdea) 730 break; 731 732 sdea++; 733 sdea_len = WPA_GET_LE16(sdea); 734 sdea += 2; 735 if (sdea_len < 1 + 2) 736 continue; 737 end = sdea + sdea_len; 738 739 if (instance_id != *sdea++) 740 continue; /* Mismatching Instance ID */ 741 742 *sdea_control = WPA_GET_LE16(sdea); 743 sdea += 2; 744 745 if (*sdea_control & NAN_SDEA_CTRL_RANGE_LIMIT) { 746 if (end - sdea < 4) 747 continue; 748 sdea += 4; 749 } 750 751 if (*sdea_control & NAN_SDEA_CTRL_SRV_UPD_INDIC) { 752 if (end - sdea < 1) 753 continue; 754 sdea++; 755 } 756 757 if (end - sdea >= 2) { 758 u16 srv_info_len; 759 760 srv_info_len = WPA_GET_LE16(sdea); 761 sdea += 2; 762 763 if (srv_info_len > end - sdea) 764 continue; 765 766 if (srv_info_len >= 4 && 767 WPA_GET_BE24(sdea) == OUI_WFA) { 768 *srv_proto_type = sdea[3]; 769 *ssi = sdea + 4; 770 *ssi_len = srv_info_len - 4; 771 } 772 } 773 } 774 } 775 776 777 static void nan_de_rx_publish(struct nan_de *de, struct nan_de_service *srv, 778 const u8 *peer_addr, u8 instance_id, 779 u8 req_instance_id, u16 sdea_control, 780 enum nan_service_protocol_type srv_proto_type, 781 const u8 *ssi, size_t ssi_len) 782 { 783 /* Subscribe function processing of a receive Publish message */ 784 if (!os_reltime_initialized(&srv->first_discovered)) { 785 os_get_reltime(&srv->first_discovered); 786 srv->needs_fsd = sdea_control & NAN_SDEA_CTRL_FSD_REQ; 787 nan_de_run_timer(de); 788 } 789 790 if (srv->subscribe.active && req_instance_id == 0) { 791 /* Active subscriber replies with a Subscribe message if it 792 * received a matching unsolicited Publish message. */ 793 nan_de_tx_multicast(de, srv, instance_id); 794 } 795 796 if (!srv->subscribe.active && req_instance_id == 0) { 797 /* Passive subscriber replies with a Follow-up message without 798 * Service Specific Info field if it received a matching 799 * unsolicited Publish message. */ 800 nan_de_transmit(de, srv->id, NULL, NULL, peer_addr, 801 instance_id); 802 } 803 804 if (de->cb.discovery_result) 805 de->cb.discovery_result( 806 de->cb.ctx, srv->id, srv_proto_type, 807 ssi, ssi_len, instance_id, 808 peer_addr, 809 sdea_control & NAN_SDEA_CTRL_FSD_REQ, 810 sdea_control & NAN_SDEA_CTRL_FSD_GAS); 811 } 812 813 814 static bool nan_de_filter_match(struct nan_de_service *srv, 815 const u8 *matching_filter, 816 size_t matching_filter_len) 817 { 818 const u8 *pos, *end; 819 820 /* Since we do not currently support matching_filter_rx values for the 821 * local Publish function, any matching filter with at least one 822 * <length,value> pair with length larger than zero implies a mismatch. 823 */ 824 825 if (!matching_filter) 826 return true; 827 828 pos = matching_filter; 829 end = matching_filter + matching_filter_len; 830 831 while (pos < end) { 832 u8 len; 833 834 len = *pos++; 835 if (len > end - pos) 836 break; 837 if (len) { 838 /* A non-empty Matching Filter entry: no match since 839 * there is no local matching_filter_rx. */ 840 return false; 841 } 842 } 843 844 return true; 845 } 846 847 848 static void nan_de_rx_subscribe(struct nan_de *de, struct nan_de_service *srv, 849 const u8 *peer_addr, u8 instance_id, 850 const u8 *matching_filter, 851 size_t matching_filter_len, 852 enum nan_service_protocol_type srv_proto_type, 853 const u8 *ssi, size_t ssi_len) 854 { 855 struct wpabuf *buf; 856 size_t len = 0, sda_len, sdea_len; 857 u8 ctrl = 0; 858 u16 sdea_ctrl = 0; 859 860 /* Publish function processing of a receive Subscribe message */ 861 862 if (!nan_de_filter_match(srv, matching_filter, matching_filter_len)) 863 return; 864 865 if (!srv->publish.solicited) 866 return; 867 868 if (os_reltime_initialized(&srv->pause_state_end) && 869 (!ether_addr_equal(peer_addr, srv->sel_peer_addr) || 870 instance_id != srv->sel_peer_id)) { 871 wpa_printf(MSG_DEBUG, 872 "NAN: In pauseState - ignore Subscribe message from another subscriber"); 873 return; 874 } 875 876 /* Reply with a solicited Publish message */ 877 /* Service Descriptor attribute */ 878 sda_len = NAN_SERVICE_ID_LEN + 1 + 1 + 1; 879 len += NAN_ATTR_HDR_LEN + sda_len; 880 881 /* Service Descriptor Extension attribute */ 882 sdea_len = 1 + 2; 883 if (srv->ssi) 884 sdea_len += 2 + 4 + wpabuf_len(srv->ssi); 885 len += NAN_ATTR_HDR_LEN + sdea_len; 886 887 /* Element Container attribute */ 888 if (srv->elems) 889 len += NAN_ATTR_HDR_LEN + 1 + wpabuf_len(srv->elems); 890 891 buf = nan_de_alloc_sdf(len); 892 if (!buf) 893 return; 894 895 /* Service Descriptor attribute */ 896 wpabuf_put_u8(buf, NAN_ATTR_SDA); 897 wpabuf_put_le16(buf, sda_len); 898 wpabuf_put_data(buf, srv->service_id, NAN_SERVICE_ID_LEN); 899 wpabuf_put_u8(buf, srv->id); /* Instance ID */ 900 wpabuf_put_u8(buf, instance_id); /* Requestor Instance ID */ 901 ctrl |= NAN_SRV_CTRL_PUBLISH; 902 wpabuf_put_u8(buf, ctrl); 903 904 /* Service Descriptor Extension attribute */ 905 if (srv->type == NAN_DE_PUBLISH || srv->ssi) { 906 wpabuf_put_u8(buf, NAN_ATTR_SDEA); 907 wpabuf_put_le16(buf, sdea_len); 908 wpabuf_put_u8(buf, srv->id); /* Instance ID */ 909 if (srv->type == NAN_DE_PUBLISH) { 910 if (srv->publish.fsd) 911 sdea_ctrl |= NAN_SDEA_CTRL_FSD_REQ; 912 if (srv->publish.fsd_gas) 913 sdea_ctrl |= NAN_SDEA_CTRL_FSD_GAS; 914 } 915 wpabuf_put_le16(buf, sdea_ctrl); 916 if (srv->ssi) { 917 wpabuf_put_le16(buf, 4 + wpabuf_len(srv->ssi)); 918 wpabuf_put_be24(buf, OUI_WFA); 919 wpabuf_put_u8(buf, srv->srv_proto_type); 920 wpabuf_put_buf(buf, srv->ssi); 921 } 922 } 923 924 /* Element Container attribute */ 925 if (srv->elems) { 926 wpabuf_put_u8(buf, NAN_ATTR_ELEM_CONTAINER); 927 wpabuf_put_le16(buf, 1 + wpabuf_len(srv->elems)); 928 wpabuf_put_u8(buf, 0); /* Map ID */ 929 wpabuf_put_buf(buf, srv->elems); 930 } 931 932 /* Wi-Fi Aware specification v4.0 uses NAN Cluster ID as A3 for USD, 933 * but there is no synchronization in USD as as such, no NAN Cluster 934 * either. Use Wildcard BSSID instead. */ 935 nan_de_tx(de, srv->freq, 100, 936 srv->publish.solicited_multicast ? nan_network_id : peer_addr, 937 de->nmi, wildcard_bssid, buf); 938 wpabuf_free(buf); 939 940 nan_de_pause_state(srv, peer_addr, instance_id); 941 942 if (!srv->publish.disable_events && de->cb.replied) 943 de->cb.replied(de->cb.ctx, srv->id, peer_addr, instance_id, 944 srv_proto_type, ssi, ssi_len); 945 } 946 947 948 static void nan_de_rx_follow_up(struct nan_de *de, struct nan_de_service *srv, 949 const u8 *peer_addr, u8 instance_id, 950 const u8 *ssi, size_t ssi_len) 951 { 952 /* Follow-up function processing of a receive Follow-up message for a 953 * Subscribe or Publish instance */ 954 955 if (srv->type == NAN_DE_PUBLISH && 956 os_reltime_initialized(&srv->pause_state_end) && 957 (!ether_addr_equal(peer_addr, srv->sel_peer_addr) || 958 instance_id != srv->sel_peer_id || 959 !ssi)) { 960 wpa_printf(MSG_DEBUG, 961 "NAN: In pauseState - ignore Follow-up message from another subscriber or without ssi"); 962 return; 963 } 964 965 os_get_reltime(&srv->last_followup); 966 967 if (srv->type == NAN_DE_PUBLISH && !ssi) 968 nan_de_pause_state(srv, peer_addr, instance_id); 969 970 if (de->cb.receive) 971 de->cb.receive(de->cb.ctx, srv->id, instance_id, ssi, ssi_len, 972 peer_addr); 973 } 974 975 976 static void nan_de_rx_sda(struct nan_de *de, const u8 *peer_addr, 977 unsigned int freq, const u8 *buf, size_t len, 978 const u8 *sda, size_t sda_len) 979 { 980 const u8 *service_id; 981 u8 instance_id, req_instance_id, ctrl; 982 u16 sdea_control = 0; 983 unsigned int i; 984 enum nan_service_control_type type = 0; 985 enum nan_service_protocol_type srv_proto_type = 0; 986 const u8 *ssi = NULL; 987 size_t ssi_len = 0; 988 bool first = true; 989 const u8 *end; 990 const u8 *matching_filter = NULL; 991 size_t matching_filter_len = 0; 992 993 if (sda_len < NAN_SERVICE_ID_LEN + 1 + 1 + 1) 994 return; 995 end = sda + sda_len; 996 997 service_id = sda; 998 sda += NAN_SERVICE_ID_LEN; 999 instance_id = *sda++; 1000 req_instance_id = *sda++; 1001 ctrl = *sda; 1002 type = ctrl & NAN_SRV_CTRL_TYPE_MASK; 1003 wpa_printf(MSG_DEBUG, 1004 "NAN: SDA - Service ID %02x%02x%02x%02x%02x%02x Instance ID %u Requestor Instance ID %u Service Control 0x%x (Service Control Type %u)", 1005 MAC2STR(service_id), instance_id, req_instance_id, 1006 ctrl, type); 1007 if (type != NAN_SRV_CTRL_PUBLISH && 1008 type != NAN_SRV_CTRL_SUBSCRIBE && 1009 type != NAN_SRV_CTRL_FOLLOW_UP) { 1010 wpa_printf(MSG_DEBUG, 1011 "NAN: Discard SDF with unknown Service Control Type %u", 1012 type); 1013 return; 1014 } 1015 1016 if (ctrl & NAN_SRV_CTRL_BINDING_BITMAP) { 1017 if (end - sda < 2) 1018 return; 1019 sda += 2; 1020 } 1021 1022 if (ctrl & NAN_SRV_CTRL_MATCHING_FILTER) { 1023 u8 flen; 1024 1025 if (end - sda < 1) 1026 return; 1027 flen = *sda++; 1028 if (end - sda < flen) 1029 return; 1030 matching_filter = sda; 1031 matching_filter_len = flen; 1032 sda += flen; 1033 } 1034 1035 if (ctrl & NAN_SRV_CTRL_RESP_FILTER) { 1036 u8 flen; 1037 1038 if (end - sda < 1) 1039 return; 1040 flen = *sda++; 1041 if (end - sda < flen) 1042 return; 1043 sda += flen; 1044 } 1045 1046 if (ctrl & NAN_SRV_CTRL_SRV_INFO) { 1047 u8 flen; 1048 1049 if (end - sda < 1) 1050 return; 1051 flen = *sda++; 1052 if (end - sda < flen) 1053 return; 1054 if (flen >= 4 && WPA_GET_BE24(sda) == OUI_WFA) { 1055 srv_proto_type = sda[3]; 1056 ssi = sda + 4; 1057 ssi_len = flen - 4; 1058 wpa_printf(MSG_DEBUG, "NAN: Service Protocol Type %d", 1059 srv_proto_type); 1060 wpa_hexdump(MSG_MSGDUMP, "NAN: ssi", ssi, ssi_len); 1061 } 1062 sda += flen; 1063 } 1064 1065 for (i = 0; i < NAN_DE_MAX_SERVICE; i++) { 1066 struct nan_de_service *srv = de->service[i]; 1067 1068 if (!srv) 1069 continue; 1070 if (os_memcmp(srv->service_id, service_id, 1071 NAN_SERVICE_ID_LEN) != 0) 1072 continue; 1073 if (type == NAN_SRV_CTRL_PUBLISH) { 1074 if (srv->type == NAN_DE_PUBLISH) 1075 continue; 1076 if (req_instance_id && srv->id != req_instance_id) 1077 continue; 1078 } 1079 if (type == NAN_SRV_CTRL_SUBSCRIBE && 1080 srv->type == NAN_DE_SUBSCRIBE) 1081 continue; 1082 wpa_printf(MSG_DEBUG, "NAN: Received SDF matches service ID %u", 1083 i + 1); 1084 1085 if (first) { 1086 first = false; 1087 nan_de_get_sdea(buf, len, instance_id, &sdea_control, 1088 &srv_proto_type, &ssi, &ssi_len); 1089 1090 if (ssi) { 1091 wpa_printf(MSG_DEBUG, 1092 "NAN: Service Protocol Type %d", 1093 srv_proto_type); 1094 wpa_hexdump(MSG_MSGDUMP, "NAN: ssi", 1095 ssi, ssi_len); 1096 } 1097 } 1098 1099 switch (type) { 1100 case NAN_SRV_CTRL_PUBLISH: 1101 nan_de_rx_publish(de, srv, peer_addr, instance_id, 1102 req_instance_id, 1103 sdea_control, srv_proto_type, 1104 ssi, ssi_len); 1105 break; 1106 case NAN_SRV_CTRL_SUBSCRIBE: 1107 nan_de_rx_subscribe(de, srv, peer_addr, instance_id, 1108 matching_filter, 1109 matching_filter_len, 1110 srv_proto_type, 1111 ssi, ssi_len); 1112 break; 1113 case NAN_SRV_CTRL_FOLLOW_UP: 1114 nan_de_rx_follow_up(de, srv, peer_addr, instance_id, 1115 ssi, ssi_len); 1116 break; 1117 } 1118 } 1119 } 1120 1121 1122 void nan_de_rx_sdf(struct nan_de *de, const u8 *peer_addr, unsigned int freq, 1123 const u8 *buf, size_t len) 1124 { 1125 const u8 *sda; 1126 u16 sda_len; 1127 unsigned int skip; 1128 1129 if (!de->num_service) 1130 return; 1131 1132 wpa_printf(MSG_DEBUG, "NAN: RX SDF from " MACSTR " freq=%u len=%zu", 1133 MAC2STR(peer_addr), freq, len); 1134 1135 wpa_hexdump(MSG_MSGDUMP, "NAN: SDF payload", buf, len); 1136 1137 for (skip = 0; ; skip++) { 1138 sda = nan_de_get_attr(buf, len, NAN_ATTR_SDA, skip); 1139 if (!sda) 1140 break; 1141 1142 sda++; 1143 sda_len = WPA_GET_LE16(sda); 1144 sda += 2; 1145 nan_de_rx_sda(de, peer_addr, freq, buf, len, sda, sda_len); 1146 } 1147 } 1148 1149 1150 static int nan_de_get_handle(struct nan_de *de) 1151 { 1152 int i = de->next_handle; 1153 1154 if (de->num_service >= NAN_DE_MAX_SERVICE) 1155 goto fail; 1156 1157 do { 1158 if (!de->service[i]) { 1159 de->next_handle = (i + 1) % NAN_DE_MAX_SERVICE; 1160 return i + 1; 1161 } 1162 i = (i + 1) % NAN_DE_MAX_SERVICE; 1163 } while (i != de->next_handle); 1164 1165 fail: 1166 wpa_printf(MSG_DEBUG, "NAN: No more room for a new service"); 1167 return -1; 1168 } 1169 1170 1171 static int nan_de_derive_service_id(struct nan_de_service *srv) 1172 { 1173 u8 hash[SHA256_MAC_LEN]; 1174 char *name, *pos; 1175 int ret; 1176 const u8 *addr[1]; 1177 size_t len[1]; 1178 1179 name = os_strdup(srv->service_name); 1180 if (!name) 1181 return -1; 1182 pos = name; 1183 while (*pos) { 1184 *pos = tolower(*pos); 1185 pos++; 1186 } 1187 1188 addr[0] = (u8 *) name; 1189 len[0] = os_strlen(name); 1190 ret = sha256_vector(1, addr, len, hash); 1191 os_free(name); 1192 if (ret == 0) 1193 os_memcpy(srv->service_id, hash, NAN_SERVICE_ID_LEN); 1194 1195 return ret; 1196 } 1197 1198 1199 int nan_de_publish(struct nan_de *de, const char *service_name, 1200 enum nan_service_protocol_type srv_proto_type, 1201 const struct wpabuf *ssi, const struct wpabuf *elems, 1202 struct nan_publish_params *params) 1203 { 1204 int publish_id; 1205 struct nan_de_service *srv; 1206 1207 if (!service_name) { 1208 wpa_printf(MSG_DEBUG, "NAN: Publish() - no service_name"); 1209 return -1; 1210 } 1211 1212 if (!params->unsolicited && !params->solicited) { 1213 wpa_printf(MSG_INFO, 1214 "NAN: Publish() - both unsolicited and solicited disabled is invalid"); 1215 return -1; 1216 } 1217 1218 publish_id = nan_de_get_handle(de); 1219 if (publish_id < 1) 1220 return -1; 1221 1222 srv = os_zalloc(sizeof(*srv)); 1223 if (!srv) 1224 return -1; 1225 srv->type = NAN_DE_PUBLISH; 1226 srv->freq = srv->default_freq = params->freq; 1227 srv->service_name = os_strdup(service_name); 1228 if (!srv->service_name) 1229 goto fail; 1230 if (nan_de_derive_service_id(srv) < 0) 1231 goto fail; 1232 os_memcpy(&srv->publish, params, sizeof(*params)); 1233 1234 if (params->freq_list) { 1235 size_t len; 1236 1237 len = (int_array_len(params->freq_list) + 1) * sizeof(int); 1238 srv->freq_list = os_memdup(params->freq_list, len); 1239 if (!srv->freq_list) 1240 goto fail; 1241 } 1242 srv->publish.freq_list = NULL; 1243 1244 srv->srv_proto_type = srv_proto_type; 1245 if (ssi) { 1246 srv->ssi = wpabuf_dup(ssi); 1247 if (!srv->ssi) 1248 goto fail; 1249 } 1250 if (elems) { 1251 srv->elems = wpabuf_dup(elems); 1252 if (!srv->elems) 1253 goto fail; 1254 } 1255 1256 /* Prepare for single and multi-channel states; starting with 1257 * single channel */ 1258 srv->first_multi_chan = true; 1259 nan_de_start_new_publish_state(srv, true); 1260 1261 wpa_printf(MSG_DEBUG, "NAN: Assigned new publish handle %d for %s", 1262 publish_id, service_name); 1263 srv->id = publish_id; 1264 nan_de_add_srv(de, srv); 1265 nan_de_run_timer(de); 1266 return publish_id; 1267 fail: 1268 nan_de_service_free(srv); 1269 return -1; 1270 } 1271 1272 1273 void nan_de_cancel_publish(struct nan_de *de, int publish_id) 1274 { 1275 struct nan_de_service *srv; 1276 1277 wpa_printf(MSG_DEBUG, "NAN: CancelPublish(publish_id=%d)", publish_id); 1278 1279 if (publish_id < 1 || publish_id > NAN_DE_MAX_SERVICE) 1280 return; 1281 srv = de->service[publish_id - 1]; 1282 if (!srv || srv->type != NAN_DE_PUBLISH) 1283 return; 1284 nan_de_del_srv(de, srv, NAN_DE_REASON_USER_REQUEST); 1285 } 1286 1287 1288 int nan_de_update_publish(struct nan_de *de, int publish_id, 1289 const struct wpabuf *ssi) 1290 { 1291 struct nan_de_service *srv; 1292 1293 wpa_printf(MSG_DEBUG, "NAN: UpdatePublish(publish_id=%d)", publish_id); 1294 1295 if (publish_id < 1 || publish_id > NAN_DE_MAX_SERVICE) 1296 return -1; 1297 srv = de->service[publish_id - 1]; 1298 if (!srv || srv->type != NAN_DE_PUBLISH) 1299 return -1; 1300 1301 wpabuf_free(srv->ssi); 1302 srv->ssi = NULL; 1303 if (!ssi) 1304 return 0; 1305 srv->ssi = wpabuf_dup(ssi); 1306 if (!srv->ssi) 1307 return -1; 1308 return 0; 1309 } 1310 1311 1312 int nan_de_subscribe(struct nan_de *de, const char *service_name, 1313 enum nan_service_protocol_type srv_proto_type, 1314 const struct wpabuf *ssi, const struct wpabuf *elems, 1315 struct nan_subscribe_params *params) 1316 { 1317 int subscribe_id; 1318 struct nan_de_service *srv; 1319 1320 if (!service_name) { 1321 wpa_printf(MSG_DEBUG, "NAN: Subscribe() - no service_name"); 1322 return -1; 1323 } 1324 1325 subscribe_id = nan_de_get_handle(de); 1326 if (subscribe_id < 1) 1327 return -1; 1328 1329 srv = os_zalloc(sizeof(*srv)); 1330 if (!srv) 1331 return -1; 1332 srv->type = NAN_DE_SUBSCRIBE; 1333 srv->freq = params->freq; 1334 srv->service_name = os_strdup(service_name); 1335 if (!srv->service_name) 1336 goto fail; 1337 if (nan_de_derive_service_id(srv) < 0) 1338 goto fail; 1339 os_memcpy(&srv->subscribe, params, sizeof(*params)); 1340 srv->srv_proto_type = srv_proto_type; 1341 if (ssi) { 1342 srv->ssi = wpabuf_dup(ssi); 1343 if (!srv->ssi) 1344 goto fail; 1345 } 1346 if (elems) { 1347 srv->elems = wpabuf_dup(elems); 1348 if (!srv->elems) 1349 goto fail; 1350 } 1351 1352 wpa_printf(MSG_DEBUG, "NAN: Assigned new subscribe handle %d for %s", 1353 subscribe_id, service_name); 1354 srv->id = subscribe_id; 1355 nan_de_add_srv(de, srv); 1356 nan_de_run_timer(de); 1357 return subscribe_id; 1358 fail: 1359 nan_de_service_free(srv); 1360 return -1; 1361 } 1362 1363 1364 void nan_de_cancel_subscribe(struct nan_de *de, int subscribe_id) 1365 { 1366 struct nan_de_service *srv; 1367 1368 if (subscribe_id < 1 || subscribe_id > NAN_DE_MAX_SERVICE) 1369 return; 1370 srv = de->service[subscribe_id - 1]; 1371 if (!srv || srv->type != NAN_DE_SUBSCRIBE) 1372 return; 1373 nan_de_del_srv(de, srv, NAN_DE_REASON_USER_REQUEST); 1374 } 1375 1376 1377 int nan_de_transmit(struct nan_de *de, int handle, 1378 const struct wpabuf *ssi, const struct wpabuf *elems, 1379 const u8 *peer_addr, u8 req_instance_id) 1380 { 1381 struct nan_de_service *srv; 1382 1383 if (handle < 1 || handle > NAN_DE_MAX_SERVICE) 1384 return -1; 1385 1386 srv = de->service[handle - 1]; 1387 if (!srv) 1388 return -1; 1389 1390 nan_de_tx_sdf(de, srv, 100, NAN_SRV_CTRL_FOLLOW_UP, 1391 peer_addr, req_instance_id, ssi); 1392 1393 os_get_reltime(&srv->last_followup); 1394 return 0; 1395 } 1396
Indexes created Fri Jun 26 00:24:39 UTC 2026