echo-cancel: align input and outputecho-cancel

Align input and output samples to do echo cancelation

1 files changed, 88 insertions, 48 deletions

diff --git a/src/modules/module-echo-cancel.c b/src/modules/module-echo-cancel.c
index 361598cd0..027e65bd1 100644
--- a/src/modules/module-echo-cancel.c
+++ b/src/modules/module-echo-cancel.c
@@ -37,7 +37,7 @@
 
 #include "module-echo-cancel-symdef.h"
 
-#undef DEBUG_AEC
+#define DEBUG_AEC
 
 PA_MODULE_AUTHOR("Wim Taymans");
 PA_MODULE_DESCRIPTION("Perform echo cancelation between source and sink");
@@ -65,13 +65,19 @@ struct buffer_state {
     uint8_t   *captured;
     size_t     captured_size;
     size_t     captured_idx;
-    pa_usec_t  captured_ts;
+    size_t     captured_offset;
 
     uint8_t   *written;
     size_t     written_size;
 
     size_t     write_idx;
-    pa_usec_t  write_ts;
+    size_t     write_offset;
+
+#ifdef DEBUG_AEC
+    FILE *captured_file;
+    FILE *played_file;
+    FILE *canceled_file;
+#endif
 };
 
 struct device_info {
@@ -84,10 +90,6 @@ struct device_info {
     pa_hook_slot
         *chunk_captured_slot,
         *chunk_written_slot;
-
-#ifdef DEBUG_AEC
-    FILE *file;
-#endif
 };
 
 static struct buffer_state *get_buffer_state(void) {
@@ -98,26 +100,58 @@ static struct buffer_state *get_buffer_state(void) {
         state->captured = pa_xmalloc0 (4096);
         state->captured_size = 4096;
         state->captured_idx = 0;
-        state->captured_ts = 0;
-        state->written = pa_xmalloc0 (4096*1024);
+        state->captured_offset = 0;
+        state->written = pa_xmalloc0 (4096*1024 + 4096);
         state->written_size = 4096*1024;
         state->write_idx = 0;
-        state->write_ts = 0;
+        state->write_offset = 0;
+#ifdef DEBUG_AEC
+        state->captured_file = fopen("/tmp/aec_rec.sw", "wb");
+        if (state->captured_file == NULL)
+            perror ("fopen failed");
+        state->played_file = fopen("/tmp/aec_play.sw", "wb");
+        if (state->played_file == NULL)
+            perror ("fopen failed");
+#endif
     }
     return state;
 }
 
 static void run_echo_cancel(struct buffer_state *state){
-    size_t write_ts, write_idx;
+    size_t write_offset, write_idx;
+    size_t captured_offset, diff;
+    uint8_t *written;
 
     /* combine with played data */
-    write_ts = state->write_ts;
+    captured_offset = state->captured_offset;
+    write_offset = state->write_offset;
     write_idx = state->write_idx;
+    written = state->written;
+
+    /* can correct for data we haven't played yet.. */
+    if (write_offset < captured_offset)
+        return;
 
-    pa_log_debug("echo cancel (%llu) (%llu)", (unsigned long long) write_ts, (unsigned long long)state->captured_ts);
+    diff = write_offset - captured_offset;
 
-    if (write_ts > state->captured_ts)
+    /* can correct for data we don't have buffered anymore */
+    if (diff > state->written_size)
         return;
+
+    if (write_idx >= diff)
+      write_idx -= diff;
+    else
+      write_idx += state->written_size - diff;
+
+    pa_log_debug("echo cancel (%llu) (%llu) (%lld)", (unsigned long long) write_offset,
+        (unsigned long long)captured_offset, (long long) diff);
+
+#ifdef DEBUG_AEC
+    if (state->captured_file)
+        fwrite(state->captured, 1, state->captured_size, state->captured_file);
+    if (state->played_file)
+        fwrite(state->written + write_idx, 1, state->captured_size, state->played_file);
+#endif
 }
 
 static pa_hook_result_t chunk_captured_cb(pa_source *s, pa_source_captured_t *c, struct device_info *d) {
@@ -125,15 +159,25 @@ static pa_hook_result_t chunk_captured_cb(pa_source *s, pa_source_captured_t *c,
     const pa_memchunk *chunk = c->chunk;
     pa_usec_t now, latency, then;
     struct buffer_state *state;
-    size_t ci, cl;
+    size_t ci, cl, offset, diff;
 
     if ((state = d->state) == NULL)
         return PA_HOOK_OK;
 
-    /* estimate the time when this was captured */
-    now = pa_rtclock_now();
-    latency = pa_source_get_latency_within_thread(s);
-    then = now - latency;
+    if (chunk->index == 0) {
+        /* estimate the time when this was captured */
+        now = pa_rtclock_now();
+        latency = pa_source_get_latency_within_thread(s);
+        then = now - latency;
+        /* convert time to sample position */
+        offset = pa_usec_to_bytes (then, &s->sample_spec);
+        if (state->captured_offset == 0)
+            state->captured_offset = offset;
+    } else
+        offset = state->captured_offset;
+
+    diff = offset - state->captured_offset;
+    state->captured_offset = offset;
 
     /* copy into ringbuffer */
     p = pa_memblock_acquire (chunk->memblock);
@@ -153,20 +197,17 @@ static pa_hook_result_t chunk_captured_cb(pa_source *s, pa_source_captured_t *c,
 
         if (state->captured_idx >= state->captured_size) {
             state->captured_idx = 0;
-            state->captured_ts = then;
 
             /* run echo cancel */
             run_echo_cancel (state);
         }
+
+        state->captured_offset += to_copy;
     }
 
-    pa_log_debug("(%llu) source %p captured chunk %p %d %d (%llu)", (unsigned long long) then,
-        s, p, (int) chunk->index, (int) chunk->length, (unsigned long long) latency);
+    pa_log_debug("(%llu %lld) source %p captured chunk %p %d %d (%llu)", (unsigned long long) offset,
+        (long long) diff, s, p, (int) chunk->index, (int) chunk->length, (unsigned long long) latency);
 
-#ifdef DEBUG_AEC
-    if (d->file)
-        fwrite((uint8_t *)p + chunk->index, 1, chunk->length, d->file);
-#endif
     pa_memblock_release (chunk->memblock);
 
     return PA_HOOK_OK;
@@ -175,15 +216,28 @@ static pa_hook_result_t chunk_captured_cb(pa_source *s, pa_source_captured_t *c,
 static pa_hook_result_t chunk_written_cb(pa_sink *s, pa_sink_written_t *w, struct device_info *d) {
     void *p;
     pa_memchunk *chunk = w->chunk;
-    pa_usec_t now, latency;
+    pa_usec_t now, latency = 0, then = 0;
     struct buffer_state *state;
+    size_t offset, diff;
 
     if ((state = d->state) == NULL)
-      return PA_HOOK_OK;
+        return PA_HOOK_OK;
+
+    if (chunk->index == 0) {
+        /* estimate the time when this will play back */
+        now = pa_rtclock_now();
+        latency = pa_sink_get_latency_within_thread(s);
+        then = now + latency;
 
-    /* estimate the time when this will play back */
-    now = pa_rtclock_now();
-    latency = pa_sink_get_latency_within_thread(s);
+        /* convert time to sample position */
+        offset = pa_usec_to_bytes (then, &s->sample_spec);
+        if (state->write_offset == 0)
+            state->write_offset = offset;
+    } else
+        offset = state->write_offset;
+
+    diff = offset - state->write_offset;
+    state->write_offset = offset;
 
     p = pa_memblock_acquire (chunk->memblock);
 
@@ -202,15 +256,11 @@ static pa_hook_result_t chunk_written_cb(pa_sink *s, pa_sink_written_t *w, struc
 
       state->write_idx += chunk->length;
     }
-    state->write_ts = now + latency;
+    state->write_offset += chunk->length;
 
-    pa_log_debug("(%llu) sink %p wrote chunk %p %d %d (%llu)", (unsigned long long) now+latency,
-        s, p, (int) chunk->index, (int) chunk->length, (unsigned long long) latency);
+    pa_log_debug("(%llu %lld) sink %p wrote chunk %p %d %d (%llu)", (unsigned long long) offset,
+        (long long) diff, s, p, (int) chunk->index, (int) chunk->length, (unsigned long long) latency);
 
-#ifdef DEBUG_AEC
-    if (d->file)
-        fwrite((uint8_t *)p + chunk->index, 1, chunk->length, d->file);
-#endif
     pa_memblock_release (chunk->memblock);
 
     return PA_HOOK_OK;
@@ -239,11 +289,6 @@ static pa_hook_result_t device_new_hook_cb(pa_core *c, pa_object *o, struct user
     d->state = get_buffer_state();
     if (source) {
         d->source = pa_source_ref(source);
-#ifdef DEBUG_AEC
-        d->file = fopen("/tmp/aec_rec.sw", "wb");
-        if (d->file == NULL)
-            perror ("fopen failed");
-#endif
         d->chunk_captured_slot = pa_hook_connect (&source->chunk_captured, PA_HOOK_NORMAL, (pa_hook_cb_t) chunk_captured_cb, d);
     } else {
         d->source = NULL;
@@ -251,11 +296,6 @@ static pa_hook_result_t device_new_hook_cb(pa_core *c, pa_object *o, struct user
 
     if (sink) {
         d->sink = pa_sink_ref(sink);
-#ifdef DEBUG_AEC
-        d->file = fopen("/tmp/aec_play.sw", "wb");
-        if (d->file == NULL)
-            perror ("fopen failed");
-#endif
         d->chunk_written_slot = pa_hook_connect (&sink->chunk_written, PA_HOOK_NORMAL, (pa_hook_cb_t) chunk_written_cb, d);
     } else {
         d->sink = NULL;