1/*
2 *  Copyright (c) 2012 The WebRTC project authors. All Rights Reserved.
3 *
4 *  Use of this source code is governed by a BSD-style license
5 *  that can be found in the LICENSE file in the root of the source
6 *  tree. An additional intellectual property rights grant can be found
7 *  in the file PATENTS.  All contributing project authors may
8 *  be found in the AUTHORS file in the root of the source tree.
9 */
10
11/* Resamples a signal to an arbitrary rate. Used by the AEC to compensate for
12 * clock skew by resampling the farend signal.
13 */
14
15#include "webrtc/modules/audio_processing/aec/aec_resampler.h"
16
17#include <assert.h>
18#include <math.h>
19#include <stdlib.h>
20#include <string.h>
21
22#include "webrtc/modules/audio_processing/aec/aec_core.h"
23
24enum {
25  kEstimateLengthFrames = 400
26};
27
28typedef struct {
29  float buffer[kResamplerBufferSize];
30  float position;
31
32  int deviceSampleRateHz;
33  int skewData[kEstimateLengthFrames];
34  int skewDataIndex;
35  float skewEstimate;
36} AecResampler;
37
38static int EstimateSkew(const int* rawSkew,
39                        int size,
40                        int absLimit,
41                        float* skewEst);
42
43void* WebRtcAec_CreateResampler() {
44  return malloc(sizeof(AecResampler));
45}
46
47int WebRtcAec_InitResampler(void* resampInst, int deviceSampleRateHz) {
48  AecResampler* obj = (AecResampler*)resampInst;
49  memset(obj->buffer, 0, sizeof(obj->buffer));
50  obj->position = 0.0;
51
52  obj->deviceSampleRateHz = deviceSampleRateHz;
53  memset(obj->skewData, 0, sizeof(obj->skewData));
54  obj->skewDataIndex = 0;
55  obj->skewEstimate = 0.0;
56
57  return 0;
58}
59
60void WebRtcAec_FreeResampler(void* resampInst) {
61  AecResampler* obj = (AecResampler*)resampInst;
62  free(obj);
63}
64
65void WebRtcAec_ResampleLinear(void* resampInst,
66                              const float* inspeech,
67                              size_t size,
68                              float skew,
69                              float* outspeech,
70                              size_t* size_out) {
71  AecResampler* obj = (AecResampler*)resampInst;
72
73  float* y;
74  float be, tnew;
75  size_t tn, mm;
76
77  assert(size <= 2 * FRAME_LEN);
78  assert(resampInst != NULL);
79  assert(inspeech != NULL);
80  assert(outspeech != NULL);
81  assert(size_out != NULL);
82
83  // Add new frame data in lookahead
84  memcpy(&obj->buffer[FRAME_LEN + kResamplingDelay],
85         inspeech,
86         size * sizeof(inspeech[0]));
87
88  // Sample rate ratio
89  be = 1 + skew;
90
91  // Loop over input frame
92  mm = 0;
93  y = &obj->buffer[FRAME_LEN];  // Point at current frame
94
95  tnew = be * mm + obj->position;
96  tn = (size_t)tnew;
97
98  while (tn < size) {
99
100    // Interpolation
101    outspeech[mm] = y[tn] + (tnew - tn) * (y[tn + 1] - y[tn]);
102    mm++;
103
104    tnew = be * mm + obj->position;
105    tn = (int)tnew;
106  }
107
108  *size_out = mm;
109  obj->position += (*size_out) * be - size;
110
111  // Shift buffer
112  memmove(obj->buffer,
113          &obj->buffer[size],
114          (kResamplerBufferSize - size) * sizeof(obj->buffer[0]));
115}
116
117int WebRtcAec_GetSkew(void* resampInst, int rawSkew, float* skewEst) {
118  AecResampler* obj = (AecResampler*)resampInst;
119  int err = 0;
120
121  if (obj->skewDataIndex < kEstimateLengthFrames) {
122    obj->skewData[obj->skewDataIndex] = rawSkew;
123    obj->skewDataIndex++;
124  } else if (obj->skewDataIndex == kEstimateLengthFrames) {
125    err = EstimateSkew(
126        obj->skewData, kEstimateLengthFrames, obj->deviceSampleRateHz, skewEst);
127    obj->skewEstimate = *skewEst;
128    obj->skewDataIndex++;
129  } else {
130    *skewEst = obj->skewEstimate;
131  }
132
133  return err;
134}
135
136int EstimateSkew(const int* rawSkew,
137                 int size,
138                 int deviceSampleRateHz,
139                 float* skewEst) {
140  const int absLimitOuter = (int)(0.04f * deviceSampleRateHz);
141  const int absLimitInner = (int)(0.0025f * deviceSampleRateHz);
142  int i = 0;
143  int n = 0;
144  float rawAvg = 0;
145  float err = 0;
146  float rawAbsDev = 0;
147  int upperLimit = 0;
148  int lowerLimit = 0;
149  float cumSum = 0;
150  float x = 0;
151  float x2 = 0;
152  float y = 0;
153  float xy = 0;
154  float xAvg = 0;
155  float denom = 0;
156  float skew = 0;
157
158  *skewEst = 0;  // Set in case of error below.
159  for (i = 0; i < size; i++) {
160    if ((rawSkew[i] < absLimitOuter && rawSkew[i] > -absLimitOuter)) {
161      n++;
162      rawAvg += rawSkew[i];
163    }
164  }
165
166  if (n == 0) {
167    return -1;
168  }
169  assert(n > 0);
170  rawAvg /= n;
171
172  for (i = 0; i < size; i++) {
173    if ((rawSkew[i] < absLimitOuter && rawSkew[i] > -absLimitOuter)) {
174      err = rawSkew[i] - rawAvg;
175      rawAbsDev += err >= 0 ? err : -err;
176    }
177  }
178  assert(n > 0);
179  rawAbsDev /= n;
180  upperLimit = (int)(rawAvg + 5 * rawAbsDev + 1);  // +1 for ceiling.
181  lowerLimit = (int)(rawAvg - 5 * rawAbsDev - 1);  // -1 for floor.
182
183  n = 0;
184  for (i = 0; i < size; i++) {
185    if ((rawSkew[i] < absLimitInner && rawSkew[i] > -absLimitInner) ||
186        (rawSkew[i] < upperLimit && rawSkew[i] > lowerLimit)) {
187      n++;
188      cumSum += rawSkew[i];
189      x += n;
190      x2 += n * n;
191      y += cumSum;
192      xy += n * cumSum;
193    }
194  }
195
196  if (n == 0) {
197    return -1;
198  }
199  assert(n > 0);
200  xAvg = x / n;
201  denom = x2 - xAvg * x;
202
203  if (denom != 0) {
204    skew = (xy - xAvg * y) / denom;
205  }
206
207  *skewEst = skew;
208  return 0;
209}
210