← Back to branch summary

~ubuntu-branches/ubuntu/wily/bluez/wily

« back to all changes in this revision

Viewing changes to health/mcap_sync.c

Import upstream version 4.81

Show diffs side-by-side

added added

removed removed

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