audio_utils/echo_reference.c

   1 /*
   2 ** Copyright 2011, The Android Open-Source Project
   3 **
   4 ** Licensed under the Apache License, Version 2.0 (the "License");
   5 ** you may not use this file except in compliance with the License.
   6 ** You may obtain a copy of the License at
   7 **
   8 **     http://www.apache.org/licenses/LICENSE-2.0
   9 **
  10 ** Unless required by applicable law or agreed to in writing, software
  11 ** distributed under the License is distributed on an "AS IS" BASIS,
  12 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13 ** See the License for the specific language governing permissions and
  14 ** limitations under the License.
  15 */
  16
  17 //#define LOG_NDEBUG 0
  18 #define LOG_TAG "echo_reference"
  19
  20 #include <errno.h>
  21 #include <inttypes.h>
  22 #include <pthread.h>
  23 #include <stdlib.h>
  24
  25 #include <log/log.h>
  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
  55     size_t wr_curr_frame_size;      // number of frames given to current write() function
  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()
  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
  63     pthread_mutex_t lock;                      // mutex protecting read/write concurrency
  64     pthread_cond_t cond;                       // condition signaled when data is ready to read
  65     struct resampler_itfe *resampler;          // input resampler
  66     struct resampler_buffer_provider provider; // resampler buffer provider
  67 };
  68
  69
  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
  79     er = (struct echo_reference *)((char *)buffer_provider -
  80                                       offsetof(struct echo_reference, provider));
  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
  88     buffer->frame_count = (buffer->frame_count > er->wr_frames_in) ?
  89             er->wr_frames_in : buffer->frame_count;
  90     // this is er->rd_channel_count here as we resample after stereo to mono conversion if any
  91     buffer->i16 = (int16_t *)er->wr_src_buf + (er->wr_curr_frame_size - er->wr_frames_in) *
  92             er->rd_channel_count;
  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
 106     er = (struct echo_reference *)((char *)buffer_provider -
 107                                       offsetof(struct echo_reference, provider));
 108
 109     er->wr_frames_in -= buffer->frame_count;
 110 }
 111
 112 static void echo_reference_reset_l(struct echo_reference *er)
 113 {
 114     ALOGV("echo_reference_reset_l()");
 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;
 124     er->delta_count = 0;
 125     er->prev_delta_sign = 0;
 126 }
 127
 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
 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) {
 146         ALOGV("echo_reference_write() stop write");
 147         er->state &= ~ECHOREF_WRITING;
 148         echo_reference_reset_l(er);
 149         goto exit;
 150     }
 151
 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 "]",
 154             (int)buffer->time_stamp.tv_sec,
 155             (int)buffer->time_stamp.tv_nsec, er->playback_delay);
 156
 157     //ALOGV("echo_reference_write() %d frames", buffer->frame_count);
 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) {
 166         ALOGV("echo_reference_write() start write");
 167         if (er->resampler != NULL) {
 168             er->resampler->reset(er->resampler);
 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
 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
 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) {
 190         size_t wrBufSize = buffer->frame_count;
 191
 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) {
 205             ALOGV("echo_reference_write() increasing write buffer size from %zu to %zu",
 206                     er->wr_buf_size, wrBufSize);
 207             er->wr_buf_size = wrBufSize;
 208             er->wr_buf = realloc(er->wr_buf, er->wr_buf_size * er->rd_frame_size);
 209         }
 210
 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) {
 222             if (er->resampler == NULL) {
 223                 int rc;
 224                 ALOGV("echo_reference_write() new ReSampler(%d, %d)",
 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,
 231                                  RESAMPLER_QUALITY_DEFAULT,
 232                                  &er->provider,
 233                                  &er->resampler);
 234                 if (rc != 0) {
 235                     er->resampler = NULL;
 236                     ALOGV("echo_reference_write() failure to create resampler %d", rc);
 237                     status = -ENODEV;
 238                     goto exit;
 239                 }
 240             }
 241             // er->wr_src_buf and er->wr_frames_in are used by getNexBuffer() called by the
 242             // resampler to get new frames
 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
 251             ALOGV("echo_reference_write() ReSampling(%d, %d)",
 252                   er->wr_sampling_rate, er->rd_sampling_rate);
 253             er->resampler->resample_from_provider(er->resampler,
 254                                                      (int16_t *)er->wr_buf, &inFrames);
 255             ALOGV_IF(er->wr_frames_in != 0,
 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) {
 266         ALOGV("echo_reference_write() increasing buffer size from %zu to %zu",
 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
 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 "]",
 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);
 280
 281     pthread_cond_signal(&er->cond);
 282 exit:
 283     pthread_mutex_unlock(&er->lock);
 284     ALOGV("echo_reference_write() END");
 285     return status;
 286 }
 287
 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
 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) {
 307         ALOGV("echo_reference_read() stop read");
 308         er->state &= ~ECHOREF_READING;
 309         goto exit;
 310     }
 311
 312     ALOGV("echo_reference_read() START, delayCapture:[%" PRId32 "], "
 313             "er->frames_in:[%zu],buffer->frame_count:[%zu]",
 314     buffer->delay_ns, er->frames_in, buffer->frame_count);
 315
 316     if ((er->state & ECHOREF_READING) == 0) {
 317         ALOGV("echo_reference_read() start read");
 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
 328 //    ALOGV("echo_reference_read() %d frames", buffer->frame_count);
 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);
 333         struct timespec ts = {0, 0};
 334
 335         clock_gettime(CLOCK_REALTIME, &ts);
 336
 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
 344         pthread_cond_timedwait(&er->cond, &er->lock, &ts);
 345
 346         ALOGV_IF((er->frames_in < buffer->frame_count),
 347                  "echo_reference_read() waited %d ms but still not enough frames"\
 348                  " er->frames_in: %d, buffer->frame_count = %d",
 349                  timeoutMs, er->frames_in, buffer->frame_count);
 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)) {
 357         ALOGV("echo_reference_read(): NEW:timestamp is zero---------setting timeDiff = 0, "\
 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
 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
 378         ALOGV("echo_reference_read(): expectedDelayNs[%" PRId64 "] = "
 379                 "er->playback_delay[%" PRId32 "] + delayCapture[%" PRId32
 380                 "] - timeDiff[%" PRId64 "]",
 381                 expectedDelayNs, er->playback_delay, buffer->delay_ns, timeDiff);
 382
 383         if (expectedDelayNs > 0) {
 384             int64_t delayNs = ((int64_t)er->frames_in * 1000000000) / er->rd_sampling_rate;
 385
 386             int64_t  deltaNs = delayNs - expectedDelayNs;
 387
 388             ALOGV("echo_reference_read(): EchoPathDelayDeviation between reference and DMA [%"
 389                     PRId64 "]", deltaNs);
 390             if (llabs(deltaNs) >= MIN_DELAY_DELTA_NS) {
 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++;
 397                 } else {
 398                     er->delta_count = 1;
 399                 }
 400                 er->prev_delta_sign = delay_sign;
 401
 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;
 406
 407                     ALOGV("echo_reference_read(): deltaNs ENOUGH and %s: "
 408                             "er->frames_in: %zu, previousFrameIn = %zu",
 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);
 416                             ALOGV("echo_reference_read(): increasing buffer size to %zu",
 417                                   er->buf_size);
 418                         }
 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                         }
 425                     } else {
 426                         // More data available in the reference buffer than expected
 427                         offset = -offset;
 428                         if (offset > 0) {
 429                             memcpy(er->buffer, (char *)er->buffer + (offset * er->rd_frame_size),
 430                                    er->frames_in * er->rd_frame_size);
 431                             ALOGV("echo_reference_read(): shifting ref buffer by [%zu]",
 432                                   er->frames_in);
 433                         }
 434                     }
 435                 }
 436             } else {
 437                 er->delta_count = 0;
 438                 er->prev_delta_sign = 0;
 439                 ALOGV("echo_reference_read(): Constant EchoPathDelay - difference "
 440                         "between reference and DMA %" PRId64, deltaNs);
 441             }
 442         } else {
 443             ALOGV("echo_reference_read(): NEGATIVE expectedDelayNs[%" PRId64
 444                  "] = er->playback_delay[%" PRId32 "] + delayCapture[%" PRId32
 445                  "] - timeDiff[%" PRId64 "]",
 446                  expectedDelayNs, er->playback_delay, buffer->delay_ns, timeDiff);
 447         }
 448     }
 449
 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);
 454             ALOGV("echo_reference_read(): increasing buffer size to %zu", er->buf_size);
 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
 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
 474     ALOGV("echo_reference_read() END %zu frames, total frames in %zu",
 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
 495     ALOGV("create_echo_reference()");
 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) {
 505         ALOGW("create_echo_reference bad format rd %d, wr %d", rdFormat, wrFormat);
 506         return -EINVAL;
 507     }
 508     if ((rdChannelCount != 1 && rdChannelCount != 2) ||
 509             wrChannelCount != 2) {
 510         ALOGW("create_echo_reference bad channel count rd %d, wr %d", rdChannelCount,
 511                 wrChannelCount);
 512         return -EINVAL;
 513     }
 514
 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
 540     ALOGV("EchoReference dstor");
 541     echo_reference_reset_l(er);
 542     if (er->resampler != NULL) {
 543         release_resampler(er->resampler);
 544     }
 545     free(er);
 546 }
 547