1/*
2 * Copyright (C) 2007 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_TAG "AudioResamplerSinc"
18//#define LOG_NDEBUG 0
19
20#include <string.h>
21#include "AudioResamplerSinc.h"
22#include <dlfcn.h>
23#include <cutils/properties.h>
24#include <stdlib.h>
25#include <utils/Log.h>
26
27namespace android {
28// ----------------------------------------------------------------------------
29
30
31/*
32 * These coeficients are computed with the "fir" utility found in
33 * tools/resampler_tools
34 * TODO: A good optimization would be to transpose this matrix, to take
35 * better advantage of the data-cache.
36 */
37const int32_t AudioResamplerSinc::mFirCoefsUp[] = {
38        0x7fffffff, 0x7f15d078, 0x7c5e0da6, 0x77ecd867, 0x71e2e251, 0x6a6c304a, 0x61be7269, 0x58170412, 0x4db8ab05, 0x42e92ea6, 0x37eee214, 0x2d0e3bb1, 0x22879366, 0x18951e95, 0x0f693d0d, 0x072d2621,
39        0x00000000, 0xf9f66655, 0xf51a5fd7, 0xf16bbd84, 0xeee0d9ac, 0xed67a922, 0xece70de6, 0xed405897, 0xee50e505, 0xeff3be30, 0xf203370f, 0xf45a6741, 0xf6d67d53, 0xf957db66, 0xfbc2f647, 0xfe00f2b9,
40        0x00000000, 0x01b37218, 0x0313a0c6, 0x041d930d, 0x04d28057, 0x053731b0, 0x05534dff, 0x05309bfd, 0x04da440d, 0x045c1aee, 0x03c1fcdd, 0x03173ef5, 0x02663ae8, 0x01b7f736, 0x0113ec79, 0x007fe6a9,
41        0x00000000, 0xff96b229, 0xff44f99f, 0xff0a86be, 0xfee5f803, 0xfed518fd, 0xfed521fd, 0xfee2f4fd, 0xfefb54f8, 0xff1b159b, 0xff3f4203, 0xff6539e0, 0xff8ac502, 0xffae1ddd, 0xffcdf3f9, 0xffe96798,
42        0x00000000, 0x00119de6, 0x001e6b7e, 0x0026cb7a, 0x002b4830, 0x002c83d6, 0x002b2a82, 0x0027e67a, 0x002356f9, 0x001e098e, 0x001875e4, 0x0012fbbe, 0x000de2d1, 0x00095c10, 0x00058414, 0x00026636,
43        0x00000000, 0xfffe44a9, 0xfffd206d, 0xfffc7b7f, 0xfffc3c8f, 0xfffc4ac2, 0xfffc8f2b, 0xfffcf5c4, 0xfffd6df3, 0xfffdeab2, 0xfffe6275, 0xfffececf, 0xffff2c07, 0xffff788c, 0xffffb471, 0xffffe0f2,
44        0x00000000, 0x000013e6, 0x00001f03, 0x00002396, 0x00002399, 0x000020b6, 0x00001c3c, 0x00001722, 0x00001216, 0x00000d81, 0x0000099c, 0x0000067c, 0x00000419, 0x0000025f, 0x00000131, 0x00000070,
45        0x00000000, 0xffffffc7, 0xffffffb3, 0xffffffb3, 0xffffffbe, 0xffffffcd, 0xffffffdb, 0xffffffe7, 0xfffffff0, 0xfffffff7, 0xfffffffb, 0xfffffffe, 0xffffffff, 0x00000000, 0x00000000, 0x00000000,
46        0x00000000 // this one is needed for lerping the last coefficient
47};
48
49/*
50 * These coefficients are optimized for 48KHz -> 44.1KHz (stop-band at 22.050KHz)
51 * It's possible to use the above coefficient for any down-sampling
52 * at the expense of a slower processing loop (we can interpolate
53 * these coefficient from the above by "Stretching" them in time).
54 */
55const int32_t AudioResamplerSinc::mFirCoefsDown[] = {
56        0x7fffffff, 0x7f55e46d, 0x7d5b4c60, 0x7a1b4b98, 0x75a7fb14, 0x7019f0bd, 0x698f875a, 0x622bfd59, 0x5a167256, 0x5178cc54, 0x487e8e6c, 0x3f53aae8, 0x36235ad4, 0x2d17047b, 0x245539ab, 0x1c00d540,
57        0x14383e57, 0x0d14d5ca, 0x06aa910b, 0x0107c38b, 0xfc351654, 0xf835abae, 0xf5076b45, 0xf2a37202, 0xf0fe9faa, 0xf00a3bbd, 0xefb4aa81, 0xefea2b05, 0xf0959716, 0xf1a11e83, 0xf2f6f7a0, 0xf481fff4,
58        0xf62e48ce, 0xf7e98ca5, 0xf9a38b4c, 0xfb4e4bfa, 0xfcde456f, 0xfe4a6d30, 0xff8c2fdf, 0x009f5555, 0x0181d393, 0x0233940f, 0x02b62f06, 0x030ca07d, 0x033afa62, 0x03461725, 0x03334f83, 0x030835fa,
59        0x02ca59cc, 0x027f12d1, 0x022b570d, 0x01d39a49, 0x017bb78f, 0x0126e414, 0x00d7aaaf, 0x008feec7, 0x0050f584, 0x001b73e3, 0xffefa063, 0xffcd46ed, 0xffb3ddcd, 0xffa29aaa, 0xff988691, 0xff949066,
60        0xff959d24, 0xff9a959e, 0xffa27195, 0xffac4011, 0xffb72d2b, 0xffc28569, 0xffcdb706, 0xffd85171, 0xffe20364, 0xffea97e9, 0xfff1f2b2, 0xfff80c06, 0xfffcec92, 0x0000a955, 0x00035fd8, 0x000532cf,
61        0x00064735, 0x0006c1f9, 0x0006c62d, 0x000673ba, 0x0005e68f, 0x00053630, 0x000475a3, 0x0003b397, 0x0002fac1, 0x00025257, 0x0001be9e, 0x0001417a, 0x0000dafd, 0x000089eb, 0x00004c28, 0x00001f1d,
62        0x00000000, 0xffffec10, 0xffffe0be, 0xffffdbc5, 0xffffdb39, 0xffffdd8b, 0xffffe182, 0xffffe638, 0xffffeb0a, 0xffffef8f, 0xfffff38b, 0xfffff6e3, 0xfffff993, 0xfffffba6, 0xfffffd30, 0xfffffe4a,
63        0xffffff09, 0xffffff85, 0xffffffd1, 0xfffffffb, 0x0000000f, 0x00000016, 0x00000015, 0x00000012, 0x0000000d, 0x00000009, 0x00000006, 0x00000003, 0x00000002, 0x00000001, 0x00000000, 0x00000000,
64        0x00000000 // this one is needed for lerping the last coefficient
65};
66
67// we use 15 bits to interpolate between these samples
68// this cannot change because the mul below rely on it.
69static const int pLerpBits = 15;
70
71static pthread_once_t once_control = PTHREAD_ONCE_INIT;
72static readCoefficientsFn readResampleCoefficients = NULL;
73
74/*static*/ AudioResamplerSinc::Constants AudioResamplerSinc::highQualityConstants;
75/*static*/ AudioResamplerSinc::Constants AudioResamplerSinc::veryHighQualityConstants;
76
77void AudioResamplerSinc::init_routine()
78{
79    // for high quality resampler, the parameters for coefficients are compile-time constants
80    Constants *c = &highQualityConstants;
81    c->coefsBits = RESAMPLE_FIR_LERP_INT_BITS;
82    c->cShift = kNumPhaseBits - c->coefsBits;
83    c->cMask = ((1<< c->coefsBits)-1) << c->cShift;
84    c->pShift = kNumPhaseBits - c->coefsBits - pLerpBits;
85    c->pMask = ((1<< pLerpBits)-1) << c->pShift;
86    c->halfNumCoefs = RESAMPLE_FIR_NUM_COEF;
87
88    // for very high quality resampler, the parameters are load-time constants
89    veryHighQualityConstants = highQualityConstants;
90
91    // Open the dll to get the coefficients for VERY_HIGH_QUALITY
92    void *resampleCoeffLib = dlopen("libaudio-resampler.so", RTLD_NOW);
93    ALOGV("Open libaudio-resampler library = %p", resampleCoeffLib);
94    if (resampleCoeffLib == NULL) {
95        ALOGE("Could not open audio-resampler library: %s", dlerror());
96        return;
97    }
98
99    readResampleCoefficients = (readCoefficientsFn) dlsym(resampleCoeffLib,
100            "readResamplerCoefficients");
101    readResampleFirNumCoeffFn readResampleFirNumCoeff = (readResampleFirNumCoeffFn)
102            dlsym(resampleCoeffLib, "readResampleFirNumCoeff");
103    readResampleFirLerpIntBitsFn readResampleFirLerpIntBits = (readResampleFirLerpIntBitsFn)
104            dlsym(resampleCoeffLib, "readResampleFirLerpIntBits");
105    if (!readResampleCoefficients || !readResampleFirNumCoeff || !readResampleFirLerpIntBits) {
106        readResampleCoefficients = NULL;
107        dlclose(resampleCoeffLib);
108        resampleCoeffLib = NULL;
109        ALOGE("Could not find symbol: %s", dlerror());
110        return;
111    }
112
113    c = &veryHighQualityConstants;
114    // we have 16 coefs samples per zero-crossing
115    c->coefsBits = readResampleFirLerpIntBits();
116    ALOGV("coefsBits = %d", c->coefsBits);
117    c->cShift = kNumPhaseBits - c->coefsBits;
118    c->cMask = ((1<<c->coefsBits)-1) << c->cShift;
119    c->pShift = kNumPhaseBits - c->coefsBits - pLerpBits;
120    c->pMask = ((1<<pLerpBits)-1) << c->pShift;
121    // number of zero-crossing on each side
122    c->halfNumCoefs = readResampleFirNumCoeff();
123    ALOGV("halfNumCoefs = %d", c->halfNumCoefs);
124    // note that we "leak" resampleCoeffLib until the process exits
125}
126
127// ----------------------------------------------------------------------------
128
129static inline
130int32_t mulRL(int left, int32_t in, uint32_t vRL)
131{
132#if defined(__arm__) && !defined(__thumb__)
133    int32_t out;
134    if (left) {
135        asm( "smultb %[out], %[in], %[vRL] \n"
136             : [out]"=r"(out)
137             : [in]"%r"(in), [vRL]"r"(vRL)
138             : );
139    } else {
140        asm( "smultt %[out], %[in], %[vRL] \n"
141             : [out]"=r"(out)
142             : [in]"%r"(in), [vRL]"r"(vRL)
143             : );
144    }
145    return out;
146#else
147    if (left) {
148        return int16_t(in>>16) * int16_t(vRL&0xFFFF);
149    } else {
150        return int16_t(in>>16) * int16_t(vRL>>16);
151    }
152#endif
153}
154
155static inline
156int32_t mulAdd(int16_t in, int32_t v, int32_t a)
157{
158#if defined(__arm__) && !defined(__thumb__)
159    int32_t out;
160    asm( "smlawb %[out], %[v], %[in], %[a] \n"
161         : [out]"=r"(out)
162         : [in]"%r"(in), [v]"r"(v), [a]"r"(a)
163         : );
164    return out;
165#else
166    return a + in * (v>>16);
167    // improved precision
168    // return a + in * (v>>16) + ((in * (v & 0xffff)) >> 16);
169#endif
170}
171
172static inline
173int32_t mulAddRL(int left, uint32_t inRL, int32_t v, int32_t a)
174{
175#if defined(__arm__) && !defined(__thumb__)
176    int32_t out;
177    if (left) {
178        asm( "smlawb %[out], %[v], %[inRL], %[a] \n"
179             : [out]"=r"(out)
180             : [inRL]"%r"(inRL), [v]"r"(v), [a]"r"(a)
181             : );
182    } else {
183        asm( "smlawt %[out], %[v], %[inRL], %[a] \n"
184             : [out]"=r"(out)
185             : [inRL]"%r"(inRL), [v]"r"(v), [a]"r"(a)
186             : );
187    }
188    return out;
189#else
190    if (left) {
191        return a + (int16_t(inRL&0xFFFF) * (v>>16));
192        //improved precision
193        // return a + (int16_t(inRL&0xFFFF) * (v>>16)) + ((int16_t(inRL&0xFFFF) * (v & 0xffff)) >> 16);
194    } else {
195        return a + (int16_t(inRL>>16) * (v>>16));
196    }
197#endif
198}
199
200// ----------------------------------------------------------------------------
201
202AudioResamplerSinc::AudioResamplerSinc(int bitDepth,
203        int inChannelCount, int32_t sampleRate, src_quality quality)
204    : AudioResampler(bitDepth, inChannelCount, sampleRate, quality),
205    mState(0)
206{
207    /*
208     * Layout of the state buffer for 32 tap:
209     *
210     * "present" sample            beginning of 2nd buffer
211     *                 v                v
212     *  0              01               2              23              3
213     *  0              F0               0              F0              F
214     * [pppppppppppppppInnnnnnnnnnnnnnnnpppppppppppppppInnnnnnnnnnnnnnnn]
215     *                 ^               ^ head
216     *
217     * p = past samples, convoluted with the (p)ositive side of sinc()
218     * n = future samples, convoluted with the (n)egative side of sinc()
219     * r = extra space for implementing the ring buffer
220     *
221     */
222
223    // Load the constants for coefficients
224    int ok = pthread_once(&once_control, init_routine);
225    if (ok != 0) {
226        ALOGE("%s pthread_once failed: %d", __func__, ok);
227    }
228    mConstants = (quality == VERY_HIGH_QUALITY) ? &veryHighQualityConstants : &highQualityConstants;
229}
230
231
232AudioResamplerSinc::~AudioResamplerSinc()
233{
234    delete[] mState;
235}
236
237void AudioResamplerSinc::init() {
238    const Constants *c = mConstants;
239
240    const size_t numCoefs = 2*c->halfNumCoefs;
241    const size_t stateSize = numCoefs * mChannelCount * 2;
242    mState = new int16_t[stateSize];
243    memset(mState, 0, sizeof(int16_t)*stateSize);
244    mImpulse = mState + (c->halfNumCoefs-1)*mChannelCount;
245    mRingFull = mImpulse + (numCoefs+1)*mChannelCount;
246}
247
248void AudioResamplerSinc::resample(int32_t* out, size_t outFrameCount,
249            AudioBufferProvider* provider)
250{
251
252    // FIXME store current state (up or down sample) and only load the coefs when the state
253    // changes. Or load two pointers one for up and one for down in the init function.
254    // Not critical now since the read functions are fast, but would be important if read was slow.
255    if (mConstants == &veryHighQualityConstants && readResampleCoefficients) {
256        ALOGV("get coefficient from libmm-audio resampler library");
257        mFirCoefs = (mInSampleRate <= mSampleRate) ? readResampleCoefficients(true) :
258                readResampleCoefficients(false);
259    } else {
260        ALOGV("Use default coefficients");
261        mFirCoefs = (mInSampleRate <= mSampleRate) ? mFirCoefsUp : mFirCoefsDown;
262    }
263
264    // select the appropriate resampler
265    switch (mChannelCount) {
266    case 1:
267        resample<1>(out, outFrameCount, provider);
268        break;
269    case 2:
270        resample<2>(out, outFrameCount, provider);
271        break;
272    }
273
274}
275
276
277template<int CHANNELS>
278void AudioResamplerSinc::resample(int32_t* out, size_t outFrameCount,
279        AudioBufferProvider* provider)
280{
281    const Constants *c = mConstants;
282    int16_t* impulse = mImpulse;
283    uint32_t vRL = mVolumeRL;
284    size_t inputIndex = mInputIndex;
285    uint32_t phaseFraction = mPhaseFraction;
286    uint32_t phaseIncrement = mPhaseIncrement;
287    size_t outputIndex = 0;
288    size_t outputSampleCount = outFrameCount * 2;
289    size_t inFrameCount = (outFrameCount*mInSampleRate)/mSampleRate;
290
291    while (outputIndex < outputSampleCount) {
292        // buffer is empty, fetch a new one
293        while (mBuffer.frameCount == 0) {
294            mBuffer.frameCount = inFrameCount;
295            provider->getNextBuffer(&mBuffer,
296                                    calculateOutputPTS(outputIndex / 2));
297            if (mBuffer.raw == NULL) {
298                goto resample_exit;
299            }
300            const uint32_t phaseIndex = phaseFraction >> kNumPhaseBits;
301            if (phaseIndex == 1) {
302                // read one frame
303                read<CHANNELS>(impulse, phaseFraction, mBuffer.i16, inputIndex);
304            } else if (phaseIndex == 2) {
305                // read 2 frames
306                read<CHANNELS>(impulse, phaseFraction, mBuffer.i16, inputIndex);
307                inputIndex++;
308                if (inputIndex >= mBuffer.frameCount) {
309                    inputIndex -= mBuffer.frameCount;
310                    provider->releaseBuffer(&mBuffer);
311                } else {
312                    read<CHANNELS>(impulse, phaseFraction, mBuffer.i16, inputIndex);
313                }
314            }
315        }
316        int16_t *in = mBuffer.i16;
317        const size_t frameCount = mBuffer.frameCount;
318
319        // Always read-in the first samples from the input buffer
320        int16_t* head = impulse + c->halfNumCoefs*CHANNELS;
321        head[0] = in[inputIndex*CHANNELS + 0];
322        if (CHANNELS == 2)
323            head[1] = in[inputIndex*CHANNELS + 1];
324
325        // handle boundary case
326        int32_t l, r;
327        while (outputIndex < outputSampleCount) {
328            filterCoefficient<CHANNELS>(l, r, phaseFraction, impulse);
329            out[outputIndex++] += 2 * mulRL(1, l, vRL);
330            out[outputIndex++] += 2 * mulRL(0, r, vRL);
331
332            phaseFraction += phaseIncrement;
333            const uint32_t phaseIndex = phaseFraction >> kNumPhaseBits;
334            if (phaseIndex == 1) {
335                inputIndex++;
336                if (inputIndex >= frameCount)
337                    break;  // need a new buffer
338                read<CHANNELS>(impulse, phaseFraction, in, inputIndex);
339            } else if (phaseIndex == 2) {    // maximum value
340                inputIndex++;
341                if (inputIndex >= frameCount)
342                    break;  // 0 frame available, 2 frames needed
343                // read first frame
344                read<CHANNELS>(impulse, phaseFraction, in, inputIndex);
345                inputIndex++;
346                if (inputIndex >= frameCount)
347                    break;  // 0 frame available, 1 frame needed
348                // read second frame
349                read<CHANNELS>(impulse, phaseFraction, in, inputIndex);
350            }
351        }
352
353        // if done with buffer, save samples
354        if (inputIndex >= frameCount) {
355            inputIndex -= frameCount;
356            provider->releaseBuffer(&mBuffer);
357        }
358    }
359
360resample_exit:
361    mImpulse = impulse;
362    mInputIndex = inputIndex;
363    mPhaseFraction = phaseFraction;
364}
365
366template<int CHANNELS>
367/***
368* read()
369*
370* This function reads only one frame from input buffer and writes it in
371* state buffer
372*
373**/
374void AudioResamplerSinc::read(
375        int16_t*& impulse, uint32_t& phaseFraction,
376        const int16_t* in, size_t inputIndex)
377{
378    const Constants *c = mConstants;
379    const uint32_t phaseIndex = phaseFraction >> kNumPhaseBits;
380    impulse += CHANNELS;
381    phaseFraction -= 1LU<<kNumPhaseBits;
382    if (impulse >= mRingFull) {
383        const size_t stateSize = (c->halfNumCoefs*2)*CHANNELS;
384        memcpy(mState, mState+stateSize, sizeof(int16_t)*stateSize);
385        impulse -= stateSize;
386    }
387    int16_t* head = impulse + c->halfNumCoefs*CHANNELS;
388    head[0] = in[inputIndex*CHANNELS + 0];
389    if (CHANNELS == 2)
390        head[1] = in[inputIndex*CHANNELS + 1];
391}
392
393template<int CHANNELS>
394void AudioResamplerSinc::filterCoefficient(
395        int32_t& l, int32_t& r, uint32_t phase, const int16_t *samples)
396{
397    const Constants *c = mConstants;
398
399    // compute the index of the coefficient on the positive side and
400    // negative side
401    uint32_t indexP = (phase & c->cMask) >> c->cShift;
402    uint16_t lerpP = (phase & c->pMask) >> c->pShift;
403    uint32_t indexN = (-phase & c->cMask) >> c->cShift;
404    uint16_t lerpN = (-phase & c->pMask) >> c->pShift;
405    if ((indexP == 0) && (lerpP == 0)) {
406        indexN = c->cMask >> c->cShift;
407        lerpN = c->pMask >> c->pShift;
408    }
409
410    l = 0;
411    r = 0;
412    const int32_t* coefs = mFirCoefs;
413    const int16_t *sP = samples;
414    const int16_t *sN = samples+CHANNELS;
415    for (unsigned int i=0 ; i < c->halfNumCoefs/4 ; i++) {
416        interpolate<CHANNELS>(l, r, coefs+indexP, lerpP, sP);
417        interpolate<CHANNELS>(l, r, coefs+indexN, lerpN, sN);
418        sP -= CHANNELS; sN += CHANNELS; coefs += 1 << c->coefsBits;
419        interpolate<CHANNELS>(l, r, coefs+indexP, lerpP, sP);
420        interpolate<CHANNELS>(l, r, coefs+indexN, lerpN, sN);
421        sP -= CHANNELS; sN += CHANNELS; coefs += 1 << c->coefsBits;
422        interpolate<CHANNELS>(l, r, coefs+indexP, lerpP, sP);
423        interpolate<CHANNELS>(l, r, coefs+indexN, lerpN, sN);
424        sP -= CHANNELS; sN += CHANNELS; coefs += 1 << c->coefsBits;
425        interpolate<CHANNELS>(l, r, coefs+indexP, lerpP, sP);
426        interpolate<CHANNELS>(l, r, coefs+indexN, lerpN, sN);
427        sP -= CHANNELS; sN += CHANNELS; coefs += 1 << c->coefsBits;
428    }
429}
430
431template<int CHANNELS>
432void AudioResamplerSinc::interpolate(
433        int32_t& l, int32_t& r,
434        const int32_t* coefs, int16_t lerp, const int16_t* samples)
435{
436    int32_t c0 = coefs[0];
437    int32_t c1 = coefs[1];
438    int32_t sinc = mulAdd(lerp, (c1-c0)<<1, c0);
439    if (CHANNELS == 2) {
440        uint32_t rl = *reinterpret_cast<const uint32_t*>(samples);
441        l = mulAddRL(1, rl, sinc, l);
442        r = mulAddRL(0, rl, sinc, r);
443    } else {
444        r = l = mulAdd(samples[0], sinc, l);
445    }
446}
447// ----------------------------------------------------------------------------
448}; // namespace android
449