alsa: add dll to estimate clock drift

Make delay signed in the clock io so that we can simply add it to the
tick to get the hw position.
Add clock drift to clock info

spa/include/spa/node/io.h

@@ -71,14 +71,17 @@ struct spa_io_range {
 	uint32_t max_size;	/**< maximum size of data */
 };
 
-/** A time source */
+/** A time source. Nodes that can report clocking information will
+ * receive this. The application sets the id. */
 struct spa_io_clock {
+	uint32_t id;			/**< unique clock id, set by application */
+	uint32_t flags;			/**< clock flags */
 	uint64_t nsec;			/**< time in nanoseconds */
 	struct spa_fraction rate;	/**< rate for position/delay */
 	uint64_t position;		/**< current position */
-	uint64_t delay;			/**< delay between position and hardware,
-					     add to position for capture,
-					     subtract for playback */
+	int64_t delay;			/**< delay between position and hardware,
+					  *  positive for capture, negative for playback */
+	double rate_diff;		/**< rate difference between clock and monotonic time */
 };
 
 /** latency reporting */

spa/plugins/alsa/alsa-sink.c

@@ -557,8 +557,6 @@ impl_node_port_use_buffers(struct spa_node *node,
 			spa_log_error(this->log, NAME " %p: need mapped memory", this);
 			return -EINVAL;
 		}
-		this->threshold = SPA_MIN(d[0].maxsize / this->frame_size,
-				this->props.max_latency);
 	}
 	this->n_buffers = n_buffers;
 
@@ -659,9 +657,6 @@ static int impl_node_process(struct spa_node *node)
 		spa_list_append(&this->ready, &b->link);
 		SPA_FLAG_UNSET(b->flags, BUFFER_FLAG_OUT);
 
-		this->threshold = SPA_MIN(b->buf->datas[0].chunk->size / this->frame_size,
-				this->props.max_latency);
-
 		spa_alsa_write(this, 0);
 
 		input->status = SPA_STATUS_OK;

spa/plugins/alsa/alsa-source.c

@@ -564,9 +564,6 @@ impl_node_port_use_buffers(struct spa_node *node,
 			return -EINVAL;
 		}
 		spa_list_append(&this->free, &b->link);
-
-		this->threshold = SPA_MIN(d[0].maxsize / this->frame_size,
-				this->props.max_latency);
 	}
 	this->n_buffers = n_buffers;
 

spa/plugins/alsa/alsa-utils.c

@@ -518,8 +518,17 @@ static int get_status(struct state *state, snd_pcm_sframes_t *avail, snd_htimest
 		if (*avail > state->buffer_frames)
 			*avail = state->buffer_frames;
 	}
-	if (now)
+	if (now) {
+#if 0
+		clock_gettime(CLOCK_MONOTONIC, now);
+#else
 		snd_pcm_status_get_htstamp(status, now);
+		if (now->tv_sec == 0 && now->tv_nsec == 0) {
+			spa_log_warn(state->log, "0 from snd_pcm_status_get_htstamp %ld", *avail);
+			clock_gettime(CLOCK_MONOTONIC, now);
+		}
+#endif
+	}
 
 	return 0;
 }
