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