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
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
send_sync_cmd(struct mcap_mcl * mcl,const void * buf,uint32_t size)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
send_unsupported_cap_req(struct mcap_mcl * mcl)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
send_unsupported_set_req(struct mcap_mcl * mcl)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
reset_tmstamp(struct mcap_csp * csp,struct timespec * base_time,uint64_t new_tmstamp)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
mcap_sync_init(struct mcap_mcl * mcl)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
mcap_sync_stop(struct mcap_mcl * mcl)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
time_us(struct timespec * tv)193 static uint64_t time_us(struct timespec *tv)
194 {
195 return tv->tv_sec * 1000000 + tv->tv_nsec / 1000;
196 }
197
bt2us(int bt)198 static int64_t bt2us(int bt)
199 {
200 return bt * 312.5;
201 }
202
bt2ms(int bt)203 static int bt2ms(int bt)
204 {
205 return bt * 312.5 / 1000;
206 }
207
btoffset(uint32_t btclk1,uint32_t btclk2)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
btdiff(uint32_t btclk1,uint32_t btclk2)220 static int btdiff(uint32_t btclk1, uint32_t btclk2)
221 {
222 return btoffset(btclk1, btclk2);
223 }
224
valid_btclock(uint32_t btclk)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 */
read_btclock(struct mcap_mcl * mcl,uint32_t * btclock,uint16_t * btaccuracy)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
read_btclock_retry(struct mcap_mcl * mcl,uint32_t * btclock,uint16_t * btaccuracy)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
get_btrole(struct mcap_mcl * mcl)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
mcap_get_timestamp(struct mcap_mcl * mcl,struct timespec * given_time)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
mcap_get_btclock(struct mcap_mcl * mcl)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
initialize_caps(struct mcap_mcl * mcl)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
caps(struct mcap_mcl * mcl)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
send_sync_cap_rsp(struct mcap_mcl * mcl,uint8_t rspcode,uint8_t btclockres,uint16_t synclead,uint16_t tmstampres,uint16_t tmstampacc)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
proc_sync_cap_req(struct mcap_mcl * mcl,uint8_t * cmd,uint32_t len)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
send_sync_set_rsp(struct mcap_mcl * mcl,uint8_t rspcode,uint32_t btclock,uint64_t timestamp,uint16_t tmstampres)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
get_all_clocks(struct mcap_mcl * mcl,uint32_t * btclock,struct timespec * base_time,uint64_t * timestamp)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
sync_send_indication(gpointer user_data)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
proc_sync_set_req_phase2(gpointer user_data)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
proc_sync_set_req(struct mcap_mcl * mcl,uint8_t * cmd,uint32_t len)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
proc_sync_cap_rsp(struct mcap_mcl * mcl,uint8_t * cmd,uint32_t len)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
proc_sync_set_rsp(struct mcap_mcl * mcl,uint8_t * cmd,uint32_t len)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
proc_sync_info_ind(struct mcap_mcl * mcl,uint8_t * cmd,uint32_t len)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
proc_sync_cmd(struct mcap_mcl * mcl,uint8_t * cmd,uint32_t len)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
mcap_sync_cap_req(struct mcap_mcl * mcl,uint16_t reqacc,mcap_sync_cap_cb cb,gpointer user_data,GError ** err)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
mcap_sync_set_req(struct mcap_mcl * mcl,uint8_t update,uint32_t btclock,uint64_t timestamp,mcap_sync_set_cb cb,gpointer user_data,GError ** err)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
mcap_enable_csp(struct mcap_instance * mi)1007 void mcap_enable_csp(struct mcap_instance *mi)
1008 {
1009 mi->csp_enabled = TRUE;
1010 }
1011
mcap_disable_csp(struct mcap_instance * mi)1012 void mcap_disable_csp(struct mcap_instance *mi)
1013 {
1014 mi->csp_enabled = FALSE;
1015 }
1016