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