1/*
2 *  Copyright (c) 2011 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 clock
12 * skew by resampling the farend signal.
13 */
14
15#include "aec_resampler.h"
16
17#include <assert.h>
18#include <stdlib.h>
19#include <string.h>
20#include <math.h>
21
22#include "aec_core.h"
23
24enum { kEstimateLengthFrames = 400 };
25
26typedef struct {
27    short buffer[kResamplerBufferSize];
28    float position;
29
30    int deviceSampleRateHz;
31    int skewData[kEstimateLengthFrames];
32    int skewDataIndex;
33    float skewEstimate;
34} resampler_t;
35
36static int EstimateSkew(const int* rawSkew,
37                        int size,
38                        int absLimit,
39                        float *skewEst);
40
41int WebRtcAec_CreateResampler(void **resampInst)
42{
43    resampler_t *obj = malloc(sizeof(resampler_t));
44    *resampInst = obj;
45    if (obj == NULL) {
46        return -1;
47    }
48
49    return 0;
50}
51
52int WebRtcAec_InitResampler(void *resampInst, int deviceSampleRateHz)
53{
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{
68    resampler_t *obj = (resampler_t*) resampInst;
69    free(obj);
70
71    return 0;
72}
73
74int WebRtcAec_ResampleLinear(void *resampInst,
75                             const short *inspeech,
76                             int size,
77                             float skew,
78                             short *outspeech)
79{
80    resampler_t *obj = (resampler_t*) resampInst;
81
82    short *y;
83    float be, tnew, interp;
84    int tn, outsize, mm;
85
86    if (size < 0 || size > 2 * FRAME_LEN) {
87        return -1;
88    }
89
90    // Add new frame data in lookahead
91    memcpy(&obj->buffer[FRAME_LEN + kResamplingDelay],
92           inspeech,
93           size * sizeof(short));
94
95    // Sample rate ratio
96    be = 1 + skew;
97
98    // Loop over input frame
99    mm = 0;
100    y = &obj->buffer[FRAME_LEN]; // Point at current frame
101
102    tnew = be * mm + obj->position;
103    tn = (int) tnew;
104
105    while (tn < size) {
106
107        // Interpolation
108        interp = y[tn] + (tnew - tn) * (y[tn+1] - y[tn]);
109
110        if (interp > 32767) {
111            interp = 32767;
112        }
113        else if (interp < -32768) {
114            interp = -32768;
115        }
116
117        outspeech[mm] = (short) interp;
118        mm++;
119
120        tnew = be * mm + obj->position;
121        tn = (int) tnew;
122    }
123
124    outsize = mm;
125    obj->position += outsize * be - size;
126
127    // Shift buffer
128    memmove(obj->buffer,
129            &obj->buffer[size],
130            (kResamplerBufferSize - size) * sizeof(short));
131
132    return outsize;
133}
134
135int WebRtcAec_GetSkew(void *resampInst, int rawSkew, float *skewEst)
136{
137    resampler_t *obj = (resampler_t*)resampInst;
138    int err = 0;
139
140    if (obj->skewDataIndex < kEstimateLengthFrames) {
141        obj->skewData[obj->skewDataIndex] = rawSkew;
142        obj->skewDataIndex++;
143    }
144    else if (obj->skewDataIndex == kEstimateLengthFrames) {
145        err = EstimateSkew(obj->skewData,
146                           kEstimateLengthFrames,
147                           obj->deviceSampleRateHz,
148                           skewEst);
149        obj->skewEstimate = *skewEst;
150        obj->skewDataIndex++;
151    }
152    else {
153        *skewEst = obj->skewEstimate;
154    }
155
156    return err;
157}
158
159int EstimateSkew(const int* rawSkew,
160                 int size,
161                 int deviceSampleRateHz,
162                 float *skewEst)
163{
164    const int absLimitOuter = (int)(0.04f * deviceSampleRateHz);
165    const int absLimitInner = (int)(0.0025f * deviceSampleRateHz);
166    int i = 0;
167    int n = 0;
168    float rawAvg = 0;
169    float err = 0;
170    float rawAbsDev = 0;
171    int upperLimit = 0;
172    int lowerLimit = 0;
173    float cumSum = 0;
174    float x = 0;
175    float x2 = 0;
176    float y = 0;
177    float xy = 0;
178    float xAvg = 0;
179    float denom = 0;
180    float skew = 0;
181
182    *skewEst = 0; // Set in case of error below.
183    for (i = 0; i < size; i++) {
184      if ((rawSkew[i] < absLimitOuter && rawSkew[i] > -absLimitOuter)) {
185        n++;
186        rawAvg += rawSkew[i];
187      }
188    }
189
190    if (n == 0) {
191      return -1;
192    }
193    assert(n > 0);
194    rawAvg /= n;
195
196    for (i = 0; i < size; i++) {
197      if ((rawSkew[i] < absLimitOuter && rawSkew[i] > -absLimitOuter)) {
198        err = rawSkew[i] - rawAvg;
199        rawAbsDev += err >= 0 ? err : -err;
200      }
201    }
202    assert(n > 0);
203    rawAbsDev /= n;
204    upperLimit = (int)(rawAvg + 5 * rawAbsDev + 1); // +1 for ceiling.
205    lowerLimit = (int)(rawAvg - 5 * rawAbsDev - 1); // -1 for floor.
206
207    n = 0;
208    for (i = 0; i < size; i++) {
209        if ((rawSkew[i] < absLimitInner && rawSkew[i] > -absLimitInner) ||
210            (rawSkew[i] < upperLimit && rawSkew[i] > lowerLimit)) {
211            n++;
212            cumSum += rawSkew[i];
213            x += n;
214            x2 += n*n;
215            y += cumSum;
216            xy += n * cumSum;
217        }
218    }
219
220    if (n == 0) {
221      return -1;
222    }
223    assert(n > 0);
224    xAvg = x / n;
225    denom = x2 - xAvg*x;
226
227    if (denom != 0) {
228        skew = (xy - xAvg*y) / denom;
229    }
230
231    *skewEst = skew;
232    return 0;
233}
234