code.delx.au - pulseaudio/blob - src/pulsecore/time-smoother.c

   1 /* $Id$ */
   2
   3 /***
   4   This file is part of PulseAudio.
   5
   6   Copyright 2007 Lennart Poettering
   7
   8   PulseAudio is free software; you can redistribute it and/or modify
   9   it under the terms of the GNU Lesser General Public License as
  10   published by the Free Software Foundation; either version 2.1 of the
  11   License, or (at your option) any later version.
  12
  13   PulseAudio is distributed in the hope that it will be useful, but
  14   WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  16   Lesser General Public License for more details.
  17
  18   You should have received a copy of the GNU Lesser General Public
  19   License along with PulseAudio; if not, write to the Free Software
  20   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
  21   USA.
  22 ***/
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <stdio.h>
  29
  30 #include <pulse/sample.h>
  31 #include <pulse/xmalloc.h>
  32
  33 #include <pulsecore/macro.h>
  34
  35 #include "time-smoother.h"
  36
  37 #define HISTORY_MAX 64
  38
  39 /*
  40  * Implementation of a time smoothing algorithm to synchronize remote
  41  * clocks to a local one. Evens out noise, adjusts to clock skew and
  42  * allows cheap estimations of the remote time while clock updates may
  43  * be seldom and recieved in non-equidistant intervals.
  44  *
  45  * Basically, we estimate the gradient of received clock samples in a
  46  * certain history window (of size 'history_time') with linear
  47  * regression. With that info we estimate the remote time in
  48  * 'adjust_time' ahead and smoothen our current estimation function
  49  * towards that point with a 3rd order polynomial interpolation with
  50  * fitting derivatives. (more or less a b-spline)
  51  *
  52  * The larger 'history_time' is chosen the better we will surpress
  53  * noise -- but we'll adjust to clock skew slower..
  54  *
  55  * The larger 'adjust_time' is chosen the smoother our estimation
  56  * function will be -- but we'll adjust to clock skew slower, too.
  57  *
  58  * If 'monotonic' is TRUE the resulting estimation function is
  59  * guaranteed to be monotonic.
  60  */
  61
  62 struct pa_smoother {
  63     pa_usec_t adjust_time, history_time;
  64
  65     pa_usec_t time_offset;
  66
  67     pa_usec_t px, py;     /* Point p, where we want to reach stability */
  68     double dp;            /* Gradient we want at point p */
  69
  70     pa_usec_t ex, ey;     /* Point e, which we estimated before and need to smooth to */
  71     double de;            /* Gradient we estimated for point e */
  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;
  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;
  83
  84     pa_bool_t monotonic:1;
  85     pa_bool_t paused:1;
  86
  87     pa_usec_t pause_time;
  88 };
  89
  90 pa_smoother* pa_smoother_new(pa_usec_t adjust_time, pa_usec_t history_time, pa_bool_t monotonic) {
  91     pa_smoother *s;
  92
  93     pa_assert(adjust_time > 0);
  94     pa_assert(history_time > 0);
  95
  96     s = pa_xnew(pa_smoother, 1);
  97     s->adjust_time = adjust_time;
  98     s->history_time = history_time;
  99     s->time_offset = 0;
 100     s->monotonic = monotonic;
 101
 102     s->px = s->py = 0;
 103     s->dp = 1;
 104
 105     s->ex = s->ey = 0;
 106     s->de = 1;
 107
 108     s->history_idx = 0;
 109     s->n_history = 0;
 110
 111     s->last_y = 0;
 112
 113     s->abc_valid = FALSE;
 114
 115     s->paused = FALSE;
 116
 117     return s;
 118 }
 119
 120 void pa_smoother_free(pa_smoother* s) {
 121     pa_assert(s);
 122
 123     pa_xfree(s);
 124 }
 125
 126 #define REDUCE(x)                               \
 127     do {                                        \
 128         x = (x) % HISTORY_MAX;                  \
 129     } while(FALSE)
 130
 131 #define REDUCE_INC(x)                           \
 132     do {                                        \
 133         x = ((x)+1) % HISTORY_MAX;              \
 134     } while(FALSE)
 135
 136
 137 static void drop_old(pa_smoother *s, pa_usec_t x) {
 138
 139     /* Drop items from history which are too old, but make sure to
 140      * always keep two entries in the history */
 141
 142     while (s->n_history > 2) {
 143
 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 */
 147             break;
 148
 149         /* Item is too old, let's drop it */
 150         REDUCE_INC(s->history_idx);
 151
 152         s->n_history --;
 153     }
 154 }
 155
 156 static void add_to_history(pa_smoother *s, pa_usec_t x, pa_usec_t y) {
 157     unsigned j, i;
 158     pa_assert(s);
 159
 160     /* First try to update an existing history entry */
 161     i = s->history_idx;
 162     for (j = s->n_history; j > 0; j--) {
 163
 164         if (s->history_x[i] == x) {
 165             s->history_y[i] = y;
 166             return;
 167         }
 168
 169         REDUCE_INC(i);
 170     }
 171
 172     /* Drop old entries */
 173     drop_old(s, x);
 174
 175     /* Calculate position for new entry */
 176     j = s->history_idx + s->n_history;
 177     REDUCE(j);
 178
 179     /* Fill in entry */
 180     s->history_x[j] = x;
 181     s->history_y[j] = y;
 182
 183     /* Adjust counter */
 184     s->n_history ++;
 185
 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;
 191     }
 192 }
 193
 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;
 197     double r;
 198
 199     drop_old(s, x);
 200
 201     /* First, calculate average of all measurements */
 202     i = s->history_idx;
 203     for (j = s->n_history; j > 0; j--) {
 204
 205         ax += s->history_x[i];
 206         ay += s->history_y[i];
 207         c++;
 208
 209         REDUCE_INC(i);
 210     }
 211
 212     /* Too few measurements, assume gradient of 1 */
 213     if (c < 2)
 214         return 1;
 215
 216     ax /= c;
 217     ay /= c;
 218
 219     /* Now, do linear regression */
 220     k = t = 0;
 221
 222     i = s->history_idx;
 223     for (j = s->n_history; j > 0; j--) {
 224         int64_t dx, dy;
 225
 226         dx = (int64_t) s->history_x[i] - ax;
 227         dy = (int64_t) s->history_y[i] - ay;
 228
 229         k += dx*dy;
 230         t += dx*dx;
 231
 232         REDUCE_INC(i);
 233     }
 234
 235     r = (double) k / t;
 236
 237     return (s->monotonic && r < 0) ? 0 : r;
 238 }
 239
 240 static void estimate(pa_smoother *s, pa_usec_t x, pa_usec_t *y, double *deriv) {
 241     pa_assert(s);
 242     pa_assert(y);
 243
 244     if (x >= s->px) {
 245         int64_t t;
 246
 247         /* The requested point is right of the point where we wanted
 248          * to be on track again, thus just linearly estimate */
 249
 250         t = (int64_t) s->py + (int64_t) (s->dp * (x - s->px));
 251
 252         if (t < 0)
 253             t = 0;
 254
 255         *y = (pa_usec_t) t;
 256
 257         if (deriv)
 258             *deriv = s->dp;
 259
 260     } else {
 261
 262         if (!s->abc_valid) {
 263             pa_usec_t ex, ey, px, py;
 264             int64_t kx, ky;
 265             double de, dp;
 266
 267             /* Ok, we're not yet on track, thus let's interpolate, and
 268              * make sure that the first derivative is smooth */
 269
 270             /* We have two points: (ex|ey) and (px|py) with two gradients
 271              * at these points de and dp. We do a polynomial interpolation
 272              * of degree 3 with these 6 values */
 273
 274             ex = s->ex; ey = s->ey;
 275             px = s->px; py = s->py;
 276             de = s->de; dp = s->dp;
 277
 278             pa_assert(ex < px);
 279
 280             /* To increase the dynamic range and symplify calculation, we
 281              * move these values to the origin */
 282             kx = (int64_t) px - (int64_t) ex;
 283             ky = (int64_t) py - (int64_t) ey;
 284
 285             /* Calculate a, b, c for y=ax^3+b^2+cx */
 286             s->c = de;
 287             s->b = (((double) (3*ky)/kx - dp - 2*de)) / kx;
 288             s->a = (dp/kx - 2*s->b - de/kx) / (3*kx);
 289
 290             s->abc_valid = TRUE;
 291         }
 292
 293         /* Move to origin */
 294         x -= s->ex;
 295
 296         /* Horner scheme */
 297         *y = (pa_usec_t) ((double) x * (s->c + (double) x * (s->b + (double) x * s->a)));
 298
 299         /* Move back from origin */
 300         *y += s->ey;
 301
 302         /* Horner scheme */
 303         if (deriv)
 304             *deriv = s->c + ((double) x * (s->b*2 + (double) x * s->a*3));
 305     }
 306
 307     /* Guarantee monotonicity */
 308     if (s->monotonic) {
 309
 310         if (*y < s->last_y)
 311             *y = s->last_y;
 312         else
 313             s->last_y = *y;
 314
 315         if (deriv && *deriv < 0)
 316             *deriv = 0;
 317     }
 318 }
 319
 320 void pa_smoother_put(pa_smoother *s, pa_usec_t x, pa_usec_t y) {
 321     pa_usec_t ney;
 322     double nde;
 323
 324     pa_assert(s);
 325
 326     /* Fix up x value */
 327     if (s->paused)
 328         x = s->pause_time;
 329
 330     x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
 331
 332     pa_assert(x >= s->ex);
 333
 334     /* First, we calculate the position we'd estimate for x, so that
 335      * we can adjust our position smoothly from this one */
 336     estimate(s, x, &ney, &nde);
 337     s->ex = x; s->ey = ney; s->de = nde;
 338
 339     /* Then, we add the new measurement to our history */
 340     add_to_history(s, x, y);
 341
 342     /* And determine the average gradient of the history */
 343     s->dp = avg_gradient(s, x);
 344
 345     /* And calculate when we want to be on track again */
 346     s->px = x + s->adjust_time;
 347     s->py = y + s->dp *s->adjust_time;
 348
 349     s->abc_valid = FALSE;
 350
 351 /*     pa_log_debug("put(%llu | %llu) = %llu", (unsigned long long)  (x + s->time_offset), (unsigned long long) x, (unsigned long long) y); */
 352 }
 353
 354 pa_usec_t pa_smoother_get(pa_smoother *s, pa_usec_t x) {
 355     pa_usec_t y;
 356
 357     pa_assert(s);
 358
 359     /* Fix up x value */
 360     if (s->paused)
 361         x = s->pause_time;
 362
 363     x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
 364     pa_assert(x >= s->ex);
 365
 366     estimate(s, x, &y, NULL);
 367
 368 /*     pa_log_debug("get(%llu | %llu) = %llu", (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y); */
 369
 370     return y;
 371 }
 372
 373 void pa_smoother_set_time_offset(pa_smoother *s, pa_usec_t offset) {
 374     pa_assert(s);
 375
 376     s->time_offset = offset;
 377
 378 /*     pa_log_debug("offset(%llu)", (unsigned long long) offset); */
 379 }
 380
 381 void pa_smoother_pause(pa_smoother *s, pa_usec_t x) {
 382     pa_assert(s);
 383
 384     if (s->paused)
 385         return;
 386
 387 /*     pa_log_debug("pause(%llu)", (unsigned long long)  x); */
 388
 389     s->paused = TRUE;
 390     s->pause_time = x;
 391 }
 392
 393 void pa_smoother_resume(pa_smoother *s, pa_usec_t x) {
 394     pa_assert(s);
 395
 396     if (!s->paused)
 397         return;
 398
 399     pa_assert(x >= s->pause_time);
 400
 401 /*     pa_log_debug("resume(%llu)", (unsigned long long) x); */
 402
 403     s->paused = FALSE;
 404     s->time_offset += x - s->pause_time;
 405 }
 406
 407 pa_usec_t pa_smoother_translate(pa_smoother *s, pa_usec_t x, pa_usec_t y_delay) {
 408     pa_usec_t ney;
 409     double nde;
 410
 411     pa_assert(s);
 412
 413     /* Fix up x value */
 414     if (s->paused)
 415         x = s->pause_time;
 416
 417     x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
 418
 419     pa_assert(x >= s->ex);
 420
 421     estimate(s, x, &ney, &nde);
 422
 423 /*     pa_log_debug("translate(%llu) = %llu (%0.2f)", (unsigned long long) y_delay, (unsigned long long) ((double) y_delay / s->dp), s->dp); */
 424
 425     return (pa_usec_t) ((double) y_delay / s->dp);
 426 }