2 This file is part of PulseAudio.
4 Copyright 2007 Lennart Poettering
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.
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.
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
29 #include <pulse/sample.h>
30 #include <pulse/xmalloc.h>
32 #include <pulsecore/macro.h>
34 #include "time-smoother.h"
36 #define HISTORY_MAX 64
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.
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)
51 * The larger 'history_time' is chosen the better we will suppress
52 * noise -- but we'll adjust to clock skew slower..
54 * The larger 'adjust_time' is chosen the smoother our estimation
55 * function will be -- but we'll adjust to clock skew slower, too.
57 * If 'monotonic' is true the resulting estimation function is
58 * guaranteed to be monotonic.
62 pa_usec_t adjust_time, history_time;
64 pa_usec_t time_offset;
66 pa_usec_t px, py; /* Point p, where we want to reach stability */
67 double dp; /* Gradient we want at point p */
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 */
73 /* History of last measurements */
74 pa_usec_t history_x[HISTORY_MAX], history_y[HISTORY_MAX];
75 unsigned history_idx, n_history;
77 /* To even out for monotonicity */
78 pa_usec_t last_y, last_x;
80 /* Cached parameters for our interpolation polynomial y=ax^3+b^2+cx */
86 bool smoothing:1; /* If false we skip the polynomial interpolation step */
93 pa_smoother* pa_smoother_new(
94 pa_usec_t adjust_time,
95 pa_usec_t history_time,
99 pa_usec_t time_offset,
104 pa_assert(adjust_time > 0);
105 pa_assert(history_time > 0);
106 pa_assert(min_history >= 2);
107 pa_assert(min_history <= HISTORY_MAX);
109 s = pa_xnew(pa_smoother, 1);
110 s->adjust_time = adjust_time;
111 s->history_time = history_time;
112 s->min_history = min_history;
113 s->monotonic = monotonic;
114 s->smoothing = smoothing;
116 pa_smoother_reset(s, time_offset, paused);
121 void pa_smoother_free(pa_smoother* s) {
129 x = (x) % HISTORY_MAX; \
132 #define REDUCE_INC(x) \
134 x = ((x)+1) % HISTORY_MAX; \
137 static void drop_old(pa_smoother *s, pa_usec_t x) {
139 /* Drop items from history which are too old, but make sure to
140 * always keep min_history in the history */
142 while (s->n_history > s->min_history) {
144 if (s->history_x[s->history_idx] + s->history_time >= x)
145 /* This item is still valid, and thus all following ones
146 * are too, so let's quit this loop */
149 /* Item is too old, let's drop it */
150 REDUCE_INC(s->history_idx);
156 static void add_to_history(pa_smoother *s, pa_usec_t x, pa_usec_t y) {
160 /* First try to update an existing history entry */
162 for (j = s->n_history; j > 0; j--) {
164 if (s->history_x[i] == x) {
172 /* Drop old entries */
175 /* Calculate position for new entry */
176 j = s->history_idx + s->n_history;
186 /* And make sure we don't store more entries than fit in */
187 if (s->n_history > HISTORY_MAX) {
188 s->history_idx += s->n_history - HISTORY_MAX;
189 REDUCE(s->history_idx);
190 s->n_history = HISTORY_MAX;
194 static double avg_gradient(pa_smoother *s, pa_usec_t x) {
195 unsigned i, j, c = 0;
196 int64_t ax = 0, ay = 0, k, t;
199 /* FIXME: Optimization: Jason Newton suggested that instead of
200 * going through the history on each iteration we could calculated
201 * avg_gradient() as we go.
203 * Second idea: it might make sense to weight history entries:
204 * more recent entries should matter more than old ones. */
206 /* Too few measurements, assume gradient of 1 */
207 if (s->n_history < s->min_history)
210 /* First, calculate average of all measurements */
212 for (j = s->n_history; j > 0; j--) {
214 ax += (int64_t) s->history_x[i];
215 ay += (int64_t) s->history_y[i];
221 pa_assert(c >= s->min_history);
225 /* Now, do linear regression */
229 for (j = s->n_history; j > 0; j--) {
232 dx = (int64_t) s->history_x[i] - ax;
233 dy = (int64_t) s->history_y[i] - ay;
241 r = (double) k / (double) t;
243 return (s->monotonic && r < 0) ? 0 : r;
246 static void calc_abc(pa_smoother *s) {
247 pa_usec_t ex, ey, px, py;
256 /* We have two points: (ex|ey) and (px|py) with two gradients at
257 * these points de and dp. We do a polynomial
258 * interpolation of degree 3 with these 6 values */
260 ex = s->ex; ey = s->ey;
261 px = s->px; py = s->py;
262 de = s->de; dp = s->dp;
266 /* To increase the dynamic range and simplify calculation, we
267 * move these values to the origin */
268 kx = (int64_t) px - (int64_t) ex;
269 ky = (int64_t) py - (int64_t) ey;
271 /* Calculate a, b, c for y=ax^3+bx^2+cx */
273 s->b = (((double) (3*ky)/ (double) kx - dp - (double) (2*de))) / (double) kx;
274 s->a = (dp/(double) kx - 2*s->b - de/(double) kx) / (double) (3*kx);
279 static void estimate(pa_smoother *s, pa_usec_t x, pa_usec_t *y, double *deriv) {
284 /* Linear interpolation right from px */
287 /* The requested point is right of the point where we wanted
288 * to be on track again, thus just linearly estimate */
290 t = (int64_t) s->py + (int64_t) llrint(s->dp * (double) (x - s->px));
300 } else if (x <= s->ex) {
301 /* Linear interpolation left from ex */
304 t = (int64_t) s->ey - (int64_t) llrint(s->de * (double) (s->ex - x));
315 /* Spline interpolation between ex and px */
318 /* Ok, we're not yet on track, thus let's interpolate, and
319 * make sure that the first derivative is smooth */
324 tx = (double) (x - s->ex);
327 ty = (tx * (s->c + tx * (s->b + tx * s->a)));
329 /* Move back from origin */
330 ty += (double) s->ey;
332 *y = ty >= 0 ? (pa_usec_t) llrint(ty) : 0;
336 *deriv = s->c + (tx * (s->b*2 + tx * s->a*3));
339 /* Guarantee monotonicity */
342 if (deriv && *deriv < 0)
347 void pa_smoother_put(pa_smoother *s, pa_usec_t x, pa_usec_t y) {
358 x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
363 /* First, we calculate the position we'd estimate for x, so that
364 * we can adjust our position smoothly from this one */
365 estimate(s, x, &ney, &nde);
366 s->ex = x; s->ey = ney; s->de = nde;
370 /* Then, we add the new measurement to our history */
371 add_to_history(s, x, y);
373 /* And determine the average gradient of the history */
374 s->dp = avg_gradient(s, x);
376 /* And calculate when we want to be on track again */
378 s->px = s->ex + s->adjust_time;
379 s->py = s->ry + (pa_usec_t) llrint(s->dp * (double) s->adjust_time);
385 s->abc_valid = false;
388 pa_log_debug("%p, put(%llu | %llu) = %llu", s, (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y);
392 pa_usec_t pa_smoother_get(pa_smoother *s, pa_usec_t x) {
401 x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
407 estimate(s, x, &y, NULL);
411 /* Make sure the querier doesn't jump forth and back. */
421 pa_log_debug("%p, get(%llu | %llu) = %llu", s, (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y);
427 void pa_smoother_set_time_offset(pa_smoother *s, pa_usec_t offset) {
430 s->time_offset = offset;
433 pa_log_debug("offset(%llu)", (unsigned long long) offset);
437 void pa_smoother_pause(pa_smoother *s, pa_usec_t x) {
444 pa_log_debug("pause(%llu)", (unsigned long long) x);
451 void pa_smoother_resume(pa_smoother *s, pa_usec_t x, bool fix_now) {
457 if (x < s->pause_time)
461 pa_log_debug("resume(%llu)", (unsigned long long) x);
465 s->time_offset += x - s->pause_time;
468 pa_smoother_fix_now(s);
471 void pa_smoother_fix_now(pa_smoother *s) {
478 pa_usec_t pa_smoother_translate(pa_smoother *s, pa_usec_t x, pa_usec_t y_delay) {
488 x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
490 estimate(s, x, &ney, &nde);
492 /* Play safe and take the larger gradient, so that we wakeup
493 * earlier when this is used for sleeping */
498 pa_log_debug("translate(%llu) = %llu (%0.2f)", (unsigned long long) y_delay, (unsigned long long) ((double) y_delay / nde), nde);
501 return (pa_usec_t) llrint((double) y_delay / nde);
504 void pa_smoother_reset(pa_smoother *s, pa_usec_t time_offset, bool paused) {
510 s->ex = s->ey = s->ry = 0;
516 s->last_y = s->last_x = 0;
518 s->abc_valid = false;
521 s->time_offset = s->pause_time = time_offset;
524 pa_log_debug("reset()");