1 /*
2 * Copyright (c) 2004-2006 Maxim Sobolev <sobomax@FreeBSD.org>
3 * Copyright (c) 2006-2015 Sippy Software, Inc., http://www.sippysoft.com
4 * All rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25 * SUCH DAMAGE.
26 *
27 */
28
29 #include <sys/time.h>
30 #include <math.h>
31 #include <stdio.h>
32 #include <string.h>
33 #include <time.h>
34
35 #include "rtpp_monotime.h"
36 #include "rtpp_time.h"
37
38 #define timespecsub2(r, v, u) \
39 do { \
40 SEC(r) = SEC(v) - SEC(u); \
41 NSEC(r) = NSEC(v) - NSEC(u); \
42 if (NSEC(r) < 0 && (SEC(r) > 0 || NSEC(r) <= -NSEC_MAX)) { \
43 SEC(r)--; \
44 NSEC(r) += NSEC_MAX; \
45 } \
46 } while (0);
47
48 #define timespecsub(v, u) timespecsub2((v), (v), (u))
49
50 #define timespecadd2(r, v, u) \
51 do { \
52 SEC(r) = SEC(v) + SEC(u); \
53 NSEC(r) = NSEC(v) + NSEC(u); \
54 if (NSEC(r) >= NSEC_MAX) { \
55 SEC(r)++; \
56 NSEC(r) -= NSEC_MAX; \
57 } \
58 } while (0);
59
60 #define timespecmean2(r, v, u) \
61 do { \
62 if ((SEC(v) % 2) ^ (SEC(u) % 2)) { \
63 SEC(r) = 1; \
64 NSEC(r) = -(NSEC_MAX / 2); \
65 } else { \
66 SEC(r) = 0; \
67 NSEC(r) = 0; \
68 } \
69 SEC(r) = (SEC(r) + SEC(v) + SEC(u)) / 2; \
70 NSEC(r) += (NSEC(v) + NSEC(u)) / 2; \
71 } while (0);
72
73 #define timespeccmp(t, c, u) \
74 ((SEC(t) == SEC(u)) ? \
75 (NSEC(t) c NSEC(u)) : \
76 (SEC(t) c SEC(u)))
77
78 #define timespecaddnsec(v, nsec) \
79 do { \
80 NSEC(v) += (nsec); \
81 if (NSEC(v) >= NSEC_MAX) { \
82 SEC(v)++; \
83 NSEC(v) -= NSEC_MAX; \
84 } \
85 } while (0);
86
87 #define timespecsubnsec(v, nsec) \
88 do { \
89 NSEC(v) -= (nsec); \
90 if (NSEC(v) < 0 && (SEC(v) > 0 || NSEC(v) <= -NSEC_MAX)) { \
91 SEC(v)--; \
92 NSEC(v) += NSEC_MAX; \
93 } \
94 } while (0);
95
96 struct r2mdatapt {
97 struct timespec r2m;
98 struct timespec r2m_rel;
99 struct timespec r2m_err;
100 struct timespec mtime_dur;
101 struct timespec rtime_dur;
102 struct timespec mtime;
103 struct timespec rtime;
104 int rejected;
105 double T;
106 double T_r2m;
107 };
108
rtime2mtime(struct r2mdatapt * dp)109 int rtime2mtime(struct r2mdatapt *dp)
110 {
111 struct timespec tp[4];
112 int i;
113
114 for (i = 0; i < 4; i++) {
115 switch (i) {
116 case 0:
117 if (clock_gettime(RTPP_CLOCK_MONO, &tp[i]) == -1)
118 return (-1);
119 break;
120
121 case 1:
122 if (clock_gettime(RTPP_CLOCK_REAL, &tp[i]) == -1)
123 return (-1);
124 break;
125
126 case 2:
127 if (clock_gettime(RTPP_CLOCK_REAL, &tp[i]) == -1)
128 return (-1);
129 if (timespeccmp(&tp[i], ==, &tp[1])) {
130 /*
131 * If we are too fast or clock resolution is too
132 * shitty for the clock to increase between (1)
133 * and (2), call again until we get at least one
134 * notch of a difference between those.
135 */
136 i -= 1;
137 }
138 break;
139
140 case 3:
141 if (clock_gettime(RTPP_CLOCK_MONO, &tp[i]) == -1)
142 return (-1);
143 if (timespeccmp(&tp[i], ==, &tp[0])) {
144 /*
145 * If we are too fast or clock resolution is too
146 * shitty for the clock to increase between (0)
147 * and (3), call again until we get at least one
148 * notch of a difference between those.
149 */
150 i -= 1;
151 }
152 break;
153 }
154 }
155 timespecsub2(&dp->rtime_dur, &tp[2], &tp[1]);
156 timespecsub2(&dp->mtime_dur, &tp[3], &tp[0]);
157 dp->rtime = tp[2];
158 dp->mtime = tp[3];
159 timespecsub(&tp[1], &tp[0]);
160 timespecsub(&tp[2], &tp[3]);
161 timespecmean2(&dp->r2m, &tp[1], &tp[2]);
162 timespecsub2(&dp->r2m_err, &tp[1], &tp[2]);
163
164 return (0);
165 }
166
167 /* 95% confidence at 50 data points */
168 #define Tcrit 2.956
169
170 #define R2M_DS_LEN 53
171 #define R2M_DS_WARM 3
172 #define R2M_DS_MIN (R2M_DS_LEN / 2)
173 /* Don't bother to recalibrare is the diff is less than 1uS (1000nS) */
174 #define R2M_MIN_PREC 1000
175
176 struct r2m_conv {
177 struct timespec cval;
178 struct timespec min;
179 struct timespec max;
180 struct timespec lastcal_mtime;
181 struct timespec lastcal_rtime;
182 };
183
184 static int
r2m_check(struct r2m_conv * r2m_old)185 r2m_check(struct r2m_conv *r2m_old)
186 {
187 struct r2mdatapt r2m_ds[R2M_DS_WARM + 1];
188 int i;
189
190 memset(r2m_ds, '\0', sizeof(r2m_ds));
191 for (i = 0; i < R2M_DS_WARM + 1; i++) {
192 rtime2mtime(&r2m_ds[i]);
193 }
194 if (timespeccmp(&r2m_old->min, <, &r2m_ds[R2M_DS_WARM].r2m) &&
195 timespeccmp(&r2m_old->max, >, &r2m_ds[R2M_DS_WARM].r2m)) {
196 r2m_old->lastcal_mtime = r2m_ds[R2M_DS_WARM].mtime;
197 r2m_old->lastcal_rtime = r2m_ds[R2M_DS_WARM].rtime;
198 return (0);
199 }
200 return (1);
201 }
202
203
204 static int
r2m_calibrate(struct r2m_conv * r2m)205 r2m_calibrate(struct r2m_conv *r2m)
206 {
207 struct r2mdatapt r2m_ds[R2M_DS_LEN];
208 int i, last_i, nsam;
209 long long mean_err, mean_r2m, variance, variance_r2m, diff;
210 double stddev, stddev_r2m;
211 struct r2m_conv r2m_rval;
212
213 /* Do a quick check to see if the update is really necessary */
214 if (!timespeciszero(&r2m->cval) && r2m_check(r2m) == 0) {
215 return (0);
216 }
217
218 restart:
219 memset(r2m_ds, '\0', sizeof(r2m_ds));
220 for (i = 0; i < R2M_DS_LEN; i++) {
221 rtime2mtime(&r2m_ds[i]);
222 if (r2m_ds[i].r2m_err.tv_sec != 0 || r2m_ds[i].r2m_err.tv_nsec < 0) {
223 /* Something weird, re-do */
224 i -= 1;
225 continue;
226 }
227 if (timespeccmp(&r2m_ds[i].rtime_dur, >, &r2m_ds[i].mtime_dur)) {
228 /* Real-time jumped, restart the measurement */
229 i = 0;
230 continue;
231 }
232 }
233 /*
234 * Reject first few samples, they are usually skewed due to the cache
235 * warm-up process.
236 */
237 for (i = 0; i < R2M_DS_WARM; i++) {
238 r2m_ds[i].rejected = 1;
239 }
240 again:
241 mean_err = 0;
242 nsam = 0;
243 for (i = 0; i < R2M_DS_LEN; i++) {
244 if (r2m_ds[i].rejected != 0) {
245 continue;
246 }
247 nsam += 1;
248 mean_err += r2m_ds[i].r2m_err.tv_nsec;
249 }
250 if (nsam < R2M_DS_MIN) {
251 /* Make sure we have enough good data to work with */
252 goto restart;
253 }
254 mean_err /= nsam;
255 variance = 0;
256 for (i = 0; i < R2M_DS_LEN; i++) {
257 if (r2m_ds[i].rejected != 0) {
258 continue;
259 }
260 diff = r2m_ds[i].r2m_err.tv_nsec - mean_err;
261 variance += (diff * diff);
262 }
263 variance /= (nsam - 1);
264 stddev = sqrt(variance);
265 for (i = 0; i < R2M_DS_LEN; i++) {
266 if (r2m_ds[i].rejected != 0) {
267 continue;
268 }
269 r2m_ds[i].T = (r2m_ds[i].r2m_err.tv_nsec - mean_err) / stddev;
270 if (r2m_ds[i].T < 0) {
271 r2m_ds[i].T = -r2m_ds[i].T;
272 }
273 if (r2m_ds[i].T > Tcrit) {
274 r2m_ds[i].rejected = 1;
275 goto again;
276 }
277 }
278 memset(&r2m_rval, '\0', sizeof(r2m_rval));
279 for (i = 0; i < R2M_DS_LEN; i++) {
280 if (r2m_ds[i].rejected != 0) {
281 continue;
282 }
283 if (timespeciszero(&r2m_rval.min) || timespeccmp(&r2m_rval.min, >, &r2m_ds[i].r2m)) {
284 r2m_rval.min = r2m_ds[i].r2m;
285 }
286 if (timespeciszero(&r2m_rval.max) || timespeccmp(&r2m_rval.max, <, &r2m_ds[i].r2m)) {
287 r2m_rval.max = r2m_ds[i].r2m;
288 }
289 #if 0
290 printf("%d,%ld,%ld,%ld,%ld,%f\n", i, r2m_ds[i].r2m.tv_sec,
291 r2m_ds[i].r2m.tv_nsec, r2m_ds[i].r2m_err.tv_sec,
292 r2m_ds[i].r2m_err.tv_nsec, r2m_ds[i].T);
293 #endif
294 }
295 #if 0
296 printf("min r2m: tv_sec = %ld, tv_nsec = %ld\n", r2m_rval.min.tv_sec, r2m_rval.min.tv_nsec);
297 printf("mean_err = %lld, variance = %lld, stddev = %f\n", mean_err, variance, stddev);
298 #endif
299 mean_r2m = 0;
300 for (i = 0; i < R2M_DS_LEN; i++) {
301 if (r2m_ds[i].rejected != 0) {
302 continue;
303 }
304 /* Normalize r2m values so we can only deal with the nsec part */
305 timespecsub2(&r2m_ds[i].r2m_rel, &r2m_ds[i].r2m, &r2m_rval.min);
306
307 if (r2m_ds[i].r2m_rel.tv_sec > 0 || r2m_ds[i].r2m_rel.tv_nsec > 0x7fff) {
308 /*
309 * The difference between min and max measured r2m values cannot be
310 * greater than about half of the tv_nsec total bit length, both
311 * practically and because the code below would overflow otherwise.
312 */
313 goto restart;
314 }
315 mean_r2m += r2m_ds[i].r2m_rel.tv_nsec;
316 }
317 mean_r2m /= nsam;
318 variance_r2m = 0;
319 for (i = 0; i < R2M_DS_LEN; i++) {
320 if (r2m_ds[i].rejected != 0) {
321 continue;
322 }
323 diff = r2m_ds[i].r2m_rel.tv_nsec - mean_r2m;
324 variance_r2m += (diff * diff);
325 }
326 variance_r2m /= (nsam - 1);
327 stddev_r2m = sqrt(variance_r2m);
328 last_i = 0;
329 for (i = 0; i < R2M_DS_LEN; i++) {
330 if (r2m_ds[i].rejected != 0) {
331 continue;
332 }
333 r2m_ds[i].T_r2m = (r2m_ds[i].r2m_rel.tv_nsec - mean_r2m) / stddev_r2m;
334 if (r2m_ds[i].T_r2m < 0) {
335 r2m_ds[i].T_r2m = -r2m_ds[i].T_r2m;
336 }
337 if (r2m_ds[i].T_r2m > Tcrit) {
338 r2m_ds[i].rejected = 1;
339 goto again;
340 }
341 last_i = i;
342 }
343 #if 0
344 for (i = 0; i < R2M_DS_LEN; i++) {
345 if (r2m_ds[i].rejected != 0) {
346 continue;
347 }
348 printf("%d,%ld,%ld,%ld,%ld,%f,%f\n", i, r2m_ds[i].r2m_rel.tv_sec,
349 r2m_ds[i].r2m_rel.tv_nsec, r2m_ds[i].r2m_err.tv_sec,
350 r2m_ds[i].r2m_err.tv_nsec, r2m_ds[i].T, r2m_ds[i].T_r2m);
351 }
352 #endif
353
354 r2m_rval.cval = r2m_rval.min;
355 timespecaddnsec(&r2m_rval.cval, mean_r2m);
356
357 if (!timespeciszero(&r2m->cval) && timespeccmp(&r2m->min, <, &r2m_rval.cval) &&
358 timespeccmp(&r2m->max, >, &r2m_rval.cval)) {
359 /* The new value is essentially the same */
360 r2m->lastcal_mtime = r2m_ds[last_i].mtime;
361 r2m->lastcal_rtime = r2m_ds[last_i].rtime;
362 return (0);
363 }
364
365 r2m_rval.lastcal_mtime = r2m_ds[last_i].mtime;
366 r2m_rval.lastcal_rtime = r2m_ds[last_i].rtime;
367
368 if (mean_r2m < (R2M_MIN_PREC / 2)) {
369 r2m_rval.max = r2m_rval.min = r2m_rval.cval;
370 timespecaddnsec(&r2m_rval.max, (R2M_MIN_PREC / 2));
371 timespecsubnsec(&r2m_rval.min, (R2M_MIN_PREC / 2));
372 }
373
374 #if 0
375 printf("mean_r2m = %ld.%.9ld, variance = %lld, stddev = %f\n", r2m_rval.cval.tv_sec,
376 r2m_rval.cval.tv_nsec, variance_r2m, stddev_r2m);
377 printf("min_r2m = %ld.%.9ld, max_r2m = %ld.%.9ld\n", r2m_rval.min.tv_sec,
378 r2m_rval.min.tv_nsec, r2m_rval.max.tv_sec, r2m_rval.max.tv_nsec);
379 #endif
380
381 *r2m = r2m_rval;
382
383 return (1);
384 }
385
386 static __thread struct r2m_conv r2m_conv1;
387 static const struct timespec recal_ival = {.tv_sec = 1, .tv_nsec = 0};
388
389 #define timeval2timespec(s, v) \
390 do { \
391 SEC(s) = SEC(v); \
392 NSEC(s) = USEC(v) * 1000; \
393 } while (0);
394 #define timespec2timeval(v, s) \
395 do { \
396 SEC(v) = SEC(s); \
397 USEC(v) = NSEC(s) / 1000; \
398 } while (0);
399
400 double
rtimeval2dtime(struct timeval * rtime)401 rtimeval2dtime(struct timeval *rtime)
402 {
403 struct timespec rtimespec, timediff;
404 struct timespec mtime;
405
406 timeval2timespec(&rtimespec, rtime);
407 if (timespeciszero(&r2m_conv1.cval)) {
408 r2m_calibrate(&r2m_conv1);
409 } else {
410 timespecsub2(&timediff, &rtimespec, &r2m_conv1.lastcal_rtime);
411 if (timespeccmp(&rtimespec, <, &r2m_conv1.lastcal_rtime) ||
412 timespeccmp(&timediff, >, &recal_ival)) {
413 r2m_calibrate(&r2m_conv1);
414 }
415 }
416 timespecsub2(&mtime, &rtimespec, &r2m_conv1.cval);
417
418 return (timespec2dtime(&mtime));
419 }
420
421 void
dtime2rtimeval(double dtime,struct timeval * rtimeval)422 dtime2rtimeval(double dtime, struct timeval *rtimeval)
423 {
424 struct timespec rtimespec, mtime, timediff;
425
426 dtime2mtimespec(dtime, &mtime);
427 if (timespeciszero(&r2m_conv1.cval)) {
428 r2m_calibrate(&r2m_conv1);
429 } else {
430 timespecsub2(&timediff, &mtime, &r2m_conv1.lastcal_mtime);
431 if (timespeccmp(&timediff, >, &recal_ival)) {
432 r2m_calibrate(&r2m_conv1);
433 }
434 }
435 timespecadd2(&rtimespec, &mtime, &r2m_conv1.cval);
436 timespec2timeval(rtimeval, &rtimespec);
437 }
438
439 double
dtime2rtime(double dtime)440 dtime2rtime(double dtime)
441 {
442 struct timespec rtimespec, mtime, timediff;
443
444 dtime2mtimespec(dtime, &mtime);
445 if (timespeciszero(&r2m_conv1.cval)) {
446 r2m_calibrate(&r2m_conv1);
447 } else {
448 timespecsub2(&timediff, &mtime, &r2m_conv1.lastcal_mtime);
449 if (timespeccmp(&timediff, >, &recal_ival)) {
450 r2m_calibrate(&r2m_conv1);
451 }
452 }
453 timespecadd2(&rtimespec, &mtime, &r2m_conv1.cval);
454 return (timespec2dtime(&rtimespec));
455 }
456