1 /* 2 * 3 * MCAP for BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2010 GSyC/LibreSoft, Universidad Rey Juan Carlos. 6 * Copyright (C) 2010 Signove 7 * 8 * Authors: 9 * Santiago Carot-Nemesio <sancane at gmail.com> 10 * Jose Antonio Santos-Cadenas <santoscadenas at gmail.com> 11 * Elvis Pftzenreuter <epx at signove.com> 12 * 13 * This program is free software; you can redistribute it and/or modify 14 * it under the terms of the GNU General Public License as published by 15 * the Free Software Foundation; either version 2 of the License, or 16 * (at your option) any later version. 17 * 18 * This program is distributed in the hope that it will be useful, 19 * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 * GNU General Public License for more details. 22 * 23 * You should have received a copy of the GNU General Public License 24 * along with this program; if not, write to the Free Software 25 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 26 * 27 */ 28 29 #include "btio.h" 30 #include <stdint.h> 31 #include <netinet/in.h> 32 #include <time.h> 33 #include <stdlib.h> 34 #include <bluetooth/bluetooth.h> 35 #include <bluetooth/l2cap.h> 36 #include "../src/adapter.h" 37 #include "../src/manager.h" 38 #include <sys/ioctl.h> 39 40 #include "config.h" 41 #include "log.h" 42 43 #include <bluetooth/bluetooth.h> 44 #include "mcap.h" 45 #include "mcap_lib.h" 46 #include "mcap_internal.h" 47 48 #define MCAP_BTCLOCK_HALF (MCAP_BTCLOCK_FIELD / 2) 49 #define CLK CLOCK_MONOTONIC 50 51 #define MCAP_CSP_ERROR g_quark_from_static_string("mcap-csp-error-quark") 52 #define MAX_RETRIES 10 53 #define SAMPLE_COUNT 20 54 55 struct mcap_csp { 56 uint64_t base_tmstamp; /* CSP base timestamp */ 57 struct timespec base_time; /* CSP base time when timestamp set */ 58 guint local_caps; /* CSP-Master: have got remote caps */ 59 guint remote_caps; /* CSP-Slave: remote master got caps */ 60 guint rem_req_acc; /* CSP-Slave: accuracy required by master */ 61 guint ind_expected; /* CSP-Master: indication expected */ 62 MCAPCtrl csp_req; /* CSP-Master: Request control flag */ 63 guint ind_timer; /* CSP-Slave: indication timer */ 64 guint set_timer; /* CSP-Slave: delayed set timer */ 65 void *set_data; /* CSP-Slave: delayed set data */ 66 void *csp_priv_data; /* CSP-Master: In-flight request data */ 67 }; 68 69 struct mcap_sync_cap_cbdata { 70 mcap_sync_cap_cb cb; 71 gpointer user_data; 72 }; 73 74 struct mcap_sync_set_cbdata { 75 mcap_sync_set_cb cb; 76 gpointer user_data; 77 }; 78 79 struct csp_caps { 80 int ts_acc; /* timestamp accuracy */ 81 int ts_res; /* timestamp resolution */ 82 int latency; /* Read BT clock latency */ 83 int preempt_thresh; /* Preemption threshold for latency */ 84 int syncleadtime_ms; /* SyncLeadTime in ms */ 85 }; 86 87 struct sync_set_data { 88 uint8_t update; 89 uint32_t sched_btclock; 90 uint64_t timestamp; 91 int ind_freq; 92 gboolean role; 93 }; 94 95 #define hton64(x) ntoh64(x) 96 97 static gboolean csp_caps_initialized = FALSE; 98 struct csp_caps _caps; 99 100 static int send_sync_cmd(struct mcap_mcl *mcl, const void *buf, uint32_t size) 101 { 102 int sock; 103 104 if (mcl->cc == NULL) 105 return -1; 106 107 sock = g_io_channel_unix_get_fd(mcl->cc); 108 return mcap_send_data(sock, buf, size); 109 } 110 111 static int send_unsupported_cap_req(struct mcap_mcl *mcl) 112 { 113 mcap_md_sync_cap_rsp *cmd; 114 int sent; 115 116 cmd = g_new0(mcap_md_sync_cap_rsp, 1); 117 cmd->op = MCAP_MD_SYNC_CAP_RSP; 118 cmd->rc = MCAP_REQUEST_NOT_SUPPORTED; 119 120 sent = send_sync_cmd(mcl, cmd, sizeof(*cmd)); 121 g_free(cmd); 122 123 return sent; 124 } 125 126 static int send_unsupported_set_req(struct mcap_mcl *mcl) 127 { 128 mcap_md_sync_set_rsp *cmd; 129 int sent; 130 131 cmd = g_new0(mcap_md_sync_set_rsp, 1); 132 cmd->op = MCAP_MD_SYNC_SET_RSP; 133 cmd->rc = MCAP_REQUEST_NOT_SUPPORTED; 134 135 sent = send_sync_cmd(mcl, cmd, sizeof(*cmd)); 136 g_free(cmd); 137 138 return sent; 139 } 140 141 static void reset_tmstamp(struct mcap_csp *csp, struct timespec *base_time, 142 uint64_t new_tmstamp) 143 { 144 csp->base_tmstamp = new_tmstamp; 145 if (base_time) 146 csp->base_time = *base_time; 147 else 148 clock_gettime(CLK, &csp->base_time); 149 } 150 151 void mcap_sync_init(struct mcap_mcl *mcl) 152 { 153 if (!mcl->mi->csp_enabled) { 154 mcl->csp = NULL; 155 return; 156 } 157 158 mcl->csp = g_new0(struct mcap_csp, 1); 159 160 mcl->csp->rem_req_acc = 10000; /* safe divisor */ 161 mcl->csp->set_data = NULL; 162 mcl->csp->csp_priv_data = NULL; 163 164 reset_tmstamp(mcl->csp, NULL, 0); 165 } 166 167 void mcap_sync_stop(struct mcap_mcl *mcl) 168 { 169 if (!mcl->csp) 170 return; 171 172 if (mcl->csp->ind_timer) 173 g_source_remove(mcl->csp->ind_timer); 174 175 if (mcl->csp->set_timer) 176 g_source_remove(mcl->csp->set_timer); 177 178 if (mcl->csp->set_data) 179 g_free(mcl->csp->set_data); 180 181 if (mcl->csp->csp_priv_data) 182 g_free(mcl->csp->csp_priv_data); 183 184 mcl->csp->ind_timer = 0; 185 mcl->csp->set_timer = 0; 186 mcl->csp->set_data = NULL; 187 mcl->csp->csp_priv_data = NULL; 188 189 g_free(mcl->csp); 190 mcl->csp = NULL; 191 } 192 193 static uint64_t time_us(struct timespec *tv) 194 { 195 return tv->tv_sec * 1000000 + tv->tv_nsec / 1000; 196 } 197 198 static int64_t bt2us(int bt) 199 { 200 return bt * 312.5; 201 } 202 203 static int bt2ms(int bt) 204 { 205 return bt * 312.5 / 1000; 206 } 207 208 static int btoffset(uint32_t btclk1, uint32_t btclk2) 209 { 210 int offset = btclk2 - btclk1; 211 212 if (offset <= -MCAP_BTCLOCK_HALF) 213 offset += MCAP_BTCLOCK_FIELD; 214 else if (offset > MCAP_BTCLOCK_HALF) 215 offset -= MCAP_BTCLOCK_FIELD; 216 217 return offset; 218 } 219 220 static int btdiff(uint32_t btclk1, uint32_t btclk2) 221 { 222 return btoffset(btclk1, btclk2); 223 } 224 225 static gboolean valid_btclock(uint32_t btclk) 226 { 227 return btclk <= MCAP_BTCLOCK_MAX; 228 } 229 230 /* This call may fail; either deal with retry or use read_btclock_retry */ 231 static gboolean read_btclock(struct mcap_mcl *mcl, uint32_t *btclock, 232 uint16_t *btaccuracy) 233 { 234 int which = 1; 235 struct btd_adapter *adapter; 236 237 adapter = manager_find_adapter(&mcl->mi->src); 238 239 if (!adapter) 240 return FALSE; 241 242 if (btd_adapter_read_clock(adapter, &mcl->addr, which, 1000, 243 btclock, btaccuracy) < 0) 244 return FALSE; 245 246 return TRUE; 247 } 248 249 static gboolean read_btclock_retry(struct mcap_mcl *mcl, uint32_t *btclock, 250 uint16_t *btaccuracy) 251 { 252 int retries = 5; 253 254 while (--retries >= 0) { 255 if (read_btclock(mcl, btclock, btaccuracy)) 256 return TRUE; 257 DBG("CSP: retrying to read bt clock..."); 258 } 259 260 return FALSE; 261 } 262 263 static gboolean get_btrole(struct mcap_mcl *mcl) 264 { 265 int sock, flags; 266 socklen_t len; 267 268 if (mcl->cc == NULL) 269 return -1; 270 271 sock = g_io_channel_unix_get_fd(mcl->cc); 272 len = sizeof(flags); 273 274 if (getsockopt(sock, SOL_L2CAP, L2CAP_LM, &flags, &len)) 275 DBG("CSP: could not read role"); 276 277 return flags & L2CAP_LM_MASTER; 278 } 279 280 uint64_t mcap_get_timestamp(struct mcap_mcl *mcl, 281 struct timespec *given_time) 282 { 283 struct timespec now; 284 uint64_t tmstamp; 285 286 if (!mcl->csp) 287 return MCAP_TMSTAMP_DONTSET; 288 289 if (given_time) 290 now = *given_time; 291 else 292 clock_gettime(CLK, &now); 293 294 tmstamp = time_us(&now) - time_us(&mcl->csp->base_time) 295 + mcl->csp->base_tmstamp; 296 297 return tmstamp; 298 } 299 300 uint32_t mcap_get_btclock(struct mcap_mcl *mcl) 301 { 302 uint32_t btclock; 303 uint16_t accuracy; 304 305 if (!mcl->csp) 306 return MCAP_BTCLOCK_IMMEDIATE; 307 308 if (!read_btclock_retry(mcl, &btclock, &accuracy)) 309 btclock = 0xffffffff; 310 311 return btclock; 312 } 313 314 static gboolean initialize_caps(struct mcap_mcl *mcl) 315 { 316 struct timespec t1, t2; 317 int latencies[SAMPLE_COUNT]; 318 int latency, avg, dev; 319 uint32_t btclock; 320 uint16_t btaccuracy; 321 int i; 322 int retries; 323 324 clock_getres(CLK, &t1); 325 326 _caps.ts_res = time_us(&t1); 327 if (_caps.ts_res < 1) 328 _caps.ts_res = 1; 329 330 _caps.ts_acc = 20; /* ppm, estimated */ 331 332 /* A little exercise before measuing latency */ 333 clock_gettime(CLK, &t1); 334 read_btclock_retry(mcl, &btclock, &btaccuracy); 335 336 /* Read clock a number of times and measure latency */ 337 avg = 0; 338 i = 0; 339 retries = MAX_RETRIES; 340 while (i < SAMPLE_COUNT && retries > 0) { 341 clock_gettime(CLK, &t1); 342 if (!read_btclock(mcl, &btclock, &btaccuracy)) { 343 retries--; 344 continue; 345 } 346 clock_gettime(CLK, &t2); 347 348 latency = time_us(&t2) - time_us(&t1); 349 latencies[i] = latency; 350 avg += latency; 351 i++; 352 } 353 354 if (retries <= 0) 355 return FALSE; 356 357 /* Calculate average and deviation */ 358 avg /= SAMPLE_COUNT; 359 dev = 0; 360 for (i = 0; i < SAMPLE_COUNT; ++i) 361 dev += abs(latencies[i] - avg); 362 dev /= SAMPLE_COUNT; 363 364 /* Calculate corrected average, without 'freak' latencies */ 365 latency = 0; 366 for (i = 0; i < SAMPLE_COUNT; ++i) { 367 if (latencies[i] > (avg + dev * 6)) 368 latency += avg; 369 else 370 latency += latencies[i]; 371 } 372 latency /= SAMPLE_COUNT; 373 374 _caps.latency = latency; 375 _caps.preempt_thresh = latency * 4; 376 _caps.syncleadtime_ms = latency * 50 / 1000; 377 378 csp_caps_initialized = TRUE; 379 return TRUE; 380 } 381 382 static struct csp_caps *caps(struct mcap_mcl *mcl) 383 { 384 if (!csp_caps_initialized) 385 if (!initialize_caps(mcl)) { 386 /* Temporary failure in reading BT clock */ 387 return NULL; 388 } 389 390 return &_caps; 391 } 392 393 static int send_sync_cap_rsp(struct mcap_mcl *mcl, uint8_t rspcode, 394 uint8_t btclockres, uint16_t synclead, 395 uint16_t tmstampres, uint16_t tmstampacc) 396 { 397 mcap_md_sync_cap_rsp *rsp; 398 int sent; 399 400 rsp = g_new0(mcap_md_sync_cap_rsp, 1); 401 402 rsp->op = MCAP_MD_SYNC_CAP_RSP; 403 rsp->rc = rspcode; 404 405 rsp->btclock = btclockres; 406 rsp->sltime = htons(synclead); 407 rsp->timestnr = htons(tmstampres); 408 rsp->timestna = htons(tmstampacc); 409 410 sent = send_sync_cmd(mcl, rsp, sizeof(*rsp)); 411 g_free(rsp); 412 413 return sent; 414 } 415 416 static void proc_sync_cap_req(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len) 417 { 418 mcap_md_sync_cap_req *req; 419 uint16_t required_accuracy; 420 uint16_t our_accuracy; 421 uint32_t btclock; 422 uint16_t btres; 423 424 if (len != sizeof(mcap_md_sync_cap_req)) { 425 send_sync_cap_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 426 0, 0, 0, 0); 427 return; 428 } 429 430 if (!caps(mcl)) { 431 send_sync_cap_rsp(mcl, MCAP_RESOURCE_UNAVAILABLE, 432 0, 0, 0, 0); 433 return; 434 } 435 436 req = (mcap_md_sync_cap_req *) cmd; 437 required_accuracy = ntohs(req->timest); 438 our_accuracy = caps(mcl)->ts_acc; 439 440 if (required_accuracy < our_accuracy || required_accuracy < 1) { 441 send_sync_cap_rsp(mcl, MCAP_RESOURCE_UNAVAILABLE, 442 0, 0, 0, 0); 443 return; 444 } 445 446 if (!read_btclock_retry(mcl, &btclock, &btres)) { 447 send_sync_cap_rsp(mcl, MCAP_RESOURCE_UNAVAILABLE, 448 0, 0, 0, 0); 449 return; 450 } 451 452 mcl->csp->remote_caps = 1; 453 mcl->csp->rem_req_acc = required_accuracy; 454 455 send_sync_cap_rsp(mcl, MCAP_SUCCESS, btres, 456 caps(mcl)->syncleadtime_ms, 457 caps(mcl)->ts_res, our_accuracy); 458 } 459 460 static int send_sync_set_rsp(struct mcap_mcl *mcl, uint8_t rspcode, 461 uint32_t btclock, uint64_t timestamp, 462 uint16_t tmstampres) 463 { 464 mcap_md_sync_set_rsp *rsp; 465 int sent; 466 467 rsp = g_new0(mcap_md_sync_set_rsp, 1); 468 469 rsp->op = MCAP_MD_SYNC_SET_RSP; 470 rsp->rc = rspcode; 471 rsp->btclock = htonl(btclock); 472 rsp->timestst = hton64(timestamp); 473 rsp->timestsa = htons(tmstampres); 474 475 sent = send_sync_cmd(mcl, rsp, sizeof(*rsp)); 476 g_free(rsp); 477 478 return sent; 479 } 480 481 static gboolean get_all_clocks(struct mcap_mcl *mcl, uint32_t *btclock, 482 struct timespec *base_time, 483 uint64_t *timestamp) 484 { 485 int latency; 486 int retry = 5; 487 uint16_t btres; 488 struct timespec t0; 489 490 if (!caps(mcl)) 491 return FALSE; 492 493 latency = caps(mcl)->preempt_thresh + 1; 494 495 while (latency > caps(mcl)->preempt_thresh && --retry >= 0) { 496 497 clock_gettime(CLK, &t0); 498 499 if (!read_btclock(mcl, btclock, &btres)) 500 continue; 501 502 clock_gettime(CLK, base_time); 503 504 /* Tries to detect preemption between clock_gettime 505 * and read_btclock by measuring transaction time 506 */ 507 latency = time_us(base_time) - time_us(&t0); 508 } 509 510 *timestamp = mcap_get_timestamp(mcl, base_time); 511 512 return TRUE; 513 } 514 515 static gboolean sync_send_indication(gpointer user_data) 516 { 517 struct mcap_mcl *mcl; 518 mcap_md_sync_info_ind *cmd; 519 uint32_t btclock; 520 uint64_t tmstamp; 521 struct timespec base_time; 522 int sent; 523 524 if (!user_data) 525 return FALSE; 526 527 mcl = user_data; 528 529 if (!caps(mcl)) 530 return FALSE; 531 532 if (!get_all_clocks(mcl, &btclock, &base_time, &tmstamp)) 533 return FALSE; 534 535 cmd = g_new0(mcap_md_sync_info_ind, 1); 536 537 cmd->op = MCAP_MD_SYNC_INFO_IND; 538 cmd->btclock = htonl(btclock); 539 cmd->timestst = hton64(tmstamp); 540 cmd->timestsa = htons(caps(mcl)->latency); 541 542 sent = send_sync_cmd(mcl, cmd, sizeof(*cmd)); 543 g_free(cmd); 544 545 return !sent; 546 } 547 548 static gboolean proc_sync_set_req_phase2(gpointer user_data) 549 { 550 struct mcap_mcl *mcl; 551 struct sync_set_data *data; 552 uint8_t update; 553 uint32_t sched_btclock; 554 uint64_t new_tmstamp; 555 int ind_freq; 556 int role; 557 uint32_t btclock; 558 uint64_t tmstamp; 559 struct timespec base_time; 560 uint16_t tmstampacc; 561 gboolean reset; 562 int delay; 563 564 if (!user_data) 565 return FALSE; 566 567 mcl = user_data; 568 569 if (!mcl->csp->set_data) 570 return FALSE; 571 572 data = mcl->csp->set_data; 573 update = data->update; 574 sched_btclock = data->sched_btclock; 575 new_tmstamp = data->timestamp; 576 ind_freq = data->ind_freq; 577 role = data->role; 578 579 if (!caps(mcl)) { 580 send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0); 581 return FALSE; 582 } 583 584 if (!get_all_clocks(mcl, &btclock, &base_time, &tmstamp)) { 585 send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0); 586 return FALSE; 587 } 588 589 if (get_btrole(mcl) != role) { 590 send_sync_set_rsp(mcl, MCAP_INVALID_OPERATION, 0, 0, 0); 591 return FALSE; 592 } 593 594 reset = (new_tmstamp != MCAP_TMSTAMP_DONTSET); 595 596 if (reset) { 597 if (sched_btclock != MCAP_BTCLOCK_IMMEDIATE) { 598 delay = bt2us(btdiff(sched_btclock, btclock)); 599 if (delay >= 0 || ((new_tmstamp - delay) > 0)) { 600 new_tmstamp += delay; 601 DBG("CSP: reset w/ delay %dus, compensated", 602 delay); 603 } else 604 DBG("CSP: reset w/ delay %dus, uncompensated", 605 delay); 606 } 607 608 reset_tmstamp(mcl->csp, &base_time, new_tmstamp); 609 tmstamp = new_tmstamp; 610 } 611 612 tmstampacc = caps(mcl)->latency + caps(mcl)->ts_acc; 613 614 if (mcl->csp->ind_timer) { 615 g_source_remove(mcl->csp->ind_timer); 616 mcl->csp->ind_timer = 0; 617 } 618 619 if (update) { 620 int when = ind_freq + caps(mcl)->syncleadtime_ms; 621 mcl->csp->ind_timer = g_timeout_add(when, 622 sync_send_indication, 623 mcl); 624 } 625 626 send_sync_set_rsp(mcl, MCAP_SUCCESS, btclock, tmstamp, tmstampacc); 627 628 /* First indication after set is immediate */ 629 if (update) 630 sync_send_indication(mcl); 631 632 return FALSE; 633 } 634 635 static void proc_sync_set_req(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len) 636 { 637 mcap_md_sync_set_req *req; 638 uint32_t sched_btclock, cur_btclock; 639 uint16_t btres; 640 uint8_t update; 641 uint64_t timestamp; 642 struct sync_set_data *set_data; 643 int phase2_delay, ind_freq, when; 644 645 if (len != sizeof(mcap_md_sync_set_req)) { 646 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0); 647 return; 648 } 649 650 req = (mcap_md_sync_set_req *) cmd; 651 sched_btclock = ntohl(req->btclock); 652 update = req->timestui; 653 timestamp = ntoh64(req->timestst); 654 655 if (sched_btclock != MCAP_BTCLOCK_IMMEDIATE && 656 !valid_btclock(sched_btclock)) { 657 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0); 658 return; 659 } 660 661 if (update > 1) { 662 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0); 663 return; 664 } 665 666 if (!mcl->csp->remote_caps) { 667 /* Remote side did not ask our capabilities yet */ 668 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0); 669 return; 670 } 671 672 if (!caps(mcl)) { 673 send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0); 674 return; 675 } 676 677 if (!read_btclock_retry(mcl, &cur_btclock, &btres)) { 678 send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0); 679 return; 680 } 681 682 if (sched_btclock == MCAP_BTCLOCK_IMMEDIATE) 683 phase2_delay = 0; 684 else { 685 phase2_delay = btdiff(cur_btclock, sched_btclock); 686 687 if (phase2_delay < 0) { 688 /* can not reset in the past tense */ 689 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 690 0, 0, 0); 691 return; 692 } 693 694 /* Convert to miliseconds */ 695 phase2_delay = bt2ms(phase2_delay); 696 697 if (phase2_delay > 61*1000) { 698 /* More than 60 seconds in the future */ 699 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 700 0, 0, 0); 701 return; 702 } else if (phase2_delay < caps(mcl)->latency / 1000) { 703 /* Too fast for us to do in time */ 704 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 705 0, 0, 0); 706 return; 707 } 708 } 709 710 if (update) { 711 /* Indication frequency: required accuracy divided by ours */ 712 /* Converted to milisseconds */ 713 ind_freq = (1000 * mcl->csp->rem_req_acc) / caps(mcl)->ts_acc; 714 715 if (ind_freq < MAX(caps(mcl)->latency * 2 / 1000, 100)) { 716 /* Too frequent, we can't handle */ 717 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 718 0, 0, 0); 719 return; 720 } 721 722 DBG("CSP: indication every %dms", ind_freq); 723 } else 724 ind_freq = 0; 725 726 if (mcl->csp->ind_timer) { 727 /* Old indications are no longer sent */ 728 g_source_remove(mcl->csp->ind_timer); 729 mcl->csp->ind_timer = 0; 730 } 731 732 if (!mcl->csp->set_data) 733 mcl->csp->set_data = g_new0(struct sync_set_data, 1); 734 735 set_data = (struct sync_set_data *) mcl->csp->set_data; 736 737 set_data->update = update; 738 set_data->sched_btclock = sched_btclock; 739 set_data->timestamp = timestamp; 740 set_data->ind_freq = ind_freq; 741 set_data->role = get_btrole(mcl); 742 743 /* TODO is there some way to schedule a call based directly on 744 * a BT clock value, instead of this estimation that uses 745 * the SO clock? */ 746 747 if (phase2_delay > 0) { 748 when = phase2_delay + caps(mcl)->syncleadtime_ms; 749 mcl->csp->set_timer = g_timeout_add(when, 750 proc_sync_set_req_phase2, 751 mcl); 752 } else 753 proc_sync_set_req_phase2(mcl); 754 755 /* First indication is immediate */ 756 if (update) 757 sync_send_indication(mcl); 758 } 759 760 static void proc_sync_cap_rsp(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len) 761 { 762 mcap_md_sync_cap_rsp *rsp; 763 uint8_t mcap_err; 764 uint8_t btclockres; 765 uint16_t synclead; 766 uint16_t tmstampres; 767 uint16_t tmstampacc; 768 struct mcap_sync_cap_cbdata *cbdata; 769 mcap_sync_cap_cb cb; 770 gpointer user_data; 771 772 if (mcl->csp->csp_req != MCAP_MD_SYNC_CAP_REQ) { 773 DBG("CSP: got unexpected cap respose"); 774 return; 775 } 776 777 if (!mcl->csp->csp_priv_data) { 778 DBG("CSP: no priv data for cap respose"); 779 return; 780 } 781 782 cbdata = mcl->csp->csp_priv_data; 783 cb = cbdata->cb; 784 user_data = cbdata->user_data; 785 g_free(cbdata); 786 787 mcl->csp->csp_priv_data = NULL; 788 mcl->csp->csp_req = 0; 789 790 if (len != sizeof(mcap_md_sync_cap_rsp)) { 791 DBG("CSP: got corrupted cap respose"); 792 return; 793 } 794 795 rsp = (mcap_md_sync_cap_rsp *) cmd; 796 mcap_err = rsp->rc; 797 btclockres = rsp->btclock; 798 synclead = ntohs(rsp->sltime); 799 tmstampres = ntohs(rsp->timestnr); 800 tmstampacc = ntohs(rsp->timestna); 801 802 if (!mcap_err) 803 mcl->csp->local_caps = TRUE; 804 805 cb(mcl, mcap_err, btclockres, synclead, tmstampres, tmstampacc, NULL, 806 user_data); 807 } 808 809 static void proc_sync_set_rsp(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len) 810 { 811 mcap_md_sync_set_rsp *rsp; 812 uint8_t mcap_err; 813 uint32_t btclock; 814 uint64_t timestamp; 815 uint16_t accuracy; 816 struct mcap_sync_set_cbdata *cbdata; 817 mcap_sync_set_cb cb; 818 gpointer user_data; 819 820 if (mcl->csp->csp_req != MCAP_MD_SYNC_SET_REQ) { 821 DBG("CSP: got unexpected set respose"); 822 return; 823 } 824 825 if (!mcl->csp->csp_priv_data) { 826 DBG("CSP: no priv data for set respose"); 827 return; 828 } 829 830 cbdata = mcl->csp->csp_priv_data; 831 cb = cbdata->cb; 832 user_data = cbdata->user_data; 833 g_free(cbdata); 834 835 mcl->csp->csp_priv_data = NULL; 836 mcl->csp->csp_req = 0; 837 838 if (len != sizeof(mcap_md_sync_set_rsp)) { 839 DBG("CSP: got corrupted set respose"); 840 return; 841 } 842 843 rsp = (mcap_md_sync_set_rsp *) cmd; 844 mcap_err = rsp->rc; 845 btclock = ntohl(rsp->btclock); 846 timestamp = ntoh64(rsp->timestst); 847 accuracy = ntohs(rsp->timestsa); 848 849 if (!mcap_err && !valid_btclock(btclock)) 850 mcap_err = MCAP_ERROR_INVALID_ARGS; 851 852 cb(mcl, mcap_err, btclock, timestamp, accuracy, NULL, user_data); 853 } 854 855 static void proc_sync_info_ind(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len) 856 { 857 mcap_md_sync_info_ind *req; 858 struct sync_info_ind_data data; 859 uint32_t btclock; 860 861 if (!mcl->csp->ind_expected) { 862 DBG("CSP: received unexpected info indication"); 863 return; 864 } 865 866 if (len != sizeof(mcap_md_sync_info_ind)) 867 return; 868 869 req = (mcap_md_sync_info_ind *) cmd; 870 871 btclock = ntohl(req->btclock); 872 873 if (!valid_btclock(btclock)) 874 return; 875 876 data.btclock = btclock; 877 data.timestamp = ntoh64(req->timestst); 878 data.accuracy = ntohs(req->timestsa); 879 880 if (mcl->mi->mcl_sync_infoind_cb) 881 mcl->mi->mcl_sync_infoind_cb(mcl, &data); 882 } 883 884 void proc_sync_cmd(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len) 885 { 886 if (!mcl->mi->csp_enabled || !mcl->csp) { 887 switch (cmd[0]) { 888 case MCAP_MD_SYNC_CAP_REQ: 889 send_unsupported_cap_req(mcl); 890 break; 891 case MCAP_MD_SYNC_SET_REQ: 892 send_unsupported_set_req(mcl); 893 break; 894 } 895 return; 896 } 897 898 switch (cmd[0]) { 899 case MCAP_MD_SYNC_CAP_REQ: 900 proc_sync_cap_req(mcl, cmd, len); 901 break; 902 case MCAP_MD_SYNC_CAP_RSP: 903 proc_sync_cap_rsp(mcl, cmd, len); 904 break; 905 case MCAP_MD_SYNC_SET_REQ: 906 proc_sync_set_req(mcl, cmd, len); 907 break; 908 case MCAP_MD_SYNC_SET_RSP: 909 proc_sync_set_rsp(mcl, cmd, len); 910 break; 911 case MCAP_MD_SYNC_INFO_IND: 912 proc_sync_info_ind(mcl, cmd, len); 913 break; 914 } 915 } 916 917 void mcap_sync_cap_req(struct mcap_mcl *mcl, uint16_t reqacc, 918 mcap_sync_cap_cb cb, gpointer user_data, 919 GError **err) 920 { 921 struct mcap_sync_cap_cbdata *cbdata; 922 mcap_md_sync_cap_req *cmd; 923 924 if (!mcl->mi->csp_enabled || !mcl->csp) { 925 g_set_error(err, 926 MCAP_CSP_ERROR, 927 MCAP_ERROR_RESOURCE_UNAVAILABLE, 928 "CSP not enabled for the instance"); 929 return; 930 } 931 932 if (mcl->csp->csp_req) { 933 g_set_error(err, 934 MCAP_CSP_ERROR, 935 MCAP_ERROR_RESOURCE_UNAVAILABLE, 936 "Pending CSP request"); 937 return; 938 } 939 940 mcl->csp->csp_req = MCAP_MD_SYNC_CAP_REQ; 941 cmd = g_new0(mcap_md_sync_cap_req, 1); 942 943 cmd->op = MCAP_MD_SYNC_CAP_REQ; 944 cmd->timest = htons(reqacc); 945 946 cbdata = g_new0(struct mcap_sync_cap_cbdata, 1); 947 cbdata->cb = cb; 948 cbdata->user_data = user_data; 949 mcl->csp->csp_priv_data = cbdata; 950 951 send_sync_cmd(mcl, cmd, sizeof(*cmd)); 952 953 g_free(cmd); 954 } 955 956 void mcap_sync_set_req(struct mcap_mcl *mcl, uint8_t update, uint32_t btclock, 957 uint64_t timestamp, mcap_sync_set_cb cb, 958 gpointer user_data, GError **err) 959 { 960 mcap_md_sync_set_req *cmd; 961 struct mcap_sync_set_cbdata *cbdata; 962 963 if (!mcl->mi->csp_enabled || !mcl->csp) { 964 g_set_error(err, 965 MCAP_CSP_ERROR, 966 MCAP_ERROR_RESOURCE_UNAVAILABLE, 967 "CSP not enabled for the instance"); 968 return; 969 } 970 971 if (!mcl->csp->local_caps) { 972 g_set_error(err, 973 MCAP_CSP_ERROR, 974 MCAP_ERROR_RESOURCE_UNAVAILABLE, 975 "Did not get CSP caps from slave yet"); 976 return; 977 } 978 979 if (mcl->csp->csp_req) { 980 g_set_error(err, 981 MCAP_CSP_ERROR, 982 MCAP_ERROR_RESOURCE_UNAVAILABLE, 983 "Pending CSP request"); 984 return; 985 } 986 987 mcl->csp->csp_req = MCAP_MD_SYNC_SET_REQ; 988 cmd = g_new0(mcap_md_sync_set_req, 1); 989 990 cmd->op = MCAP_MD_SYNC_SET_REQ; 991 cmd->timestui = update; 992 cmd->btclock = htonl(btclock); 993 cmd->timestst = hton64(timestamp); 994 995 mcl->csp->ind_expected = update; 996 997 cbdata = g_new0(struct mcap_sync_set_cbdata, 1); 998 cbdata->cb = cb; 999 cbdata->user_data = user_data; 1000 mcl->csp->csp_priv_data = cbdata; 1001 1002 send_sync_cmd(mcl, cmd, sizeof(*cmd)); 1003 1004 g_free(cmd); 1005 } 1006 1007 void mcap_enable_csp(struct mcap_instance *mi) 1008 { 1009 mi->csp_enabled = TRUE; 1010 } 1011 1012 void mcap_disable_csp(struct mcap_instance *mi) 1013 { 1014 mi->csp_enabled = FALSE; 1015 } 1016
