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} resampler_t;
37
38static int EstimateSkew(const int* rawSkew,
39                        int size,
40                        int absLimit,
41                        float* skewEst);
42
43int WebRtcAec_CreateResampler(void** resampInst) {
44  resampler_t* obj = malloc(sizeof(resampler_t));
45  *resampInst = obj;
46  if (obj == NULL) {
47    return -1;
48  }
49
50  return 0;
51}
52
53int WebRtcAec_InitResampler(void* resampInst, int deviceSampleRateHz) {
54  resampler_t* obj = (resampler_t*)resampInst;
55  memset(obj->buffer, 0, sizeof(obj->buffer));
56  obj->position = 0.0;
57
58  obj->deviceSampleRateHz = deviceSampleRateHz;
59  memset(obj->skewData, 0, sizeof(obj->skewData));
60  obj->skewDataIndex = 0;
61  obj->skewEstimate = 0.0;
62
63  return 0;
64}
65
66int WebRtcAec_FreeResampler(void* resampInst) {
67  resampler_t* obj = (resampler_t*)resampInst;
68  free(obj);
69
70  return 0;
71}
72
73void WebRtcAec_ResampleLinear(void* resampInst,
74                              const float* inspeech,
75                              int size,
76                              float skew,
77                              float* outspeech,
78                              int* size_out) {
79  resampler_t* obj = (resampler_t*)resampInst;
80
81  float* y;
82  float be, tnew;
83  int tn, mm;
84
85  assert(!(size < 0 || size > 2 * FRAME_LEN));
86  assert(resampInst != NULL);
87  assert(inspeech != NULL);
88  assert(outspeech != NULL);
89  assert(size_out != NULL);
90
91  // Add new frame data in lookahead
92  memcpy(&obj->buffer[FRAME_LEN + kResamplingDelay],
93         inspeech,
94         size * sizeof(inspeech[0]));
95
96  // Sample rate ratio
97  be = 1 + skew;
98
99  // Loop over input frame
100  mm = 0;
101  y = &obj->buffer[FRAME_LEN];  // Point at current frame
102
103  tnew = be * mm + obj->position;
104  tn = (int)tnew;
105
106  while (tn < size) {
107
108    // Interpolation
109    outspeech[mm] = y[tn] + (tnew - tn) * (y[tn + 1] - y[tn]);
110    mm++;
111
112    tnew = be * mm + obj->position;
113    tn = (int)tnew;
114  }
115
116  *size_out = mm;
117  obj->position += (*size_out) * be - size;
118
119  // Shift buffer
120  memmove(obj->buffer,
121          &obj->buffer[size],
122          (kResamplerBufferSize - size) * sizeof(obj->buffer[0]));
123}
124
125int WebRtcAec_GetSkew(void* resampInst, int rawSkew, float* skewEst) {
126  resampler_t* obj = (resampler_t*)resampInst;
127  int err = 0;
128
129  if (obj->skewDataIndex < kEstimateLengthFrames) {
130    obj->skewData[obj->skewDataIndex] = rawSkew;
131    obj->skewDataIndex++;
132  } else if (obj->skewDataIndex == kEstimateLengthFrames) {
133    err = EstimateSkew(
134        obj->skewData, kEstimateLengthFrames, obj->deviceSampleRateHz, skewEst);
135    obj->skewEstimate = *skewEst;
136    obj->skewDataIndex++;
137  } else {
138    *skewEst = obj->skewEstimate;
139  }
140
141  return err;
142}
143
144int EstimateSkew(const int* rawSkew,
145                 int size,
146                 int deviceSampleRateHz,
147                 float* skewEst) {
148  const int absLimitOuter = (int)(0.04f * deviceSampleRateHz);
149  const int absLimitInner = (int)(0.0025f * deviceSampleRateHz);
150  int i = 0;
151  int n = 0;
152  float rawAvg = 0;
153  float err = 0;
154  float rawAbsDev = 0;
155  int upperLimit = 0;
156  int lowerLimit = 0;
157  float cumSum = 0;
158  float x = 0;
159  float x2 = 0;
160  float y = 0;
161  float xy = 0;
162  float xAvg = 0;
163  float denom = 0;
164  float skew = 0;
165
166  *skewEst = 0;  // Set in case of error below.
167  for (i = 0; i < size; i++) {
168    if ((rawSkew[i] < absLimitOuter && rawSkew[i] > -absLimitOuter)) {
169      n++;
170      rawAvg += rawSkew[i];
171    }
172  }
173
174  if (n == 0) {
175    return -1;
176  }
177  assert(n > 0);
178  rawAvg /= n;
179
180  for (i = 0; i < size; i++) {
181    if ((rawSkew[i] < absLimitOuter && rawSkew[i] > -absLimitOuter)) {
182      err = rawSkew[i] - rawAvg;
183      rawAbsDev += err >= 0 ? err : -err;
184    }
185  }
186  assert(n > 0);
187  rawAbsDev /= n;
188  upperLimit = (int)(rawAvg + 5 * rawAbsDev + 1);  // +1 for ceiling.
189  lowerLimit = (int)(rawAvg - 5 * rawAbsDev - 1);  // -1 for floor.
190
191  n = 0;
192  for (i = 0; i < size; i++) {
193    if ((rawSkew[i] < absLimitInner && rawSkew[i] > -absLimitInner) ||
194        (rawSkew[i] < upperLimit && rawSkew[i] > lowerLimit)) {
195      n++;
196      cumSum += rawSkew[i];
197      x += n;
198      x2 += n * n;
199      y += cumSum;
200      xy += n * cumSum;
201    }
202  }
203
204  if (n == 0) {
205    return -1;
206  }
207  assert(n > 0);
208  xAvg = x / n;
209  denom = x2 - xAvg * x;
210
211  if (denom != 0) {
212    skew = (xy - xAvg * y) / denom;
213  }
214
215  *skewEst = skew;
216  return 0;
217}
218