pulsecore: Add alternative time smoother implementation

This patch adds an alternative time smoother implementation based on the theory
found at https://www.freedesktop.org/software/pulseaudio/misc/rate_estimator.odt.

The functions were written to replace the current smoother functions nearly on
a one-to-one basis, though there are a few differences:
- The smoother_2_put() function takes a byte count instead of a sound card
  time as argument. This was changed because in most places a sample count
  was converted to a time before passing it to the smoother.
- The smoother needs to know sample rate and frame size to convert byte
  counts to time.
- A smoother_2_get_delay() function was added to directly retrieve the stream
  delay from the smoother.
- A hack for USB devices was added which works around an issue in the alsa
  latency reports for USB devices.

The smoother delivers much better precision than the current implementation.
For results, see the document referenced above.

The new functions are still unused. The following patches will convert all
callers of the smoother functions so that they can use both smoother
implementations, depending on a configure option.

Part-of: <https://gitlab.freedesktop.org/pulseaudio/pulseaudio/-/merge_requests/55>

4 files changed, 464 insertions, 0 deletions

diff --git a/po/POTFILES.in b/po/POTFILES.in
index 7a914f1d7..c92665d9b 100644
--- a/po/POTFILES.in
+++ b/po/POTFILES.in
@@ -170,6 +170,7 @@ src/pulsecore/thread-mq.c
 src/pulsecore/thread-posix.c
 src/pulsecore/thread-win32.c
 src/pulsecore/time-smoother.c
+src/pulsecore/time-smoother_2.c
 src/pulsecore/tokenizer.c
 src/pulsecore/x11prop.c
 src/pulsecore/x11wrap.c
