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