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