From d776a0917d7028f22fcaa8eb8364f895e0f1e887 Mon Sep 17 00:00:00 2001 From: Wim Taymans Date: Wed, 9 Dec 2020 12:09:40 +0100 Subject: [PATCH] alsa: refactor dll code --- spa/plugins/alsa/alsa-pcm.c | 49 +++++++----------------- spa/plugins/alsa/alsa-pcm.h | 6 +-- spa/plugins/alsa/alsa-seq.c | 41 ++++++-------------- spa/plugins/alsa/alsa-seq.h | 9 ++--- spa/plugins/alsa/dll.h | 71 +++++++++++++++++++++++++++++++++++ spa/plugins/alsa/test-timer.c | 46 ++++++----------------- 6 files changed, 114 insertions(+), 108 deletions(-) create mode 100644 spa/plugins/alsa/dll.h diff --git a/spa/plugins/alsa/alsa-pcm.c b/spa/plugins/alsa/alsa-pcm.c index 738c21cbc..f39b7cd12 100644 --- a/spa/plugins/alsa/alsa-pcm.c +++ b/spa/plugins/alsa/alsa-pcm.c @@ -600,21 +600,6 @@ static int set_timeout(struct state *state, uint64_t time) return 0; } -static void init_loop(struct state *state) -{ - state->bw = 0.0; - state->z1 = state->z2 = state->z3 = 0.0; -} - -static void set_loop(struct state *state, double bw) -{ - double w = 2 * M_PI * bw * state->threshold / state->rate; - state->w0 = 1.0 - exp (-20.0 * w); - state->w1 = w * 1.5 / state->threshold; - state->w2 = w / 1.5; - state->bw = bw; -} - static int alsa_recover(struct state *state, int err) { int res, st; @@ -666,7 +651,7 @@ recover: state, snd_strerror(res)); return res; } - init_loop(state); + spa_dll_init(&state->dll); state->alsa_recovering = true; if (state->stream == SND_PCM_STREAM_CAPTURE) { @@ -740,16 +725,12 @@ static int update_time(struct state *state, uint64_t nsec, snd_pcm_sframes_t del else err = (target + 128) - delay; - if (SPA_UNLIKELY(state->bw == 0.0)) { - set_loop(state, BW_MAX); + if (SPA_UNLIKELY(state->dll.bw == 0.0)) { + spa_dll_set_bw(&state->dll, SPA_DLL_BW_MAX, state->threshold, state->rate); state->next_time = nsec; state->base_time = nsec; } - state->z1 += state->w0 * (state->w1 * err - state->z1); - state->z2 += state->w0 * (state->z1 - state->z2); - state->z3 += state->w2 * state->z2; - - corr = 1.0 - (state->z2 + state->z3); + corr = spa_dll_update(&state->dll, err); if (SPA_UNLIKELY(state->last_threshold != state->threshold)) { int32_t diff = (int32_t) (state->last_threshold - state->threshold); @@ -761,14 +742,12 @@ static int update_time(struct state *state, uint64_t nsec, snd_pcm_sframes_t del if (SPA_UNLIKELY((state->next_time - state->base_time) > BW_PERIOD)) { state->base_time = state->next_time; - if (state->bw == BW_MAX) - set_loop(state, BW_MED); - else if (state->bw == BW_MED) - set_loop(state, BW_MIN); + if (state->dll.bw > SPA_DLL_BW_MIN) + spa_dll_set_bw(&state->dll, state->dll.bw / 2.0, state->threshold, state->rate); spa_log_debug(state->log, NAME" %p: follower:%d match:%d rate:%f bw:%f del:%d target:%ld err:%f (%f %f %f)", - state, follower, state->matching, corr, state->bw, state->delay, target, - err, state->z1, state->z2, state->z3); + state, follower, state->matching, corr, state->dll.bw, state->delay, target, + err, state->dll.z1, state->dll.z2, state->dll.z3); } if (state->rate_match) { @@ -819,8 +798,8 @@ int spa_alsa_write(struct state *state, snd_pcm_uframes_t silence) if (SPA_UNLIKELY(!state->alsa_recovering && delay > target + state->threshold)) { spa_log_warn(state->log, NAME" %p: follower delay:%ld resync %f %f %f", - state, delay, state->z1, state->z2, state->z3); - init_loop(state); + state, delay, state->dll.z1, state->dll.z2, state->dll.z3); + spa_dll_init(&state->dll); state->alsa_sync = true; } if (SPA_UNLIKELY(state->alsa_sync)) { @@ -1051,8 +1030,8 @@ int spa_alsa_read(struct state *state, snd_pcm_uframes_t silence) if (!state->alsa_recovering && (delay < target || delay > target * 2)) { spa_log_warn(state->log, NAME" %p: follower delay:%lu target:%lu resync %f %f %f", - state, delay, target, state->z1, state->z2, state->z3); - init_loop(state); + state, delay, target, state->dll.z1, state->dll.z2, state->dll.z3); + spa_dll_init(&state->dll); state->alsa_sync = true; } if (state->alsa_sync) { @@ -1282,7 +1261,7 @@ int spa_alsa_start(struct state *state) state->threshold = (state->duration * state->rate + state->rate_denom-1) / state->rate_denom; state->last_threshold = state->threshold; - init_loop(state); + spa_dll_init(&state->dll); state->safety = 0.0; spa_log_debug(state->log, NAME" %p: start %d duration:%d rate:%d follower:%d match:%d resample:%d", @@ -1338,7 +1317,7 @@ static int do_reassign_follower(struct spa_loop *loop, { struct state *state = user_data; set_timers(state); - init_loop(state); + spa_dll_init(&state->dll); return 0; } diff --git a/spa/plugins/alsa/alsa-pcm.h b/spa/plugins/alsa/alsa-pcm.h index 463bb350c..2bf6a7e49 100644 --- a/spa/plugins/alsa/alsa-pcm.h +++ b/spa/plugins/alsa/alsa-pcm.h @@ -45,6 +45,8 @@ extern "C" { #include #include +#include "dll.h" + #define MIN_LATENCY 16 #define MAX_LATENCY 8192 @@ -159,9 +161,7 @@ struct state { uint64_t underrun; double safety; - double bw; - double z1, z2, z3; - double w0, w1, w2; + struct spa_dll dll; }; int diff --git a/spa/plugins/alsa/alsa-seq.c b/spa/plugins/alsa/alsa-seq.c index 5fc269ce0..67048de14 100644 --- a/spa/plugins/alsa/alsa-seq.c +++ b/spa/plugins/alsa/alsa-seq.c @@ -39,6 +39,7 @@ #define NAME "alsa-seq" +#include "dll.h" #include "alsa-seq.h" #define CHECK(s,msg,...) if ((res = (s)) < 0) { spa_log_error(state->log, msg ": %s", ##__VA_ARGS__, snd_strerror(res)); return res; } @@ -668,21 +669,6 @@ static int process_write(struct seq_state *state) return res; } -static void init_loop(struct seq_state *state) -{ - state->bw = 0.0; - state->z1 = state->z2 = state->z3 = 0.0; -} - -static void set_loop(struct seq_state *state, double bw) -{ - double w = 2 * M_PI * bw * state->threshold * state->rate.num / state->rate.denom; - state->w0 = 1.0 - exp (-20.0 * w); - state->w1 = w * 1.5 / state->threshold; - state->w2 = w / 1.5; - state->bw = bw; -} - #define NSEC_TO_CLOCK(c,n) ((n) * (c)->rate.denom / ((c)->rate.num * SPA_NSEC_PER_SEC)) static int update_time(struct seq_state *state, uint64_t nsec, bool follower) @@ -714,7 +700,7 @@ static int update_time(struct seq_state *state, uint64_t nsec, bool follower) state->clock_base = position; } - corr = 1.0 - (state->z2 + state->z3); + corr = 1.0 - (state->dll.z2 + state->dll.z3); clock_elapsed = position - state->clock_base; state->queue_time = nsec - state->queue_base; @@ -722,26 +708,23 @@ static int update_time(struct seq_state *state, uint64_t nsec, bool follower) err = ((int64_t)clock_elapsed - (int64_t) queue_elapsed); - if (state->bw == 0.0) { - set_loop(state, BW_MAX); + if (state->dll.bw == 0.0) { + spa_dll_set_bw(&state->dll, SPA_DLL_BW_MAX, state->threshold, + state->rate.num / state->rate.denom); state->next_time = nsec; state->base_time = nsec; } - state->z1 += state->w0 * (state->w1 * err - state->z1); - state->z2 += state->w0 * (state->z1 - state->z2); - state->z3 += state->w2 * state->z2; - - corr = 1.0 - (state->z2 + state->z3); + corr = spa_dll_update(&state->dll, err); if ((state->next_time - state->base_time) > BW_PERIOD) { state->base_time = state->next_time; - if (state->bw == BW_MAX) - set_loop(state, BW_MED); - else if (state->bw == BW_MED) - set_loop(state, BW_MIN); + if (state->dll.bw > SPA_DLL_BW_MIN) + spa_dll_set_bw(&state->dll, state->dll.bw / 2.0, + state->threshold, state->rate.num / state->rate.denom); spa_log_debug(state->log, NAME" %p: follower:%d rate:%f bw:%f err:%f (%f %f %f)", - state, follower, corr, state->bw, err, state->z1, state->z2, state->z3); + state, follower, corr, state->dll.bw, err, + state->dll.z1, state->dll.z2, state->dll.z3); } state->next_time += state->threshold / corr * 1e9 / state->rate.denom; @@ -885,7 +868,7 @@ int spa_alsa_seq_start(struct seq_state *state) spa_loop_add_source(state->data_loop, &state->source); state->queue_base = 0; - init_loop(state); + spa_dll_init(&state->dll); set_timers(state); return 0; diff --git a/spa/plugins/alsa/alsa-seq.h b/spa/plugins/alsa/alsa-seq.h index 32e9b4697..fc4019003 100644 --- a/spa/plugins/alsa/alsa-seq.h +++ b/spa/plugins/alsa/alsa-seq.h @@ -45,6 +45,8 @@ extern "C" { #include #include +#include "dll.h" + struct props { char device[64]; }; @@ -105,9 +107,6 @@ struct seq_conn { struct spa_source source; }; -#define BW_MAX 0.128 -#define BW_MED 0.064 -#define BW_MIN 0.016 #define BW_PERIOD (3 * SPA_NSEC_PER_SEC) struct seq_state { @@ -155,9 +154,7 @@ struct seq_state { struct seq_stream streams[2]; - double bw; - double z1, z2, z3; - double w0, w1, w2; + struct spa_dll dll; }; #define VALID_DIRECTION(this,d) ((d) == SPA_DIRECTION_INPUT || (d) == SPA_DIRECTION_OUTPUT) diff --git a/spa/plugins/alsa/dll.h b/spa/plugins/alsa/dll.h new file mode 100644 index 000000000..5b11d6f4f --- /dev/null +++ b/spa/plugins/alsa/dll.h @@ -0,0 +1,71 @@ +/* Simple DLL + * + * Copyright © 2019 Wim Taymans + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice (including the next + * paragraph) shall be included in all copies or substantial portions of the + * Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + */ + +#ifndef SPA_DLL_H +#define SPA_DLL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#define SPA_DLL_BW_MAX 0.128 +#define SPA_DLL_BW_MIN 0.016 + +struct spa_dll { + double bw; + double z1, z2, z3; + double w0, w1, w2; +}; + +static inline void spa_dll_init(struct spa_dll *dll) +{ + dll->bw = 0.0; + dll->z1 = dll->z2 = dll->z3 = 0.0; +} + +static inline void spa_dll_set_bw(struct spa_dll *dll, double bw, uint32_t period, uint32_t rate) +{ + double w = 2 * M_PI * bw * period / rate; + dll->w0 = 1.0 - exp (-20.0 * w); + dll->w1 = w * 1.5 / period; + dll->w2 = w / 1.5; + dll->bw = bw; +} + +static inline double spa_dll_update(struct spa_dll *dll, double err) +{ + dll->z1 += dll->w0 * (dll->w1 * err - dll->z1); + dll->z2 += dll->w0 * (dll->z1 - dll->z2); + dll->z3 += dll->w2 * dll->z2; + return 1.0 - (dll->z2 + dll->z3); +} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /* SPA_DLL_H */ diff --git a/spa/plugins/alsa/test-timer.c b/spa/plugins/alsa/test-timer.c index 3510ff200..bd521cfbf 100644 --- a/spa/plugins/alsa/test-timer.c +++ b/spa/plugins/alsa/test-timer.c @@ -30,6 +30,8 @@ #include +#include + #define DEFAULT_DEVICE "hw:0" #define M_PI_M2 (M_PI + M_PI) @@ -37,8 +39,6 @@ #define NSEC_PER_SEC 1000000000ll #define TIMESPEC_TO_NSEC(ts) ((ts)->tv_sec * NSEC_PER_SEC + (ts)->tv_nsec) -#define BW_MAX 0.128 -#define BW_MIN 0.016 #define BW_PERIOD (NSEC_PER_SEC * 3) struct state { @@ -54,34 +54,9 @@ struct state { uint64_t next_time; uint64_t prev_time; - double bw; - double z1, z2, z3; - double w0, w1, w2; + struct spa_dll dll; }; -static void dll_init(struct state *state) -{ - state->bw = 0.0; - state->z1 = state->z2 = state->z3 = 0.0; -} - -static void dll_set_bw(struct state *state, double bw) -{ - double w = M_PI_M2 * bw * state->period / state->rate; - state->w0 = 1.0 - exp (-20.0 * w); - state->w1 = w * 1.5 / state->period; - state->w2 = w / 1.5; - state->bw = bw; -} - -static double dll_update(struct state *state, double err) -{ - state->z1 += state->w0 * (state->w1 * err - state->z1); - state->z2 += state->w0 * (state->z1 - state->z2); - state->z3 += state->w2 * state->z2; - return 1.0 - (state->z2 + state->z3); -} - static int set_timeout(struct state *state, uint64_t time) { struct itimerspec ts; @@ -138,8 +113,8 @@ static int on_timer_wakeup(struct state *state) * samples remaining in the device when we wakeup. */ error = (double)delay - (double)state->period; - /* update the dll with the error, this gives a rate corection */ - corr = dll_update(state, error); + /* update the dll with the error, this gives a rate correction */ + corr = spa_dll_update(&state->dll, error); /* set our new adjusted timeout. alternatively, this value can * instead be used to drive a resampler if this device is @@ -151,11 +126,12 @@ static int on_timer_wakeup(struct state *state) state->prev_time = state->next_time; /* reduce bandwidth and show some stats */ - if (state->bw > BW_MIN) - dll_set_bw(state, state->bw / 2.0); + if (state->dll.bw > SPA_DLL_BW_MIN) + spa_dll_set_bw(&state->dll, state->dll.bw / 2.0, + state->period, state->rate); fprintf(stdout, "corr:%f error:%f bw:%f\n", - corr, error, state->bw); + corr, error, state->dll.bw); } /* pull in new samples write a new period */ write_period(state); @@ -193,8 +169,8 @@ int main(int argc, char *argv[]) snd_pcm_format_name(SND_PCM_FORMAT_S32_LE), state.rate, state.channels); - dll_init(&state); - dll_set_bw(&state, BW_MAX); + spa_dll_init(&state.dll); + spa_dll_set_bw(&state.dll, SPA_DLL_BW_MAX, state.period, state.rate); if ((state.timerfd = timerfd_create(CLOCK_MONOTONIC, 0)) < 0) perror("timerfd");