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