@@ -693,37 +702,50 @@ static int alsa_try_resume(struct state *state)
 
 static void alsa_on_playback_timeout_event(struct spa_source *source)
 {
-	uint64_t exp;
+	uint64_t exp, nsec_now;
 	int res;
 	struct state *state = source->data;
 	snd_pcm_t *hndl = state->hndl;
 	snd_pcm_sframes_t avail;
+	struct timespec now;
+	double pts, dts, rate_diff;
 
 	if (state->started && read(state->timerfd, &exp, sizeof(uint64_t)) != sizeof(uint64_t))
 		spa_log_warn(state->log, "error reading timerfd: %s", strerror(errno));
 
-	if ((res = get_status(state, &avail, &state->now)) < 0)
+	if ((res = get_status(state, &avail, &now)) < 0)
 		return;
 
+	if (state->position)
+		state->threshold = state->position->size;
+
+	state->now = now;
+
 	if (avail > state->buffer_frames)
 		avail = state->buffer_frames;
 
-	if (state->now.tv_sec == 0 && state->now.tv_nsec == 0) {
-		spa_log_warn(state->log, "0 from snd_pcm_status_get_htstamp %ld", avail);
-		clock_gettime(CLOCK_MONOTONIC, &state->now);
-	}
-
 	state->filled = state->buffer_frames - avail;
+	nsec_now = SPA_TIMESPEC_TO_NSEC(&state->now);
+
+	dts = nsec_now / 1000ll - (state->filled * 1000000ll / state->rate);
+	pts = dll_update(&state->dll, dts, state->threshold);
+	rate_diff = state->dll.T * state->rate / 1000000.f;
 
 	if (state->clock) {
-		state->clock->nsec = SPA_TIMESPEC_TO_NSEC(&state->now);
+		state->clock->nsec = nsec_now;
 		state->clock->rate = SPA_FRACTION(1, state->rate);
 		state->clock->position = state->sample_count;
-		state->clock->delay = state->filled;
+		state->clock->delay = -state->filled;
+		state->clock->rate_diff = rate_diff;
 	}
 
-	spa_log_trace(state->log, "timeout %ld %d %ld %ld %ld", state->filled, state->threshold,
-		      state->sample_count, state->now.tv_sec, state->now.tv_nsec);
+	if (state->bw != 0.05 && state->sample_count / state->rate > 4) {
+		state->bw = 0.05;
+		dll_bandwidth(&state->dll, state->threshold, state->rate, state->bw);
+	}
+
+	spa_log_trace(state->log, "timeout %ld %d %ld %ld %f %f %f", state->filled, state->threshold,
+		      state->sample_count, nsec_now, pts, dts, rate_diff);
 
 	if (state->filled > state->threshold * 2) {
 		if (snd_pcm_state(hndl) == SND_PCM_STATE_SUSPENDED) {
@@ -770,26 +792,18 @@ static void alsa_on_capture_timeout_event(struct spa_source *source)
 	snd_pcm_uframes_t total_read = 0;
 	struct itimerspec ts;
 	const snd_pcm_channel_area_t *my_areas;
-	snd_pcm_status_t *status;
 	struct timespec now;
 
 	if (state->started && read(state->timerfd, &exp, sizeof(uint64_t)) != sizeof(uint64_t))
 		spa_log_warn(state->log, "error reading timerfd: %s", strerror(errno));
 
-	snd_pcm_status_alloca(&status);
-
-	if ((res = snd_pcm_status(hndl, status)) < 0) {
-		spa_log_error(state->log, "snd_pcm_status error: %s", snd_strerror(res));
+	if ((res = get_status(state, &avail, &now)) < 0)
 		return;
-	}
 
-	if (state->position) {
+	state->now = now;
+
+	if (state->position)
 		state->threshold = state->position->size;
-	}
-
-	avail = snd_pcm_status_get_avail(status);
-	snd_pcm_status_get_htstamp(status, &state->now);
-	clock_gettime(CLOCK_MONOTONIC, &state->now);
 
 	if (state->clock) {
 		state->clock->nsec = SPA_TIMESPEC_TO_NSEC(&state->now);
@@ -802,8 +816,6 @@ static void alsa_on_capture_timeout_event(struct spa_source *source)
 		      state->sample_count, state->now.tv_sec, state->now.tv_nsec,
 		      now.tv_sec, now.tv_nsec);
 
-	state->now = now;
-
 	if (avail < state->threshold) {
 		if (snd_pcm_state(hndl) == SND_PCM_STATE_SUSPENDED) {
 			spa_log_error(state->log, "suspended: try resume");
@@ -850,6 +862,12 @@ int spa_alsa_start(struct state *state, bool xrun_recover)
 	if (state->started)
 		return 0;
 
+	if (state->position)
+		state->threshold = state->position->size;
+
+	state->bw = 1.0;
+	dll_init(&state->dll, state->threshold, state->rate, state->bw);
+
 	spa_log_debug(state->log, "alsa %p: start %d", state, state->threshold);
 
 	CHECK(set_swparams(state), "swparams");

spa/plugins/alsa/alsa-utils.h

@@ -30,6 +30,7 @@ extern "C" {
 #endif
 
 #include <stddef.h>
+#include <math.h>
 
 #include <asoundlib.h>
 
@@ -63,6 +64,12 @@ struct buffer {
 	struct spa_list link;
 };
 
+struct dll {
+    double T;
+    double b, c;
+    double n0;
+};
+
 struct state {
 	struct spa_handle handle;
 	struct spa_node node;
@@ -86,6 +93,8 @@ struct state {
 
 	bool have_format;
 	struct spa_audio_info current_format;
+	struct dll dll;
+	double bw;
 
 	snd_pcm_uframes_t buffer_frames;
 	snd_pcm_uframes_t period_frames;
@@ -136,6 +145,36 @@ int spa_alsa_close(struct state *state);
 
 int spa_alsa_write(struct state *state, snd_pcm_uframes_t silence);
 
+
+static inline void dll_bandwidth(struct dll *dll, double period, double rate, double bandwidth)
+{
+	double w = 2 * M_PI * bandwidth * period / rate;
+	dll->b = 1.0 - exp(-M_SQRT2 * w);
+	dll->c = (1.0 - exp(-w * w)) / period;
+}
+
+static inline void dll_init(struct dll *dll, double period, double rate, double bandwidth)
+{
+	dll->T = 1000000.0 / rate;
+	dll->n0 = 0.0;
+	dll_bandwidth(dll, period, rate, bandwidth);
+}
+
+static inline double dll_update(struct dll *dll, double system_time, double period)
+{
+	double e;
+
+	if (dll->n0 == 0.0) {
+		dll->n0 = system_time;
+	} else {
+		dll->n0 += period * dll->T;
+		e = system_time - dll->n0;
+		dll->n0 += dll->b * e;
+		dll->T += dll->c * e;
+	}
+	return dll->n0;
+}
+
 #ifdef __cplusplus
 } /* extern "C" */
 #endif

spa/plugins/alsa/meson.build

@@ -7,6 +7,6 @@ spa_alsa_sources = ['alsa.c',
 spa_alsa = shared_library('spa-alsa',
                            spa_alsa_sources,
                            include_directories : [spa_inc],
-                           dependencies : [ alsa_dep, libudev_dep ],
+                           dependencies : [ alsa_dep, libudev_dep, mathlib ],
                            install : true,
                            install_dir : '@0@/spa/alsa'.format(get_option('libdir')))