src/pulsecore/time-smoother.c

   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 recieved 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 surpress
  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 polonyomial interpolation step */
  87
  88     pa_usec_t pause_time;
  89
  90     unsigned min_history;
  91 };
  92
  93 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     pa_assert(adjust_time > 0);
 105     pa_assert(history_time > 0);
 106     pa_assert(min_history >= 2);
 107     pa_assert(min_history <= HISTORY_MAX);
 108
 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;
 115
 116     pa_smoother_reset(s, time_offset, paused);
 117
 118     return s;
 119 }
 120
 121 void pa_smoother_free(pa_smoother* s) {
 122     pa_assert(s);
 123
 124     pa_xfree(s);
 125 }
 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     while (s->n_history > s->min_history) {
 144
 145         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         REDUCE_INC(s->history_idx);
 152
 153         s->n_history --;
 154     }
 155 }
 156
 157 static void add_to_history(pa_smoother *s, pa_usec_t x, pa_usec_t y) {
 158     unsigned j, i;
 159     pa_assert(s);
 160
 161     /* First try to update an existing history entry */
 162     i = s->history_idx;
 163     for (j = s->n_history; j > 0; j--) {
 164
 165         if (s->history_x[i] == x) {
 166             s->history_y[i] = y;
 167             return;
 168         }
 169
 170         REDUCE_INC(i);
 171     }
 172
 173     /* Drop old entries */
 174     drop_old(s, x);
 175
 176     /* Calculate position for new entry */
 177     j = s->history_idx + s->n_history;
 178     REDUCE(j);
 179
 180     /* Fill in entry */
 181     s->history_x[j] = x;
 182     s->history_y[j] = y;
 183
 184     /* Adjust counter */
 185     s->n_history ++;
 186
 187     /* And make sure we don't store more entries than fit in */
 188     if (s->n_history > HISTORY_MAX) {
 189         s->history_idx += s->n_history - HISTORY_MAX;
 190         REDUCE(s->history_idx);
 191         s->n_history = HISTORY_MAX;
 192     }
 193 }
 194
 195 static double avg_gradient(pa_smoother *s, pa_usec_t x) {
 196     unsigned i, j, c = 0;
 197     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     if (s->n_history < s->min_history)
 209         return 1;
 210
 211     /* First, calculate average of all measurements */
 212     i = s->history_idx;
 213     for (j = s->n_history; j > 0; j--) {
 214
 215         ax += (int64_t) s->history_x[i];
 216         ay += (int64_t) s->history_y[i];
 217         c++;
 218
 219         REDUCE_INC(i);
 220     }
 221
 222     pa_assert(c >= s->min_history);
 223     ax /= c;
 224     ay /= c;
 225
 226     /* Now, do linear regression */
 227     k = t = 0;
 228
 229     i = s->history_idx;
 230     for (j = s->n_history; j > 0; j--) {
 231         int64_t dx, dy;
 232
 233         dx = (int64_t) s->history_x[i] - ax;
 234         dy = (int64_t) s->history_y[i] - ay;
 235
 236         k += dx*dy;
 237         t += dx*dx;
 238
 239         REDUCE_INC(i);
 240     }
 241
 242     r = (double) k / (double) t;
 243
 244     return (s->monotonic && r < 0) ? 0 : r;
 245 }
 246
 247 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     pa_assert(s);
 253
 254     if (s->abc_valid)
 255         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     ex = s->ex; ey = s->ey;
 262     px = s->px; py = s->py;
 263     de = s->de; dp = s->dp;
 264
 265     pa_assert(ex < px);
 266
 267     /* To increase the dynamic range and symplify calculation, we
 268      * move these values to the origin */
 269     kx = (int64_t) px - (int64_t) ex;
 270     ky = (int64_t) py - (int64_t) ey;
 271
 272     /* Calculate a, b, c for y=ax^3+bx^2+cx */
 273     s->c = de;
 274     s->b = (((double) (3*ky)/ (double) kx - dp - (double) (2*de))) / (double) kx;
 275     s->a = (dp/(double) kx - 2*s->b - de/(double) kx) / (double) (3*kx);
 276
 277     s->abc_valid = TRUE;
 278 }
 279
 280 static void estimate(pa_smoother *s, pa_usec_t x, pa_usec_t *y, double *deriv) {
 281     pa_assert(s);
 282     pa_assert(y);
 283
 284     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         t = (int64_t) s->py + (int64_t) llrint(s->dp * (double) (x - s->px));
 292
 293         if (t < 0)
 294             t = 0;
 295
 296         *y = (pa_usec_t) t;
 297
 298         if (deriv)
 299             *deriv = s->dp;
 300
 301     } else if (x <= s->ex) {
 302         /* Linear interpolation left from ex */
 303         int64_t t;
 304
 305         t = (int64_t) s->ey - (int64_t) llrint(s->de * (double) (s->ex - x));
 306
 307         if (t < 0)
 308             t = 0;
 309
 310         *y = (pa_usec_t) t;
 311
 312         if (deriv)
 313             *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         calc_abc(s);
 323
 324         /* Move to origin */
 325         tx = (double) (x - s->ex);
 326
 327         /* Horner scheme */
 328         ty = (tx * (s->c + tx * (s->b + tx * s->a)));
 329
 330         /* Move back from origin */
 331         ty += (double) s->ey;
 332
 333         *y = ty >= 0 ? (pa_usec_t) llrint(ty) : 0;
 334
 335         /* Horner scheme */
 336         if (deriv)
 337             *deriv = s->c + (tx * (s->b*2 + tx * s->a*3));
 338     }
 339
 340     /* Guarantee monotonicity */
 341     if (s->monotonic) {
 342
 343         if (deriv && *deriv < 0)
 344             *deriv = 0;
 345     }
 346 }
 347
 348 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     pa_assert(s);
 354
 355     /* Fix up x value */
 356     if (s->paused)
 357         x = s->pause_time;
 358
 359     x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
 360
 361     is_new = x >= s->ex;
 362
 363     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         estimate(s, x, &ney, &nde);
 367         s->ex = x; s->ey = ney; s->de = nde;
 368         s->ry = y;
 369     }
 370
 371     /* Then, we add the new measurement to our history */
 372     add_to_history(s, x, y);
 373
 374     /* And determine the average gradient of the history */
 375     s->dp = avg_gradient(s, x);
 376
 377     /* And calculate when we want to be on track again */
 378     if (s->smoothing) {
 379         s->px = s->ex + s->adjust_time;
 380         s->py = s->ry + (pa_usec_t) llrint(s->dp * (double) s->adjust_time);
 381     } else {
 382         s->px = s->ex;
 383         s->py = s->ry;
 384     }
 385
 386     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 }
 392
 393 pa_usec_t pa_smoother_get(pa_smoother *s, pa_usec_t x) {
 394     pa_usec_t y;
 395
 396     pa_assert(s);
 397
 398     /* Fix up x value */
 399     if (s->paused)
 400         x = s->pause_time;
 401
 402     x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
 403
 404     if (s->monotonic)
 405         if (x <= s->last_x)
 406             x = s->last_x;
 407
 408     estimate(s, x, &y, NULL);
 409
 410     if (s->monotonic) {
 411
 412         /* Make sure the querier doesn't jump forth and back. */
 413         s->last_x = x;
 414
 415         if (y < s->last_y)
 416             y = s->last_y;
 417         else
 418             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     return y;
 426 }
 427
 428 void pa_smoother_set_time_offset(pa_smoother *s, pa_usec_t offset) {
 429     pa_assert(s);
 430
 431     s->time_offset = offset;
 432
 433 #ifdef DEBUG_DATA
 434     pa_log_debug("offset(%llu)", (unsigned long long) offset);
 435 #endif
 436 }
 437
 438 void pa_smoother_pause(pa_smoother *s, pa_usec_t x) {
 439     pa_assert(s);
 440
 441     if (s->paused)
 442         return;
 443
 444 #ifdef DEBUG_DATA
 445     pa_log_debug("pause(%llu)", (unsigned long long) x);
 446 #endif
 447
 448     s->paused = TRUE;
 449     s->pause_time = x;
 450 }
 451
 452 void pa_smoother_resume(pa_smoother *s, pa_usec_t x, pa_bool_t fix_now) {
 453     pa_assert(s);
 454
 455     if (!s->paused)
 456         return;
 457
 458     if (x < s->pause_time)
 459         x = s->pause_time;
 460
 461 #ifdef DEBUG_DATA
 462     pa_log_debug("resume(%llu)", (unsigned long long) x);
 463 #endif
 464
 465     s->paused = FALSE;
 466     s->time_offset += x - s->pause_time;
 467
 468     if (fix_now)
 469         pa_smoother_fix_now(s);
 470 }
 471
 472 void pa_smoother_fix_now(pa_smoother *s) {
 473     pa_assert(s);
 474
 475     s->px = s->ex;
 476     s->py = s->ry;
 477 }
 478
 479 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     pa_assert(s);
 484
 485     /* Fix up x value */
 486     if (s->paused)
 487         x = s->pause_time;
 488
 489     x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
 490
 491     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     if (s->dp > nde)
 496         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     return (pa_usec_t) llrint((double) y_delay / nde);
 503 }
 504
 505 void pa_smoother_reset(pa_smoother *s, pa_usec_t time_offset, pa_bool_t paused) {
 506     pa_assert(s);
 507
 508     s->px = s->py = 0;
 509     s->dp = 1;
 510
 511     s->ex = s->ey = s->ry = 0;
 512     s->de = 1;
 513
 514     s->history_idx = 0;
 515     s->n_history = 0;
 516
 517     s->last_y = s->last_x = 0;
 518
 519     s->abc_valid = FALSE;
 520
 521     s->paused = paused;
 522     s->time_offset = s->pause_time = time_offset;
 523
 524 #ifdef DEBUG_DATA
 525     pa_log_debug("reset()");
 526 #endif
 527 }