Branch data Line data Source code
1 : : /***
2 : : This file is part of PulseAudio.
3 : :
4 : : Copyright 2007 Lennart Poettering
5 : :
6 : : PulseAudio is free software; you can redistribute it and/or modify
7 : : it under the terms of the GNU Lesser General Public License as
8 : : published by the Free Software Foundation; either version 2.1 of the
9 : : License, or (at your option) any later version.
10 : :
11 : : PulseAudio is distributed in the hope that it will be useful, but
12 : : WITHOUT ANY WARRANTY; without even the implied warranty of
13 : : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 : : Lesser General Public License for more details.
15 : :
16 : : You should have received a copy of the GNU Lesser General Public
17 : : License along with PulseAudio; if not, write to the Free Software
18 : : Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
19 : : USA.
20 : : ***/
21 : :
22 : : #ifdef HAVE_CONFIG_H
23 : : #include <config.h>
24 : : #endif
25 : :
26 : : #include <stdio.h>
27 : : #include <math.h>
28 : :
29 : : #include <pulse/sample.h>
30 : : #include <pulse/xmalloc.h>
31 : :
32 : : #include <pulsecore/macro.h>
33 : :
34 : : #include "time-smoother.h"
35 : :
36 : : #define HISTORY_MAX 64
37 : :
38 : : /*
39 : : * Implementation of a time smoothing algorithm to synchronize remote
40 : : * clocks to a local one. Evens out noise, adjusts to clock skew and
41 : : * allows cheap estimations of the remote time while clock updates may
42 : : * be seldom and received in non-equidistant intervals.
43 : : *
44 : : * Basically, we estimate the gradient of received clock samples in a
45 : : * certain history window (of size 'history_time') with linear
46 : : * regression. With that info we estimate the remote time in
47 : : * 'adjust_time' ahead and smoothen our current estimation function
48 : : * towards that point with a 3rd order polynomial interpolation with
49 : : * fitting derivatives. (more or less a b-spline)
50 : : *
51 : : * The larger 'history_time' is chosen the better we will suppress
52 : : * noise -- but we'll adjust to clock skew slower..
53 : : *
54 : : * The larger 'adjust_time' is chosen the smoother our estimation
55 : : * function will be -- but we'll adjust to clock skew slower, too.
56 : : *
57 : : * If 'monotonic' is TRUE the resulting estimation function is
58 : : * guaranteed to be monotonic.
59 : : */
60 : :
61 : : struct pa_smoother {
62 : : pa_usec_t adjust_time, history_time;
63 : :
64 : : pa_usec_t time_offset;
65 : :
66 : : pa_usec_t px, py; /* Point p, where we want to reach stability */
67 : : double dp; /* Gradient we want at point p */
68 : :
69 : : pa_usec_t ex, ey; /* Point e, which we estimated before and need to smooth to */
70 : : double de; /* Gradient we estimated for point e */
71 : : pa_usec_t ry; /* The original y value for ex */
72 : :
73 : : /* History of last measurements */
74 : : pa_usec_t history_x[HISTORY_MAX], history_y[HISTORY_MAX];
75 : : unsigned history_idx, n_history;
76 : :
77 : : /* To even out for monotonicity */
78 : : pa_usec_t last_y, last_x;
79 : :
80 : : /* Cached parameters for our interpolation polynomial y=ax^3+b^2+cx */
81 : : double a, b, c;
82 : : pa_bool_t abc_valid:1;
83 : :
84 : : pa_bool_t monotonic:1;
85 : : pa_bool_t paused:1;
86 : : pa_bool_t smoothing:1; /* If FALSE we skip the polynomial interpolation step */
87 : :
88 : : pa_usec_t pause_time;
89 : :
90 : : unsigned min_history;
91 : : };
92 : :
93 : 1 : pa_smoother* pa_smoother_new(
94 : : pa_usec_t adjust_time,
95 : : pa_usec_t history_time,
96 : : pa_bool_t monotonic,
97 : : pa_bool_t smoothing,
98 : : unsigned min_history,
99 : : pa_usec_t time_offset,
100 : : pa_bool_t paused) {
101 : :
102 : : pa_smoother *s;
103 : :
104 [ - + ]: 1 : pa_assert(adjust_time > 0);
105 [ - + ]: 1 : pa_assert(history_time > 0);
106 [ - + ]: 1 : pa_assert(min_history >= 2);
107 [ - + ]: 1 : pa_assert(min_history <= HISTORY_MAX);
108 : :
109 : 1 : s = pa_xnew(pa_smoother, 1);
110 : 1 : s->adjust_time = adjust_time;
111 : 1 : s->history_time = history_time;
112 : 1 : s->min_history = min_history;
113 : 1 : s->monotonic = monotonic;
114 : 1 : s->smoothing = smoothing;
115 : :
116 : 1 : pa_smoother_reset(s, time_offset, paused);
117 : :
118 : 1 : return s;
119 : : }
120 : :
121 : 1 : void pa_smoother_free(pa_smoother* s) {
122 [ - + ]: 1 : pa_assert(s);
123 : :
124 : 1 : pa_xfree(s);
125 : 1 : }
126 : :
127 : : #define REDUCE(x) \
128 : : do { \
129 : : x = (x) % HISTORY_MAX; \
130 : : } while(FALSE)
131 : :
132 : : #define REDUCE_INC(x) \
133 : : do { \
134 : : x = ((x)+1) % HISTORY_MAX; \
135 : : } while(FALSE)
136 : :
137 : :
138 : : static void drop_old(pa_smoother *s, pa_usec_t x) {
139 : :
140 : : /* Drop items from history which are too old, but make sure to
141 : : * always keep min_history in the history */
142 : :
143 [ + + ]: 153 : while (s->n_history > s->min_history) {
144 : :
145 [ + + ]: 146 : if (s->history_x[s->history_idx] + s->history_time >= x)
146 : : /* This item is still valid, and thus all following ones
147 : : * are too, so let's quit this loop */
148 : : break;
149 : :
150 : : /* Item is too old, let's drop it */
151 : 54 : REDUCE_INC(s->history_idx);
152 : :
153 : 54 : s->n_history --;
154 : : }
155 : : }
156 : :
157 : 100 : static void add_to_history(pa_smoother *s, pa_usec_t x, pa_usec_t y) {
158 : : unsigned j, i;
159 [ - + ]: 100 : pa_assert(s);
160 : :
161 : : /* First try to update an existing history entry */
162 : 100 : i = s->history_idx;
163 [ + + ]: 3357 : for (j = s->n_history; j > 0; j--) {
164 : :
165 [ + + ]: 3258 : if (s->history_x[i] == x) {
166 : 1 : s->history_y[i] = y;
167 : 100 : return;
168 : : }
169 : :
170 : 3257 : REDUCE_INC(i);
171 : : }
172 : :
173 : : /* Drop old entries */
174 : : drop_old(s, x);
175 : :
176 : : /* Calculate position for new entry */
177 : 99 : j = s->history_idx + s->n_history;
178 : 99 : REDUCE(j);
179 : :
180 : : /* Fill in entry */
181 : 99 : s->history_x[j] = x;
182 : 99 : s->history_y[j] = y;
183 : :
184 : : /* Adjust counter */
185 : 99 : s->n_history ++;
186 : :
187 : : /* And make sure we don't store more entries than fit in */
188 [ - + ]: 99 : if (s->n_history > HISTORY_MAX) {
189 : 0 : s->history_idx += s->n_history - HISTORY_MAX;
190 : 0 : REDUCE(s->history_idx);
191 : 0 : s->n_history = HISTORY_MAX;
192 : : }
193 : : }
194 : :
195 : 100 : static double avg_gradient(pa_smoother *s, pa_usec_t x) {
196 : 100 : unsigned i, j, c = 0;
197 : 100 : int64_t ax = 0, ay = 0, k, t;
198 : : double r;
199 : :
200 : : /* FIXME: Optimization: Jason Newton suggested that instead of
201 : : * going through the history on each iteration we could calculated
202 : : * avg_gradient() as we go.
203 : : *
204 : : * Second idea: it might make sense to weight history entries:
205 : : * more recent entries should matter more than old ones. */
206 : :
207 : : /* Too few measurements, assume gradient of 1 */
208 [ + + ][ # # ]: 100 : if (s->n_history < s->min_history)
209 : : return 1;
210 : :
211 : : /* First, calculate average of all measurements */
212 : 94 : i = s->history_idx;
213 [ + + ][ # # ]: 3381 : for (j = s->n_history; j > 0; j--) {
214 : :
215 : 3287 : ax += (int64_t) s->history_x[i];
216 : 3287 : ay += (int64_t) s->history_y[i];
217 : 3287 : c++;
218 : :
219 : 3287 : REDUCE_INC(i);
220 : : }
221 : :
222 [ - + ][ # # ]: 94 : pa_assert(c >= s->min_history);
223 : 94 : ax /= c;
224 : 94 : ay /= c;
225 : :
226 : : /* Now, do linear regression */
227 : 94 : k = t = 0;
228 : :
229 : 94 : i = s->history_idx;
230 [ + + ][ # # ]: 3381 : for (j = s->n_history; j > 0; j--) {
231 : : int64_t dx, dy;
232 : :
233 : 3287 : dx = (int64_t) s->history_x[i] - ax;
234 : 3287 : dy = (int64_t) s->history_y[i] - ay;
235 : :
236 : 3287 : k += dx*dy;
237 : 3287 : t += dx*dx;
238 : :
239 : 3287 : REDUCE_INC(i);
240 : : }
241 : :
242 : 94 : r = (double) k / (double) t;
243 : :
244 [ - + ][ # # ]: 100 : return (s->monotonic && r < 0) ? 0 : r;
[ # # ][ # # ]
245 : : }
246 : :
247 : 5497 : static void calc_abc(pa_smoother *s) {
248 : : pa_usec_t ex, ey, px, py;
249 : : int64_t kx, ky;
250 : : double de, dp;
251 : :
252 [ - + ]: 5497 : pa_assert(s);
253 : :
254 [ + + ]: 5497 : if (s->abc_valid)
255 : 5497 : return;
256 : :
257 : : /* We have two points: (ex|ey) and (px|py) with two gradients at
258 : : * these points de and dp. We do a polynomial
259 : : * interpolation of degree 3 with these 6 values */
260 : :
261 : 78 : ex = s->ex; ey = s->ey;
262 : 78 : px = s->px; py = s->py;
263 : 78 : de = s->de; dp = s->dp;
264 : :
265 [ - + ]: 78 : pa_assert(ex < px);
266 : :
267 : : /* To increase the dynamic range and simplify calculation, we
268 : : * move these values to the origin */
269 : 78 : kx = (int64_t) px - (int64_t) ex;
270 : 78 : ky = (int64_t) py - (int64_t) ey;
271 : :
272 : : /* Calculate a, b, c for y=ax^3+bx^2+cx */
273 : 78 : s->c = de;
274 : 78 : s->b = (((double) (3*ky)/ (double) kx - dp - (double) (2*de))) / (double) kx;
275 : 78 : s->a = (dp/(double) kx - 2*s->b - de/(double) kx) / (double) (3*kx);
276 : :
277 : 78 : s->abc_valid = TRUE;
278 : : }
279 : :
280 : 10079 : static void estimate(pa_smoother *s, pa_usec_t x, pa_usec_t *y, double *deriv) {
281 [ - + ]: 10079 : pa_assert(s);
282 [ - + ]: 10079 : pa_assert(y);
283 : :
284 [ + + ]: 10079 : if (x >= s->px) {
285 : : /* Linear interpolation right from px */
286 : : int64_t t;
287 : :
288 : : /* The requested point is right of the point where we wanted
289 : : * to be on track again, thus just linearly estimate */
290 : :
291 : 9164 : t = (int64_t) s->py + (int64_t) llrint(s->dp * (double) (x - s->px));
292 : :
293 [ - + ]: 4582 : if (t < 0)
294 : 0 : t = 0;
295 : :
296 : 4582 : *y = (pa_usec_t) t;
297 : :
298 [ + + ]: 4582 : if (deriv)
299 : 2 : *deriv = s->dp;
300 : :
301 [ - + ]: 5497 : } else if (x <= s->ex) {
302 : : /* Linear interpolation left from ex */
303 : : int64_t t;
304 : :
305 : 0 : t = (int64_t) s->ey - (int64_t) llrint(s->de * (double) (s->ex - x));
306 : :
307 [ # # ]: 0 : if (t < 0)
308 : 0 : t = 0;
309 : :
310 : 0 : *y = (pa_usec_t) t;
311 : :
312 [ # # ]: 0 : if (deriv)
313 : 0 : *deriv = s->de;
314 : :
315 : : } else {
316 : : /* Spline interpolation between ex and px */
317 : : double tx, ty;
318 : :
319 : : /* Ok, we're not yet on track, thus let's interpolate, and
320 : : * make sure that the first derivative is smooth */
321 : :
322 : 5497 : calc_abc(s);
323 : :
324 : : /* Move to origin */
325 : 5497 : tx = (double) (x - s->ex);
326 : :
327 : : /* Horner scheme */
328 : 5497 : ty = (tx * (s->c + tx * (s->b + tx * s->a)));
329 : :
330 : : /* Move back from origin */
331 : 5497 : ty += (double) s->ey;
332 : :
333 [ + - ]: 5497 : *y = ty >= 0 ? (pa_usec_t) llrint(ty) : 0;
334 : :
335 : : /* Horner scheme */
336 [ + + ]: 5497 : if (deriv)
337 : 77 : *deriv = s->c + (tx * (s->b*2 + tx * s->a*3));
338 : : }
339 : :
340 : : /* Guarantee monotonicity */
341 [ - + ]: 10079 : if (s->monotonic) {
342 : :
343 [ # # ][ # # ]: 0 : if (deriv && *deriv < 0)
344 : 0 : *deriv = 0;
345 : : }
346 : 10079 : }
347 : :
348 : 100 : void pa_smoother_put(pa_smoother *s, pa_usec_t x, pa_usec_t y) {
349 : : pa_usec_t ney;
350 : : double nde;
351 : : pa_bool_t is_new;
352 : :
353 [ - + ]: 100 : pa_assert(s);
354 : :
355 : : /* Fix up x value */
356 [ + + ]: 100 : if (s->paused)
357 : 1 : x = s->pause_time;
358 : :
359 [ + - ]: 100 : x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
360 : :
361 : 100 : is_new = x >= s->ex;
362 : :
363 [ + + ]: 100 : if (is_new) {
364 : : /* First, we calculate the position we'd estimate for x, so that
365 : : * we can adjust our position smoothly from this one */
366 : 79 : estimate(s, x, &ney, &nde);
367 : 79 : s->ex = x; s->ey = ney; s->de = nde;
368 : 79 : s->ry = y;
369 : : }
370 : :
371 : : /* Then, we add the new measurement to our history */
372 : 100 : add_to_history(s, x, y);
373 : :
374 : : /* And determine the average gradient of the history */
375 : 100 : s->dp = avg_gradient(s, x);
376 : :
377 : : /* And calculate when we want to be on track again */
378 [ + - ]: 100 : if (s->smoothing) {
379 : 100 : s->px = s->ex + s->adjust_time;
380 : 100 : s->py = s->ry + (pa_usec_t) llrint(s->dp * (double) s->adjust_time);
381 : : } else {
382 : 0 : s->px = s->ex;
383 : 0 : s->py = s->ry;
384 : : }
385 : :
386 : 100 : s->abc_valid = FALSE;
387 : :
388 : : #ifdef DEBUG_DATA
389 : : pa_log_debug("%p, put(%llu | %llu) = %llu", s, (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y);
390 : : #endif
391 : 100 : }
392 : :
393 : 10000 : pa_usec_t pa_smoother_get(pa_smoother *s, pa_usec_t x) {
394 : : pa_usec_t y;
395 : :
396 [ - + ]: 10000 : pa_assert(s);
397 : :
398 : : /* Fix up x value */
399 [ + + ]: 10000 : if (s->paused)
400 : 35 : x = s->pause_time;
401 : :
402 [ + + ]: 10000 : x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
403 : :
404 [ - + ]: 10000 : if (s->monotonic)
405 [ # # ]: 0 : if (x <= s->last_x)
406 : 0 : x = s->last_x;
407 : :
408 : 10000 : estimate(s, x, &y, NULL);
409 : :
410 [ - + ]: 10000 : if (s->monotonic) {
411 : :
412 : : /* Make sure the querier doesn't jump forth and back. */
413 : 0 : s->last_x = x;
414 : :
415 [ # # ]: 0 : if (y < s->last_y)
416 : 0 : y = s->last_y;
417 : : else
418 : 0 : s->last_y = y;
419 : : }
420 : :
421 : : #ifdef DEBUG_DATA
422 : : pa_log_debug("%p, get(%llu | %llu) = %llu", s, (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y);
423 : : #endif
424 : :
425 : 10000 : return y;
426 : : }
427 : :
428 : 0 : void pa_smoother_set_time_offset(pa_smoother *s, pa_usec_t offset) {
429 [ # # ]: 0 : pa_assert(s);
430 : :
431 : 0 : s->time_offset = offset;
432 : :
433 : : #ifdef DEBUG_DATA
434 : : pa_log_debug("offset(%llu)", (unsigned long long) offset);
435 : : #endif
436 : 0 : }
437 : :
438 : 0 : void pa_smoother_pause(pa_smoother *s, pa_usec_t x) {
439 [ # # ]: 0 : pa_assert(s);
440 : :
441 [ # # ]: 0 : if (s->paused)
442 : 0 : return;
443 : :
444 : : #ifdef DEBUG_DATA
445 : : pa_log_debug("pause(%llu)", (unsigned long long) x);
446 : : #endif
447 : :
448 : 0 : s->paused = TRUE;
449 : 0 : s->pause_time = x;
450 : : }
451 : :
452 : 100 : void pa_smoother_resume(pa_smoother *s, pa_usec_t x, pa_bool_t fix_now) {
453 [ - + ]: 100 : pa_assert(s);
454 : :
455 [ + + ]: 100 : if (!s->paused)
456 : 100 : return;
457 : :
458 [ - + ]: 1 : if (x < s->pause_time)
459 : 0 : x = s->pause_time;
460 : :
461 : : #ifdef DEBUG_DATA
462 : : pa_log_debug("resume(%llu)", (unsigned long long) x);
463 : : #endif
464 : :
465 : 1 : s->paused = FALSE;
466 : 1 : s->time_offset += x - s->pause_time;
467 : :
468 [ + - ]: 1 : if (fix_now)
469 : 1 : pa_smoother_fix_now(s);
470 : : }
471 : :
472 : 1 : void pa_smoother_fix_now(pa_smoother *s) {
473 [ - + ]: 1 : pa_assert(s);
474 : :
475 : 1 : s->px = s->ex;
476 : 1 : s->py = s->ry;
477 : 1 : }
478 : :
479 : 0 : pa_usec_t pa_smoother_translate(pa_smoother *s, pa_usec_t x, pa_usec_t y_delay) {
480 : : pa_usec_t ney;
481 : : double nde;
482 : :
483 [ # # ]: 0 : pa_assert(s);
484 : :
485 : : /* Fix up x value */
486 [ # # ]: 0 : if (s->paused)
487 : 0 : x = s->pause_time;
488 : :
489 [ # # ]: 0 : x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
490 : :
491 : 0 : estimate(s, x, &ney, &nde);
492 : :
493 : : /* Play safe and take the larger gradient, so that we wakeup
494 : : * earlier when this is used for sleeping */
495 [ # # ]: 0 : if (s->dp > nde)
496 : 0 : nde = s->dp;
497 : :
498 : : #ifdef DEBUG_DATA
499 : : pa_log_debug("translate(%llu) = %llu (%0.2f)", (unsigned long long) y_delay, (unsigned long long) ((double) y_delay / nde), nde);
500 : : #endif
501 : :
502 : 0 : return (pa_usec_t) llrint((double) y_delay / nde);
503 : : }
504 : :
505 : 1 : void pa_smoother_reset(pa_smoother *s, pa_usec_t time_offset, pa_bool_t paused) {
506 [ - + ]: 1 : pa_assert(s);
507 : :
508 : 1 : s->px = s->py = 0;
509 : 1 : s->dp = 1;
510 : :
511 : 1 : s->ex = s->ey = s->ry = 0;
512 : 1 : s->de = 1;
513 : :
514 : 1 : s->history_idx = 0;
515 : 1 : s->n_history = 0;
516 : :
517 : 1 : s->last_y = s->last_x = 0;
518 : :
519 : 1 : s->abc_valid = FALSE;
520 : :
521 : 1 : s->paused = paused;
522 : 1 : s->time_offset = s->pause_time = time_offset;
523 : :
524 : : #ifdef DEBUG_DATA
525 : : pa_log_debug("reset()");
526 : : #endif
527 : 1 : }
|