diff --git a/src/meson.build b/src/meson.build
index 59a9b16bf..bbf83d7b1 100644
--- a/src/meson.build
+++ b/src/meson.build
@@ -62,6 +62,7 @@ libpulsecommon_sources = [
   'pulsecore/strlist.c',
   'pulsecore/tagstruct.c',
   'pulsecore/time-smoother.c',
+  'pulsecore/time-smoother_2.c',
   'pulsecore/tokenizer.c',
   'pulsecore/usergroup.c',
   'pulsecore/sndfile-util.c',
@@ -141,6 +142,7 @@ libpulsecommon_headers = [
   'pulsecore/tagstruct.h',
   'pulsecore/thread.h',
   'pulsecore/time-smoother.h',
+  'pulsecore/time-smoother_2.h',
   'pulsecore/tokenizer.h',
   'pulsecore/usergroup.h',
   'pulsecore/sndfile-util.h',
diff --git a/src/pulsecore/time-smoother_2.c b/src/pulsecore/time-smoother_2.c
new file mode 100644
index 000000000..8f4447e0c
--- /dev/null
+++ b/src/pulsecore/time-smoother_2.c
@@ -0,0 +1,408 @@
+/***
+  This file is part of PulseAudio.
+
+  PulseAudio is free software; you can redistribute it and/or modify
+  it under the terms of the GNU Lesser General Public License as
+  published by the Free Software Foundation; either version 2.1 of the
+  License, or (at your option) any later version.
+
+  PulseAudio is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with PulseAudio; if not, see <http://www.gnu.org/licenses/>.
+***/
+
+/* The code in this file is based on the theoretical background found at
+ * https://www.freedesktop.org/software/pulseaudio/misc/rate_estimator.odt.
+ * The theory has never been reviewed, so it may be inaccurate in places. */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <pulsecore/macro.h>
+#include <pulse/sample.h>
+#include <pulse/xmalloc.h>
+#include <pulse/timeval.h>
+
+#include "time-smoother_2.h"
+
+struct pa_smoother_2 {
+    /* Values set when the smoother is created */
+    pa_usec_t smoother_window_time;
+    uint32_t rate;
+    uint32_t frame_size;
+
+    /* USB hack parameters */
+    bool usb_hack;
+    bool enable_usb_hack;
+    uint32_t hack_threshold;
+
+    /* Smoother state */
+    bool init;
+    bool paused;
+
+    /* Current byte count start value */
+    double start_pos;
+    /* System time corresponding to start_pos */
+    pa_usec_t start_time;
+    /* Conversion factor between time domains */
+    double time_factor;
+
+    /* Used if the smoother is paused while still in init state */
+    pa_usec_t fixup_time;
+
+    /* Time offset for USB devices */
+    int64_t time_offset;
+
+    /* Various time stamps */
+    pa_usec_t resume_time;
+    pa_usec_t pause_time;
+    pa_usec_t smoother_start_time;
+    pa_usec_t last_time;
+
+    /* Variables used for Kalman filter */
+    double time_variance;
+    double time_factor_variance;
+    double kalman_variance;
+
+    /* Variables used for low pass filter */
+    double drift_filter;
+    double drift_filter_1;
+};
+
+/* Create new smoother */
+pa_smoother_2* pa_smoother_2_new(pa_usec_t window, pa_usec_t time_stamp, uint32_t frame_size, uint32_t rate) {
+    pa_smoother_2 *s;
+
+    pa_assert(window > 0);
+
+    s = pa_xnew(pa_smoother_2, 1);
+    s->enable_usb_hack = false;
+    s->usb_hack = false;
+    s->hack_threshold = 0;
+    s->smoother_window_time = window;
+    s->rate = rate;
+    s->frame_size = frame_size;
+
+    pa_smoother_2_reset(s, time_stamp);
+
+    return s;
+}
+
+/* Free the smoother */
+void pa_smoother_2_free(pa_smoother_2* s) {
+
+    pa_assert(s);
+
+    pa_xfree(s);
+}
+
+void pa_smoother_2_set_rate(pa_smoother_2 *s, pa_usec_t time_stamp, uint32_t rate) {
+
+    pa_assert(s);
+    pa_assert(rate > 0);
+
+    /* If the rate has changed, data in the smoother will be invalid,
+     * therefore also reset the smoother */
+    if (rate != s->rate) {
+        s->rate = rate;
+        pa_smoother_2_reset(s, time_stamp);
+    }
+}
+
+void pa_smoother_2_set_sample_spec(pa_smoother_2 *s, pa_usec_t time_stamp, pa_sample_spec *spec) {
+    size_t frame_size;
+
+    pa_assert(s);
+    pa_assert(pa_sample_spec_valid(spec));
+
+    /* If the sample spec has changed, data in the smoother will be invalid,
+     * therefore also reset the smoother */
+    frame_size = pa_frame_size(spec);
+    if (frame_size != s->frame_size || spec->rate != s->rate) {
+        s->frame_size = frame_size;
+        s->rate = spec->rate;
+        pa_smoother_2_reset(s, time_stamp);
+    }
+}
+
+/* Add a new data point and re-calculate time conversion factor */
+void pa_smoother_2_put(pa_smoother_2 *s, pa_usec_t time_stamp, int64_t byte_count) {
+    double byte_difference, iteration_time;
+    double time_delta_system, time_delta_card, drift, filter_constant, filter_constant_1;
+    double temp, filtered_time_delta_card, expected_time_delta_card;
+
+    pa_assert(s);
+
+    /* Smoother is paused, nothing to do */
+    if (s->paused)
+        return;
+
+    /* Initial setup or resume */
+    if PA_UNLIKELY((s->init)) {
+        s->resume_time = time_stamp;
+
+        /* We have no data yet, nothing to do */
+        if (byte_count <= 0)
+            return;
+
+        /* Now we are playing/recording.
+         * Get fresh time stamps and save the start count */
+        s->start_pos = (double)byte_count;
+        s->last_time = time_stamp;
+        s->start_time = time_stamp;
+        s->smoother_start_time = time_stamp;
+
+        s->usb_hack = s->enable_usb_hack;
+        s->init = false;
+        return;
+    }
+
+    /* Duration of last iteration */
+    iteration_time = (double)time_stamp - s->last_time;
+
+    /* Don't go backwards in time */
+    if (iteration_time <= 0)
+        return;
+
+    /* Wait at least 100 ms before starting calculations, otherwise the
+     * impact of the offset error will slow down convergence */
+    if (time_stamp < s->smoother_start_time + 100 * PA_USEC_PER_MSEC)
+        return;
+
+    /* Time difference in system time domain */
+    time_delta_system = time_stamp - s->start_time;
+
+    /* Number of bytes played since start_time */
+    byte_difference = (double)byte_count - s->start_pos;
+
+    /* Time difference in soundcard time domain. Don't use
+     * pa_bytes_to_usec() here because byte_difference need not
+     * be on a sample boundary */
+    time_delta_card = byte_difference / s->frame_size / s->rate * PA_USEC_PER_SEC;
+    filtered_time_delta_card = time_delta_card;
+
+    /* Prediction of measurement */
+    expected_time_delta_card = time_delta_system * s->time_factor;
+
+    /* Filtered variance of card time measurements */
+    s->time_variance = 0.9 * s->time_variance + 0.1 * (time_delta_card - expected_time_delta_card) * (time_delta_card - expected_time_delta_card);
+
+    /* Kalman filter, will only be used when the time factor has converged good enough,
+     * the value of 100 corresponds to a change rate of approximately 10e-6 per second. */
+    if (s->time_factor_variance < 100) {
+        filtered_time_delta_card = (time_delta_card * s->kalman_variance + expected_time_delta_card * s->time_variance) / (s->kalman_variance + s->time_variance);
+        s->kalman_variance = s->kalman_variance * s->time_variance / (s->kalman_variance + s->time_variance) + s->time_variance / 4 + 500;
+    }
+
+    /* This is a horrible hack which is necessary because USB sinks seem to fix up
+     * the reported delay by some millisecondsconds shortly after startup. This is
+     * an artifact, the real latency does not change on the reported jump. If the
+     * change is not caught or if the hack is triggered inadvertently, it will lead to
+     * prolonged convergence time and decreased stability of the reported latency.
+     * Since the fix up will occur within the first seconds, it is disabled later to
+     * avoid false triggers. When run as batch device, the threshold for the hack must
+     * be lower (1000) than for timer based scheduling (2000). */
+    if (s->usb_hack && time_stamp - s->smoother_start_time < 5 * PA_USEC_PER_SEC) {
+        if ((time_delta_system - filtered_time_delta_card / s->time_factor) > (double)s->hack_threshold) {
+            /* Recalculate initial conditions */
+            temp = time_stamp - time_delta_card - s->start_time;
+            s->start_time += temp;
+            s->smoother_start_time += temp;
+            s->time_offset = -temp;
+
+            /* Reset time factor variance */
+            s->time_factor_variance = 10000;
+
+            pa_log_debug("USB Hack, start time corrected by %0.2f usec", temp);
+            s->usb_hack = false;
+            return;
+         }
+    }
+
+    /* Parameter for lowpass filters with time constants of smoother_window_time
+     * and smoother_window_time/8 */
+    temp = (double)s->smoother_window_time / 6.2831853;
+    filter_constant = iteration_time / (iteration_time + temp / 8.0);
+    filter_constant_1 = iteration_time / (iteration_time + temp);
+
+    /* Temporarily save the current time factor */
+    temp = s->time_factor;
+
+    /* Calculate geometric series */
+    drift = (s->drift_filter_1 + 1.0) * (1.5 - filtered_time_delta_card / time_delta_system);
+
+    /* 2nd order lowpass */
+    s->drift_filter = (1 - filter_constant) * s->drift_filter + filter_constant * drift;
+    s->drift_filter_1 = (1 - filter_constant) * s->drift_filter_1 + filter_constant * s->drift_filter;
+
+    /* Calculate time conversion factor, filter again */
+    s->time_factor = (1 - filter_constant_1) * s->time_factor + filter_constant_1 * (s->drift_filter_1 + 3) / (s->drift_filter_1 + 1) / 2;
+
+    /* Filtered variance of time factor derivative, used as measure for the convergence of the time factor */
+    temp = (s->time_factor - temp) / iteration_time * 10000000000000;
+    s->time_factor_variance = (1 - filter_constant_1) * s->time_factor_variance + filter_constant_1 * temp * temp;
+
+    /* Calculate new start time and corresponding sample count after window time */
+    if (time_stamp > s->smoother_start_time + s->smoother_window_time) {
+        s->start_pos += ((double)byte_count - s->start_pos) / (time_stamp - s->start_time) * iteration_time;
+        s->start_time += (pa_usec_t)iteration_time;
+    }
+
+    /* Save current system time */
+    s->last_time = time_stamp;
+}
+
+/* Calculate the current latency. For a source, the sign must be inverted */
+int64_t pa_smoother_2_get_delay(pa_smoother_2 *s, pa_usec_t time_stamp, size_t byte_count) {
+    int64_t now, delay;
+
+    pa_assert(s);
+
+    /* If we do not have a valid frame size and rate, just return 0 */
+    if (!s->frame_size || !s->rate)
+        return 0;
+
+    /* Smoother is paused or has been resumed but no new data has been received */
+    if (s->paused || s->init) {
+        delay = (int64_t)((double)byte_count * PA_USEC_PER_SEC / s->frame_size / s->rate);
+        return delay - pa_smoother_2_get(s, time_stamp);
+    }
+
+    /* Convert system time difference to soundcard time difference */
+    now = (time_stamp - s->start_time - s->time_offset) * s->time_factor;
+
+    /* Don't use pa_bytes_to_usec(), u->start_pos needs not be on a sample boundary */
+    return (int64_t)(((double)byte_count - s->start_pos) / s->frame_size / s->rate * PA_USEC_PER_SEC) - now;
+}
+
+/* Convert system time to sound card time */
+pa_usec_t pa_smoother_2_get(pa_smoother_2 *s, pa_usec_t time_stamp) {
+    pa_usec_t current_time;
+
+    pa_assert(s);
+
+    /* If we do not have a valid frame size and rate, just return 0 */
+    if (!s->frame_size || !s->rate)
+        return 0;
+
+    /* Sound card time at start_time */
+    current_time = (pa_usec_t)(s->start_pos / s->frame_size / s->rate * PA_USEC_PER_SEC);
+
+    /* If the smoother has not started, just return system time since resume */
+    if (!s->start_time) {
+        if (time_stamp >= s->resume_time)
+            current_time = time_stamp - s->resume_time;
+        else
+            current_time = 0;
+
+    /* If we are paused return the sound card time at pause_time */
+    } else if (s->paused)
+        current_time += (s->pause_time - s->start_time - s->time_offset - s->fixup_time) * s->time_factor;
+
+    /* If we are initializing, add the time since resume to the card time at pause_time */
+    else if (s->init) {
+        current_time += (s->pause_time - s->start_time - s->time_offset - s->fixup_time) * s->time_factor;
+        current_time += (time_stamp - s->resume_time) * s->time_factor;
+
+    /* Smoother is running, calculate current sound card time */
+    } else
+        current_time += (time_stamp - s->start_time - s->time_offset) * s->time_factor;
+
+    return current_time;
+}
+
+/* Convert a time interval from sound card time to system time */
+pa_usec_t pa_smoother_2_translate(pa_smoother_2 *s, pa_usec_t time_difference) {
+
+    pa_assert(s);
+
+    /* If not started yet, return the time difference */
+    if (!s->start_time)
+        return time_difference;
+
+    return (pa_usec_t)(time_difference / s->time_factor);
+}
+
+/* Enable USB hack */
+void pa_smoother_2_usb_hack_enable(pa_smoother_2 *s, bool enable, pa_usec_t offset) {
+
+    pa_assert(s);
+
+    s->enable_usb_hack = enable;
+    s->hack_threshold = offset;
+}
+
+/* Reset the smoother */
+void pa_smoother_2_reset(pa_smoother_2 *s, pa_usec_t time_stamp) {
+
+    pa_assert(s);
+
+   /* Reset variables for time estimation */
+    s->drift_filter = 1.0;
+    s->drift_filter_1 = 1.0;
+    s->time_factor = 1.0;
+    s->start_pos = 0;
+    s->init = true;
+    s->time_offset = 0;
+    s->time_factor_variance = 10000.0;
+    s->kalman_variance = 10000000.0;
+    s->time_variance = 100000.0;
+    s->start_time = 0;
+    s->last_time = 0;
+    s->smoother_start_time = 0;
+    s->usb_hack = false;
+    s->pause_time = time_stamp;
+    s->fixup_time = 0;
+    s->resume_time = time_stamp;
+    s->paused = false;
+
+    /* Set smoother to paused if rate or frame size are invalid */
+    if (!s->frame_size || !s->rate)
+        s->paused = true;
+}
+
+/* Pause the smoother */
+void pa_smoother_2_pause(pa_smoother_2 *s, pa_usec_t time_stamp) {
+
+    pa_assert(s);
+
+    /* Smoother is already paused, nothing to do */
+    if (s->paused)
+        return;
+
+    /* If we are in init state, add the pause time to the fixup time */
+    if (s->init)
+        s->fixup_time += s->resume_time - s->pause_time;
+    else
+        s->fixup_time = 0;
+
+    s->smoother_start_time = 0;
+    s->resume_time = time_stamp;
+    s->pause_time = time_stamp;
+    s->time_factor_variance = 10000.0;
+    s->kalman_variance = 10000000.0;
+    s->time_variance = 100000.0;
+    s->init = true;
+    s->paused = true;
+}
+
+/* Resume the smoother */
+void pa_smoother_2_resume(pa_smoother_2 *s, pa_usec_t time_stamp) {
+
+    pa_assert(s);
+
+    if (!s->paused)
+        return;
+
+    /* Keep smoother paused if rate or frame size is not set */
+    if (!s->frame_size || !s->rate)
+        return;
+
+    s->resume_time = time_stamp;
+    s->paused = false;
+}
diff --git a/src/pulsecore/time-smoother_2.h b/src/pulsecore/time-smoother_2.h
new file mode 100644
index 000000000..57fc1e31c
--- /dev/null
+++ b/src/pulsecore/time-smoother_2.h
@@ -0,0 +1,53 @@
+#ifndef foopulsetimesmoother2hfoo
+#define foopulsetimesmoother2hfoo
+
+/***
+  This file is part of PulseAudio.
+
+  PulseAudio is free software; you can redistribute it and/or modify
+  it under the terms of the GNU Lesser General Public License as
+  published by the Free Software Foundation; either version 2.1 of the
+  License, or (at your option) any later version.
+
+  PulseAudio is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with PulseAudio; if not, see <http://www.gnu.org/licenses/>.
+***/
+
+#include <pulse/sample.h>
+
+typedef struct pa_smoother_2 pa_smoother_2;
+
+/* Create new smoother */
+pa_smoother_2* pa_smoother_2_new(pa_usec_t window, pa_usec_t time_stamp, uint32_t frame_size, uint32_t rate);
+/* Free the smoother */
+void pa_smoother_2_free(pa_smoother_2* s);
+/* Reset the smoother */
+void pa_smoother_2_reset(pa_smoother_2 *s, pa_usec_t time_stamp);
+/* Pause the smoother */
+void pa_smoother_2_pause(pa_smoother_2 *s, pa_usec_t time_stamp);
+/* Resume the smoother */
+void pa_smoother_2_resume(pa_smoother_2 *s, pa_usec_t time_stamp);
+
+/* Add a new data point and re-calculate time conversion factor */
+void pa_smoother_2_put(pa_smoother_2 *s, pa_usec_t time_stamp, int64_t byte_count);
+
+/* Calculate the current latency. For a source, the sign of the result must be inverted */
+int64_t pa_smoother_2_get_delay(pa_smoother_2 *s, pa_usec_t time_stamp, size_t byte_count);
+/* Convert system time since start to sound card time */
+pa_usec_t pa_smoother_2_get(pa_smoother_2 *s, pa_usec_t time_stamp);
+/* Convert a time interval from sound card time to system time */
+pa_usec_t pa_smoother_2_translate(pa_smoother_2 *s, pa_usec_t time_difference);
+
+/* Enable USB hack, only used for alsa sinks */
+void pa_smoother_2_usb_hack_enable(pa_smoother_2 *s, bool enable, pa_usec_t offset);
+/* Set sample rate */
+void pa_smoother_2_set_rate(pa_smoother_2 *s, pa_usec_t time_stamp, uint32_t rate);
+/* Set rate and frame size */
+void pa_smoother_2_set_sample_spec(pa_smoother_2 *s, pa_usec_t time_stamp, pa_sample_spec *spec);
+
+#endif