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