1 /*
2 ** Copyright 2011, The Android Open-Source Project
3 **
4 ** Licensed under the Apache License, Version 2.0 (the "License");
5 ** you may not use this file except in compliance with the License.
6 ** You may obtain a copy of the License at
7 **
8 ** http://www.apache.org/licenses/LICENSE-2.0
9 **
10 ** Unless required by applicable law or agreed to in writing, software
11 ** distributed under the License is distributed on an "AS IS" BASIS,
12 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 ** See the License for the specific language governing permissions and
14 ** limitations under the License.
15 */
16
17 //#define LOG_NDEBUG 0
18 #define LOG_TAG "echo_reference"
19
20 #include <errno.h>
21 #include <inttypes.h>
22 #include <pthread.h>
23 #include <stddef.h>
24 #include <stdlib.h>
25
26 #include <log/log.h>
27 #include <system/audio.h>
28 #include <audio_utils/resampler.h>
29 #include <audio_utils/echo_reference.h>
30
31 // echo reference state: bit field indicating if read, write or both are active.
32 enum state {
33 ECHOREF_IDLE = 0x00, // idle
34 ECHOREF_READING = 0x01, // reading is active
35 ECHOREF_WRITING = 0x02 // writing is active
36 };
37
38 struct echo_reference {
39 struct echo_reference_itfe itfe;
40 int status; // init status
41 uint32_t state; // active state: reading, writing or both
42 audio_format_t rd_format; // read sample format
43 uint32_t rd_channel_count; // read number of channels
44 uint32_t rd_sampling_rate; // read sampling rate in Hz
45 size_t rd_frame_size; // read frame size (bytes per sample)
46 audio_format_t wr_format; // write sample format
47 uint32_t wr_channel_count; // write number of channels
48 uint32_t wr_sampling_rate; // write sampling rate in Hz
49 size_t wr_frame_size; // write frame size (bytes per sample)
50 void *buffer; // main buffer
51 size_t buf_size; // main buffer size in frames
52 size_t frames_in; // number of frames in main buffer
53 void *wr_buf; // buffer for input conversions
54 size_t wr_buf_size; // size of conversion buffer in frames
55 size_t wr_frames_in; // number of frames in conversion buffer
56 size_t wr_curr_frame_size; // number of frames given to current write() function
57 void *wr_src_buf; // resampler input buf (either wr_buf or buffer used by write())
58 struct timespec wr_render_time; // latest render time indicated by write()
59 // default ALSA gettimeofday() format
60 int32_t playback_delay; // playback buffer delay indicated by last write()
61 int16_t prev_delta_sign; // sign of previous delay difference:
62 // 1: positive, -1: negative, 0: unknown
63 uint16_t delta_count; // number of consecutive delay differences with same sign
64 pthread_mutex_t lock; // mutex protecting read/write concurrency
65 pthread_cond_t cond; // condition signaled when data is ready to read
66 struct resampler_itfe *resampler; // input resampler
67 struct resampler_buffer_provider provider; // resampler buffer provider
68 };
69
70
echo_reference_get_next_buffer(struct resampler_buffer_provider * buffer_provider,struct resampler_buffer * buffer)71 int echo_reference_get_next_buffer(struct resampler_buffer_provider *buffer_provider,
72 struct resampler_buffer* buffer)
73 {
74 struct echo_reference *er;
75
76 if (buffer_provider == NULL) {
77 return -EINVAL;
78 }
79
80 er = (struct echo_reference *)((char *)buffer_provider -
81 offsetof(struct echo_reference, provider));
82
83 if (er->wr_src_buf == NULL || er->wr_frames_in == 0) {
84 buffer->raw = NULL;
85 buffer->frame_count = 0;
86 return -ENODATA;
87 }
88
89 buffer->frame_count = (buffer->frame_count > er->wr_frames_in) ?
90 er->wr_frames_in : buffer->frame_count;
91 // this is er->rd_channel_count here as we resample after stereo to mono conversion if any
92 buffer->i16 = (int16_t *)er->wr_src_buf + (er->wr_curr_frame_size - er->wr_frames_in) *
93 er->rd_channel_count;
94
95 return 0;
96 }
97
echo_reference_release_buffer(struct resampler_buffer_provider * buffer_provider,struct resampler_buffer * buffer)98 void echo_reference_release_buffer(struct resampler_buffer_provider *buffer_provider,
99 struct resampler_buffer* buffer)
100 {
101 struct echo_reference *er;
102
103 if (buffer_provider == NULL) {
104 return;
105 }
106
107 er = (struct echo_reference *)((char *)buffer_provider -
108 offsetof(struct echo_reference, provider));
109
110 er->wr_frames_in -= buffer->frame_count;
111 }
112
echo_reference_reset_l(struct echo_reference * er)113 static void echo_reference_reset_l(struct echo_reference *er)
114 {
115 ALOGV("echo_reference_reset_l()");
116 free(er->buffer);
117 er->buffer = NULL;
118 er->buf_size = 0;
119 er->frames_in = 0;
120 free(er->wr_buf);
121 er->wr_buf = NULL;
122 er->wr_buf_size = 0;
123 er->wr_render_time.tv_sec = 0;
124 er->wr_render_time.tv_nsec = 0;
125 er->delta_count = 0;
126 er->prev_delta_sign = 0;
127 }
128
129 /* additional space in resampler buffer allowing for extra samples to be returned
130 * by speex resampler when sample rates ratio is not an integer.
131 */
132 #define RESAMPLER_HEADROOM_SAMPLES 10
133
echo_reference_write(struct echo_reference_itfe * echo_reference,struct echo_reference_buffer * buffer)134 static int echo_reference_write(struct echo_reference_itfe *echo_reference,
135 struct echo_reference_buffer *buffer)
136 {
137 struct echo_reference *er = (struct echo_reference *)echo_reference;
138 int status = 0;
139
140 if (er == NULL) {
141 return -EINVAL;
142 }
143
144 pthread_mutex_lock(&er->lock);
145
146 if (buffer == NULL) {
147 ALOGV("echo_reference_write() stop write");
148 er->state &= ~ECHOREF_WRITING;
149 echo_reference_reset_l(er);
150 goto exit;
151 }
152
153 ALOGV("echo_reference_write() START trying to write %zu frames", buffer->frame_count);
154 ALOGV("echo_reference_write() playbackTimestamp:[%d].[%d], er->playback_delay:[%d]",
155 (int)buffer->time_stamp.tv_sec,
156 (int)buffer->time_stamp.tv_nsec, er->playback_delay);
157
158 //ALOGV("echo_reference_write() %d frames", buffer->frame_count);
159 // discard writes until a valid time stamp is provided.
160
161 if ((buffer->time_stamp.tv_sec == 0) && (buffer->time_stamp.tv_nsec == 0) &&
162 (er->wr_render_time.tv_sec == 0) && (er->wr_render_time.tv_nsec == 0)) {
163 goto exit;
164 }
165
166 if ((er->state & ECHOREF_WRITING) == 0) {
167 ALOGV("echo_reference_write() start write");
168 if (er->resampler != NULL) {
169 er->resampler->reset(er->resampler);
170 }
171 er->state |= ECHOREF_WRITING;
172 }
173
174 if ((er->state & ECHOREF_READING) == 0) {
175 goto exit;
176 }
177
178 er->wr_render_time.tv_sec = buffer->time_stamp.tv_sec;
179 er->wr_render_time.tv_nsec = buffer->time_stamp.tv_nsec;
180
181 er->playback_delay = buffer->delay_ns;
182
183 // this will be used in the get_next_buffer, to support variable input buffer sizes
184 er->wr_curr_frame_size = buffer->frame_count;
185
186 void *srcBuf;
187 size_t inFrames;
188 // do stereo to mono and down sampling if necessary
189 if (er->rd_channel_count != er->wr_channel_count ||
190 er->rd_sampling_rate != er->wr_sampling_rate) {
191 size_t wrBufSize = buffer->frame_count;
192
193 inFrames = buffer->frame_count;
194
195 if (er->rd_sampling_rate != er->wr_sampling_rate) {
196 inFrames = (buffer->frame_count * er->rd_sampling_rate) / er->wr_sampling_rate +
197 RESAMPLER_HEADROOM_SAMPLES;
198 // wr_buf is not only used as resampler output but also for stereo to mono conversion
199 // output so buffer size is driven by both write and read sample rates
200 if (inFrames > wrBufSize) {
201 wrBufSize = inFrames;
202 }
203 }
204
205 if (er->wr_buf_size < wrBufSize) {
206 ALOGV("echo_reference_write() increasing write buffer size from %zu to %zu",
207 er->wr_buf_size, wrBufSize);
208 er->wr_buf_size = wrBufSize;
209 void *new_buf = realloc(er->wr_buf, er->wr_buf_size * er->rd_frame_size);
210 if (new_buf == NULL) {
211 status = -ENOMEM;
212 goto exit;
213 } else {
214 er->wr_buf = new_buf;
215 }
216 }
217
218 if (er->rd_channel_count != er->wr_channel_count) {
219 // must be stereo to mono
220 int16_t *src16 = (int16_t *)buffer->raw;
221 int16_t *dst16 = (int16_t *)er->wr_buf;
222 size_t frames = buffer->frame_count;
223 while (frames--) {
224 *dst16++ = (int16_t)(((int32_t)*src16 + (int32_t)*(src16 + 1)) >> 1);
225 src16 += 2;
226 }
227 }
228 if (er->wr_sampling_rate != er->rd_sampling_rate) {
229 if (er->resampler == NULL) {
230 int rc;
231 ALOGV("echo_reference_write() new ReSampler(%d, %d)",
232 er->wr_sampling_rate, er->rd_sampling_rate);
233 er->provider.get_next_buffer = echo_reference_get_next_buffer;
234 er->provider.release_buffer = echo_reference_release_buffer;
235 rc = create_resampler(er->wr_sampling_rate,
236 er->rd_sampling_rate,
237 er->rd_channel_count,
238 RESAMPLER_QUALITY_DEFAULT,
239 &er->provider,
240 &er->resampler);
241 if (rc != 0) {
242 er->resampler = NULL;
243 ALOGV("echo_reference_write() failure to create resampler %d", rc);
244 status = -ENODEV;
245 goto exit;
246 }
247 }
248 // er->wr_src_buf and er->wr_frames_in are used by getNexBuffer() called by the
249 // resampler to get new frames
250 if (er->rd_channel_count != er->wr_channel_count) {
251 er->wr_src_buf = er->wr_buf;
252 } else {
253 er->wr_src_buf = buffer->raw;
254 }
255 er->wr_frames_in = buffer->frame_count;
256 // inFrames is always more than we need here to get frames remaining from previous runs
257 // inFrames is updated by resample() with the number of frames produced
258 ALOGV("echo_reference_write() ReSampling(%d, %d)",
259 er->wr_sampling_rate, er->rd_sampling_rate);
260 er->resampler->resample_from_provider(er->resampler,
261 (int16_t *)er->wr_buf, &inFrames);
262 ALOGV_IF(er->wr_frames_in != 0,
263 "echo_reference_write() er->wr_frames_in not 0 (%zu) after resampler",
264 er->wr_frames_in);
265 }
266 srcBuf = er->wr_buf;
267 } else {
268 inFrames = buffer->frame_count;
269 srcBuf = buffer->raw;
270 }
271
272 if (er->frames_in + inFrames > er->buf_size) {
273 ALOGV("echo_reference_write() increasing buffer size from %zu to %zu",
274 er->buf_size, er->frames_in + inFrames);
275 er->buf_size = er->frames_in + inFrames;
276 void *new_buf = realloc(er->buffer, er->buf_size * er->rd_frame_size);
277 if (new_buf == NULL) {
278 status = -ENOMEM;
279 goto exit;
280 } else {
281 er->buffer = new_buf;
282 }
283 }
284 memcpy((char *)er->buffer + er->frames_in * er->rd_frame_size,
285 srcBuf,
286 inFrames * er->rd_frame_size);
287 er->frames_in += inFrames;
288
289 ALOGV("echo_reference_write() frames written:[%zu], frames total:[%zu] buffer size:[%zu]\n"
290 " er->wr_render_time:[%d].[%d], er->playback_delay:[%d]",
291 inFrames, er->frames_in, er->buf_size,
292 (int)er->wr_render_time.tv_sec, (int)er->wr_render_time.tv_nsec, er->playback_delay);
293
294 pthread_cond_signal(&er->cond);
295 exit:
296 pthread_mutex_unlock(&er->lock);
297 ALOGV("echo_reference_write() END");
298 return status;
299 }
300
301 // delay jump threshold to update ref buffer: 6 samples at 8kHz in nsecs
302 #define MIN_DELAY_DELTA_NS (375000*2)
303 // number of consecutive delta with same sign between expected and actual delay before adjusting
304 // the buffer
305 #define MIN_DELTA_NUM 4
306
307
echo_reference_read(struct echo_reference_itfe * echo_reference,struct echo_reference_buffer * buffer)308 static int echo_reference_read(struct echo_reference_itfe *echo_reference,
309 struct echo_reference_buffer *buffer)
310 {
311 struct echo_reference *er = (struct echo_reference *)echo_reference;
312 int status = 0;
313
314 if (er == NULL) {
315 return -EINVAL;
316 }
317
318 pthread_mutex_lock(&er->lock);
319
320 if (buffer == NULL) {
321 ALOGV("echo_reference_read() stop read");
322 er->state &= ~ECHOREF_READING;
323 goto exit;
324 }
325
326 ALOGV("echo_reference_read() START, delayCapture:[%d], "
327 "er->frames_in:[%zu],buffer->frame_count:[%zu]",
328 buffer->delay_ns, er->frames_in, buffer->frame_count);
329
330 if ((er->state & ECHOREF_READING) == 0) {
331 ALOGV("echo_reference_read() start read");
332 echo_reference_reset_l(er);
333 er->state |= ECHOREF_READING;
334 }
335
336 if ((er->state & ECHOREF_WRITING) == 0) {
337 memset(buffer->raw, 0, er->rd_frame_size * buffer->frame_count);
338 buffer->delay_ns = 0;
339 goto exit;
340 }
341
342 // ALOGV("echo_reference_read() %d frames", buffer->frame_count);
343
344 // allow some time for new frames to arrive if not enough frames are ready for read
345 if (er->frames_in < buffer->frame_count) {
346 uint32_t timeoutMs = (uint32_t)((1000 * buffer->frame_count) / er->rd_sampling_rate / 2);
347 struct timespec ts = {0, 0};
348
349 clock_gettime(CLOCK_REALTIME, &ts);
350
351 ts.tv_sec += timeoutMs/1000;
352 ts.tv_nsec += (timeoutMs%1000) * 1000000;
353 if (ts.tv_nsec >= 1000000000) {
354 ts.tv_nsec -= 1000000000;
355 ts.tv_sec += 1;
356 }
357
358 pthread_cond_timedwait(&er->cond, &er->lock, &ts);
359
360 ALOGV_IF((er->frames_in < buffer->frame_count),
361 "echo_reference_read() waited %d ms but still not enough frames"
362 " er->frames_in: %zu, buffer->frame_count = %zu",
363 timeoutMs, er->frames_in, buffer->frame_count);
364 }
365
366 int64_t timeDiff;
367 struct timespec tmp;
368
369 if ((er->wr_render_time.tv_sec == 0 && er->wr_render_time.tv_nsec == 0) ||
370 (buffer->time_stamp.tv_sec == 0 && buffer->time_stamp.tv_nsec == 0)) {
371 ALOGV("echo_reference_read(): NEW:timestamp is zero---------setting timeDiff = 0, "
372 "not updating delay this time");
373 timeDiff = 0;
374 } else {
375 if (buffer->time_stamp.tv_nsec < er->wr_render_time.tv_nsec) {
376 tmp.tv_sec = buffer->time_stamp.tv_sec - er->wr_render_time.tv_sec - 1;
377 tmp.tv_nsec = 1000000000 + buffer->time_stamp.tv_nsec - er->wr_render_time.tv_nsec;
378 } else {
379 tmp.tv_sec = buffer->time_stamp.tv_sec - er->wr_render_time.tv_sec;
380 tmp.tv_nsec = buffer->time_stamp.tv_nsec - er->wr_render_time.tv_nsec;
381 }
382 timeDiff = (((int64_t)tmp.tv_sec * 1000000000 + tmp.tv_nsec));
383
384 int64_t expectedDelayNs = er->playback_delay + buffer->delay_ns - timeDiff;
385
386 if (er->resampler != NULL) {
387 // Resampler already compensates part of the delay
388 int32_t rsmp_delay = er->resampler->delay_ns(er->resampler);
389 expectedDelayNs -= rsmp_delay;
390 }
391
392 ALOGV("echo_reference_read(): expectedDelayNs[%" PRId64 "] = "
393 "er->playback_delay[%d] + delayCapture[%d"
394 "] - timeDiff[%" PRId64 "]",
395 expectedDelayNs, er->playback_delay, buffer->delay_ns, timeDiff);
396
397 if (expectedDelayNs > 0) {
398 int64_t delayNs = ((int64_t)er->frames_in * 1000000000) / er->rd_sampling_rate;
399
400 int64_t deltaNs = delayNs - expectedDelayNs;
401
402 ALOGV("echo_reference_read(): EchoPathDelayDeviation between reference and DMA [%"
403 PRId64 "]", deltaNs);
404 if (llabs(deltaNs) >= MIN_DELAY_DELTA_NS) {
405 // smooth the variation and update the reference buffer only
406 // if a deviation in the same direction is observed for more than MIN_DELTA_NUM
407 // consecutive reads.
408 int16_t delay_sign = (deltaNs >= 0) ? 1 : -1;
409 if (delay_sign == er->prev_delta_sign) {
410 er->delta_count++;
411 } else {
412 er->delta_count = 1;
413 }
414 er->prev_delta_sign = delay_sign;
415
416 if (er->delta_count > MIN_DELTA_NUM) {
417 size_t previousFrameIn = er->frames_in;
418 er->frames_in = (size_t)((expectedDelayNs * er->rd_sampling_rate)/1000000000);
419 int offset = er->frames_in - previousFrameIn;
420
421 ALOGV("echo_reference_read(): deltaNs ENOUGH and %s: "
422 "er->frames_in: %zu, previousFrameIn = %zu",
423 delay_sign ? "positive" : "negative", er->frames_in, previousFrameIn);
424
425 if (deltaNs < 0) {
426 // Less data available in the reference buffer than expected
427 if (er->frames_in > er->buf_size) {
428 er->buf_size = er->frames_in;
429 ALOGV("echo_reference_read(): increasing buffer size to %zu",
430 er->buf_size);
431 void *new_buf = realloc(er->buffer, er->buf_size * er->rd_frame_size);
432 if (new_buf == NULL) {
433 status = -ENOMEM;
434 goto exit;
435 } else {
436 er->buffer = new_buf;
437 }
438 }
439
440 if (offset > 0) {
441 memset((char *)er->buffer + previousFrameIn * er->rd_frame_size,
442 0, offset * er->rd_frame_size);
443 ALOGV("echo_reference_read(): pushing ref buffer by [%d]", offset);
444 }
445 } else {
446 // More data available in the reference buffer than expected
447 offset = -offset;
448 if (offset > 0) {
449 memcpy(er->buffer, (char *)er->buffer + (offset * er->rd_frame_size),
450 er->frames_in * er->rd_frame_size);
451 ALOGV("echo_reference_read(): shifting ref buffer by [%zu]",
452 er->frames_in);
453 }
454 }
455 }
456 } else {
457 er->delta_count = 0;
458 er->prev_delta_sign = 0;
459 ALOGV("echo_reference_read(): Constant EchoPathDelay - difference "
460 "between reference and DMA %" PRId64, deltaNs);
461 }
462 } else {
463 ALOGV("echo_reference_read(): NEGATIVE expectedDelayNs[%" PRId64
464 "] = er->playback_delay[%d] + delayCapture[%d"
465 "] - timeDiff[%" PRId64 "]",
466 expectedDelayNs, er->playback_delay, buffer->delay_ns, timeDiff);
467 }
468 }
469
470 if (er->frames_in < buffer->frame_count) {
471 if (buffer->frame_count > er->buf_size) {
472 er->buf_size = buffer->frame_count;
473 ALOGV("echo_reference_read(): increasing buffer size to %zu", er->buf_size);
474 void *new_buf = realloc(er->buffer, er->buf_size * er->rd_frame_size);
475 if (new_buf == NULL) {
476 status = -ENOMEM;
477 goto exit;
478 } else {
479 er->buffer = new_buf;
480 }
481 }
482 // filling up the reference buffer with 0s to match the expected delay.
483 memset((char *)er->buffer + er->frames_in * er->rd_frame_size,
484 0, (buffer->frame_count - er->frames_in) * er->rd_frame_size);
485 er->frames_in = buffer->frame_count;
486 }
487
488 memcpy(buffer->raw,
489 (char *)er->buffer,
490 buffer->frame_count * er->rd_frame_size);
491
492 er->frames_in -= buffer->frame_count;
493 memcpy(er->buffer,
494 (char *)er->buffer + buffer->frame_count * er->rd_frame_size,
495 er->frames_in * er->rd_frame_size);
496
497 // As the reference buffer is now time aligned to the microphone signal there is a zero delay
498 buffer->delay_ns = 0;
499
500 ALOGV("echo_reference_read() END %zu frames, total frames in %zu",
501 buffer->frame_count, er->frames_in);
502
503 pthread_cond_signal(&er->cond);
504
505 exit:
506 pthread_mutex_unlock(&er->lock);
507 return status;
508 }
509
510
create_echo_reference(audio_format_t rdFormat,uint32_t rdChannelCount,uint32_t rdSamplingRate,audio_format_t wrFormat,uint32_t wrChannelCount,uint32_t wrSamplingRate,struct echo_reference_itfe ** echo_reference)511 int create_echo_reference(audio_format_t rdFormat,
512 uint32_t rdChannelCount,
513 uint32_t rdSamplingRate,
514 audio_format_t wrFormat,
515 uint32_t wrChannelCount,
516 uint32_t wrSamplingRate,
517 struct echo_reference_itfe **echo_reference)
518 {
519 struct echo_reference *er;
520
521 ALOGV("create_echo_reference()");
522
523 if (echo_reference == NULL) {
524 return -EINVAL;
525 }
526
527 *echo_reference = NULL;
528
529 if (rdFormat != AUDIO_FORMAT_PCM_16_BIT ||
530 rdFormat != wrFormat) {
531 ALOGW("create_echo_reference bad format rd %d, wr %d", rdFormat, wrFormat);
532 return -EINVAL;
533 }
534 if ((rdChannelCount != 1 && rdChannelCount != 2) ||
535 wrChannelCount != 2) {
536 ALOGW("create_echo_reference bad channel count rd %d, wr %d", rdChannelCount,
537 wrChannelCount);
538 return -EINVAL;
539 }
540
541 er = (struct echo_reference *)calloc(1, sizeof(struct echo_reference));
542
543 er->itfe.read = echo_reference_read;
544 er->itfe.write = echo_reference_write;
545
546 er->state = ECHOREF_IDLE;
547 er->rd_format = rdFormat;
548 er->rd_channel_count = rdChannelCount;
549 er->rd_sampling_rate = rdSamplingRate;
550 er->wr_format = wrFormat;
551 er->wr_channel_count = wrChannelCount;
552 er->wr_sampling_rate = wrSamplingRate;
553 er->rd_frame_size = audio_bytes_per_sample(rdFormat) * rdChannelCount;
554 er->wr_frame_size = audio_bytes_per_sample(wrFormat) * wrChannelCount;
555 *echo_reference = &er->itfe;
556 return 0;
557 }
558
release_echo_reference(struct echo_reference_itfe * echo_reference)559 void release_echo_reference(struct echo_reference_itfe *echo_reference) {
560 struct echo_reference *er = (struct echo_reference *)echo_reference;
561
562 if (er == NULL) {
563 return;
564 }
565
566 ALOGV("EchoReference dstor");
567 echo_reference_reset_l(er);
568 if (er->resampler != NULL) {
569 release_resampler(er->resampler);
570 }
571 free(er);
572 }
573