1/***
2  This file is part of PulseAudio.
3
4  PulseAudio is free software; you can redistribute it and/or modify
5  it under the terms of the GNU Lesser General Public License as
6  published by the Free Software Foundation; either version 2.1 of the
7  License, or (at your option) any later version.
8
9  PulseAudio is distributed in the hope that it will be useful, but
10  WITHOUT ANY WARRANTY; without even the implied warranty of
11  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12  Lesser General Public License for more details.
13
14  You should have received a copy of the GNU Lesser General Public
15  License along with PulseAudio; if not, see <http://www.gnu.org/licenses/>.
16***/
17
18/* The code in this file is based on the theoretical background found at
19 * https://www.freedesktop.org/software/pulseaudio/misc/rate_estimator.odt.
20 * The theory has never been reviewed, so it may be inaccurate in places. */
21
22#ifdef HAVE_CONFIG_H
23#include <config.h>
24#endif
25
26#include <pulsecore/macro.h>
27#include <pulse/sample.h>
28#include <pulse/xmalloc.h>
29#include <pulse/timeval.h>
30
31#include "time-smoother_2.h"
32
33struct pa_smoother_2 {
34    /* Values set when the smoother is created */
35    pa_usec_t smoother_window_time;
36    uint32_t rate;
37    uint32_t frame_size;
38
39    /* USB hack parameters */
40    bool usb_hack;
41    bool enable_usb_hack;
42    uint32_t hack_threshold;
43
44    /* Smoother state */
45    bool init;
46    bool paused;
47
48    /* Current byte count start value */
49    double start_pos;
50    /* System time corresponding to start_pos */
51    pa_usec_t start_time;
52    /* Conversion factor between time domains */
53    double time_factor;
54
55    /* Used if the smoother is paused while still in init state */
56    pa_usec_t fixup_time;
57
58    /* Time offset for USB devices */
59    int64_t time_offset;
60
61    /* Various time stamps */
62    pa_usec_t resume_time;
63    pa_usec_t pause_time;
64    pa_usec_t smoother_start_time;
65    pa_usec_t last_time;
66
67    /* Variables used for Kalman filter */
68    double time_variance;
69    double time_factor_variance;
70    double kalman_variance;
71
72    /* Variables used for low pass filter */
73    double drift_filter;
74    double drift_filter_1;
75};
76
77/* Create new smoother */
78pa_smoother_2* pa_smoother_2_new(pa_usec_t window, pa_usec_t time_stamp, uint32_t frame_size, uint32_t rate) {
79    pa_smoother_2 *s;
80
81    pa_assert(window > 0);
82
83    s = pa_xnew(pa_smoother_2, 1);
84    s->enable_usb_hack = false;
85    s->usb_hack = false;
86    s->hack_threshold = 0;
87    s->smoother_window_time = window;
88    s->rate = rate;
89    s->frame_size = frame_size;
90
91    pa_smoother_2_reset(s, time_stamp);
92
93    return s;
94}
95
96/* Free the smoother */
97void pa_smoother_2_free(pa_smoother_2* s) {
98
99    pa_assert(s);
100
101    pa_xfree(s);
102}
103
104void pa_smoother_2_set_rate(pa_smoother_2 *s, pa_usec_t time_stamp, uint32_t rate) {
105
106    pa_assert(s);
107    pa_assert(rate > 0);
108
109    /* If the rate has changed, data in the smoother will be invalid,
110     * therefore also reset the smoother */
111    if (rate != s->rate) {
112        s->rate = rate;
113        pa_smoother_2_reset(s, time_stamp);
114    }
115}
116
117void pa_smoother_2_set_sample_spec(pa_smoother_2 *s, pa_usec_t time_stamp, pa_sample_spec *spec) {
118    size_t frame_size;
119
120    pa_assert(s);
121    pa_assert(pa_sample_spec_valid(spec));
122
123    /* If the sample spec has changed, data in the smoother will be invalid,
124     * therefore also reset the smoother */
125    frame_size = pa_frame_size(spec);
126    if (frame_size != s->frame_size || spec->rate != s->rate) {
127        s->frame_size = frame_size;
128        s->rate = spec->rate;
129        pa_smoother_2_reset(s, time_stamp);
130    }
131}
132
133/* Add a new data point and re-calculate time conversion factor */
134void pa_smoother_2_put(pa_smoother_2 *s, pa_usec_t time_stamp, int64_t byte_count) {
135    double byte_difference, iteration_time;
136    double time_delta_system, time_delta_card, drift, filter_constant, filter_constant_1;
137    double temp, filtered_time_delta_card, expected_time_delta_card;
138
139    pa_assert(s);
140
141    /* Smoother is paused, nothing to do */
142    if (s->paused)
143        return;
144
145    /* Initial setup or resume */
146    if PA_UNLIKELY((s->init)) {
147        s->resume_time = time_stamp;
148
149        /* We have no data yet, nothing to do */
150        if (byte_count <= 0)
151            return;
152
153        /* Now we are playing/recording.
154         * Get fresh time stamps and save the start count */
155        s->start_pos = (double)byte_count;
156        s->last_time = time_stamp;
157        s->start_time = time_stamp;
158        s->smoother_start_time = time_stamp;
159
160        s->usb_hack = s->enable_usb_hack;
161        s->init = false;
162        return;
163    }
164
165    /* Duration of last iteration */
166    iteration_time = (double)time_stamp - s->last_time;
167
168    /* Don't go backwards in time */
169    if (iteration_time <= 0)
170        return;
171
172    /* Wait at least 100 ms before starting calculations, otherwise the
173     * impact of the offset error will slow down convergence */
174    if (time_stamp < s->smoother_start_time + 100 * PA_USEC_PER_MSEC)
175        return;
176
177    /* Time difference in system time domain */
178    time_delta_system = time_stamp - s->start_time;
179
180    /* Number of bytes played since start_time */
181    byte_difference = (double)byte_count - s->start_pos;
182
183    /* Time difference in soundcard time domain. Don't use
184     * pa_bytes_to_usec() here because byte_difference need not
185     * be on a sample boundary */
186    time_delta_card = byte_difference / s->frame_size / s->rate * PA_USEC_PER_SEC;
187    filtered_time_delta_card = time_delta_card;
188
189    /* Prediction of measurement */
190    expected_time_delta_card = time_delta_system * s->time_factor;
191
192    /* Filtered variance of card time measurements */
193    s->time_variance = 0.9 * s->time_variance + 0.1 * (time_delta_card - expected_time_delta_card) * (time_delta_card - expected_time_delta_card);
194
195    /* Kalman filter, will only be used when the time factor has converged good enough,
196     * the value of 100 corresponds to a change rate of approximately 10e-6 per second. */
197    if (s->time_factor_variance < 100) {
198        filtered_time_delta_card = (time_delta_card * s->kalman_variance + expected_time_delta_card * s->time_variance) / (s->kalman_variance + s->time_variance);
199        s->kalman_variance = s->kalman_variance * s->time_variance / (s->kalman_variance + s->time_variance) + s->time_variance / 4 + 500;
200    }
201
202    /* This is a horrible hack which is necessary because USB sinks seem to fix up
203     * the reported delay by some millisecondsconds shortly after startup. This is
204     * an artifact, the real latency does not change on the reported jump. If the
205     * change is not caught or if the hack is triggered inadvertently, it will lead to
206     * prolonged convergence time and decreased stability of the reported latency.
207     * Since the fix up will occur within the first seconds, it is disabled later to
208     * avoid false triggers. When run as batch device, the threshold for the hack must
209     * be lower (1000) than for timer based scheduling (2000). */
210    if (s->usb_hack && time_stamp - s->smoother_start_time < 5 * PA_USEC_PER_SEC) {
211        if ((time_delta_system - filtered_time_delta_card / s->time_factor) > (double)s->hack_threshold) {
212            /* Recalculate initial conditions */
213            temp = time_stamp - time_delta_card - s->start_time;
214            s->start_time += temp;
215            s->smoother_start_time += temp;
216            s->time_offset = -temp;
217
218            /* Reset time factor variance */
219            s->time_factor_variance = 10000;
220
221            pa_log_debug("USB Hack, start time corrected by %0.2f usec", temp);
222            s->usb_hack = false;
223            return;
224         }
225    }
226
227    /* Parameter for lowpass filters with time constants of smoother_window_time
228     * and smoother_window_time/8 */
229    temp = (double)s->smoother_window_time / 6.2831853;
230    filter_constant = iteration_time / (iteration_time + temp / 8.0);
231    filter_constant_1 = iteration_time / (iteration_time + temp);
232
233    /* Temporarily save the current time factor */
234    temp = s->time_factor;
235
236    /* Calculate geometric series */
237    drift = (s->drift_filter_1 + 1.0) * (1.5 - filtered_time_delta_card / time_delta_system);
238
239    /* 2nd order lowpass */
240    s->drift_filter = (1 - filter_constant) * s->drift_filter + filter_constant * drift;
241    s->drift_filter_1 = (1 - filter_constant) * s->drift_filter_1 + filter_constant * s->drift_filter;
242
243    /* Calculate time conversion factor, filter again */
244    s->time_factor = (1 - filter_constant_1) * s->time_factor + filter_constant_1 * (s->drift_filter_1 + 3) / (s->drift_filter_1 + 1) / 2;
245
246    /* Filtered variance of time factor derivative, used as measure for the convergence of the time factor */
247    temp = (s->time_factor - temp) / iteration_time * 10000000000000;
248    s->time_factor_variance = (1 - filter_constant_1) * s->time_factor_variance + filter_constant_1 * temp * temp;
249
250    /* Calculate new start time and corresponding sample count after window time */
251    if (time_stamp > s->smoother_start_time + s->smoother_window_time) {
252        s->start_pos += ((double)byte_count - s->start_pos) / (time_stamp - s->start_time) * iteration_time;
253        s->start_time += (pa_usec_t)iteration_time;
254    }
255
256    /* Save current system time */
257    s->last_time = time_stamp;
258}
259
260/* Calculate the current latency. For a source, the sign must be inverted */
261int64_t pa_smoother_2_get_delay(pa_smoother_2 *s, pa_usec_t time_stamp, uint64_t byte_count) {
262    int64_t now, delay;
263
264    pa_assert(s);
265
266    /* If we do not have a valid frame size and rate, just return 0 */
267    if (!s->frame_size || !s->rate)
268        return 0;
269
270    /* Smoother is paused or has been resumed but no new data has been received */
271    if (s->paused || s->init) {
272        delay = (int64_t)((double)byte_count * PA_USEC_PER_SEC / s->frame_size / s->rate);
273        return delay - pa_smoother_2_get(s, time_stamp);
274    }
275
276    /* Convert system time difference to soundcard time difference */
277    now = (time_stamp - s->start_time - s->time_offset) * s->time_factor;
278
279    /* Don't use pa_bytes_to_usec(), u->start_pos needs not be on a sample boundary */
280    return (int64_t)(((double)byte_count - s->start_pos) / s->frame_size / s->rate * PA_USEC_PER_SEC) - now;
281}
282
283/* Convert system time to sound card time */
284pa_usec_t pa_smoother_2_get(pa_smoother_2 *s, pa_usec_t time_stamp) {
285    pa_usec_t current_time;
286
287    pa_assert(s);
288
289    /* If we do not have a valid frame size and rate, just return 0 */
290    if (!s->frame_size || !s->rate)
291        return 0;
292
293    /* Sound card time at start_time */
294    current_time = (pa_usec_t)(s->start_pos / s->frame_size / s->rate * PA_USEC_PER_SEC);
295
296    /* If the smoother has not started, just return system time since resume */
297    if (!s->start_time) {
298        if (time_stamp >= s->resume_time)
299            current_time = time_stamp - s->resume_time;
300        else
301            current_time = 0;
302
303    /* If we are paused return the sound card time at pause_time */
304    } else if (s->paused)
305        current_time += (s->pause_time - s->start_time - s->time_offset - s->fixup_time) * s->time_factor;
306
307    /* If we are initializing, add the time since resume to the card time at pause_time */
308    else if (s->init) {
309        current_time += (s->pause_time - s->start_time - s->time_offset - s->fixup_time) * s->time_factor;
310        current_time += (time_stamp - s->resume_time) * s->time_factor;
311
312    /* Smoother is running, calculate current sound card time */
313    } else
314        current_time += (time_stamp - s->start_time - s->time_offset) * s->time_factor;
315
316    return current_time;
317}
318
319/* Convert a time interval from sound card time to system time */
320pa_usec_t pa_smoother_2_translate(pa_smoother_2 *s, pa_usec_t time_difference) {
321
322    pa_assert(s);
323
324    /* If not started yet, return the time difference */
325    if (!s->start_time)
326        return time_difference;
327
328    return (pa_usec_t)(time_difference / s->time_factor);
329}
330
331/* Enable USB hack */
332void pa_smoother_2_usb_hack_enable(pa_smoother_2 *s, bool enable, pa_usec_t offset) {
333
334    pa_assert(s);
335
336    s->enable_usb_hack = enable;
337    s->hack_threshold = offset;
338}
339
340/* Reset the smoother */
341void pa_smoother_2_reset(pa_smoother_2 *s, pa_usec_t time_stamp) {
342
343    pa_assert(s);
344
345   /* Reset variables for time estimation */
346    s->drift_filter = 1.0;
347    s->drift_filter_1 = 1.0;
348    s->time_factor = 1.0;
349    s->start_pos = 0;
350    s->init = true;
351    s->time_offset = 0;
352    s->time_factor_variance = 10000.0;
353    s->kalman_variance = 10000000.0;
354    s->time_variance = 100000.0;
355    s->start_time = 0;
356    s->last_time = 0;
357    s->smoother_start_time = 0;
358    s->usb_hack = false;
359    s->pause_time = time_stamp;
360    s->fixup_time = 0;
361    s->resume_time = time_stamp;
362    s->paused = false;
363
364    /* Set smoother to paused if rate or frame size are invalid */
365    if (!s->frame_size || !s->rate)
366        s->paused = true;
367}
368
369/* Pause the smoother */
370void pa_smoother_2_pause(pa_smoother_2 *s, pa_usec_t time_stamp) {
371
372    pa_assert(s);
373
374    /* Smoother is already paused, nothing to do */
375    if (s->paused)
376        return;
377
378    /* If we are in init state, add the pause time to the fixup time */
379    if (s->init)
380        s->fixup_time += s->resume_time - s->pause_time;
381    else
382        s->fixup_time = 0;
383
384    s->smoother_start_time = 0;
385    s->resume_time = time_stamp;
386    s->pause_time = time_stamp;
387    s->time_factor_variance = 10000.0;
388    s->kalman_variance = 10000000.0;
389    s->time_variance = 100000.0;
390    s->init = true;
391    s->paused = true;
392}
393
394/* Resume the smoother */
395void pa_smoother_2_resume(pa_smoother_2 *s, pa_usec_t time_stamp) {
396
397    pa_assert(s);
398
399    if (!s->paused)
400        return;
401
402    /* Keep smoother paused if rate or frame size is not set */
403    if (!s->frame_size || !s->rate)
404        return;
405
406    s->resume_time = time_stamp;
407    s->paused = false;
408}
409