////  AudioSRC.cpp//  libraries/audio/src////  Created by Ken Cooke on 9/18/15.//  Copyright 2015 High Fidelity, Inc.////  Distributed under the Apache License, Version 2.0.//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html// #include "AudioSRC.h" #include <assert.h>#include <stdlib.h>#include <string.h> #include "AudioSRCData.h" #ifndef MAX#define MAX(a,b)  (((a) > (b)) ? (a) : (b))#endif#ifndef MIN#define MIN(a,b)  (((a) < (b)) ? (a) : (b))#endif // high/low part of int64_t#define LO32(a)   ((uint32_t)(a))#define HI32(a)   ((int32_t)((a) >> 32)) //// Portable aligned malloc/free//static void* aligned_malloc(size_t size, size_t alignment) {    if ((alignment & (alignment-1)) == 0) {        void* p = malloc(size + sizeof(void*) + (alignment-1));        if (p) {            void* ptr = (void*)(((size_t)p + sizeof(void*) + (alignment-1)) & ~(alignment-1));            ((void**)ptr)[-1] = p;            return ptr;        }    }    return nullptr;} static void aligned_free(void* ptr) {    if (ptr) {        void* p = ((void**)ptr)[-1];        free(p);    }} // find the greatest common divisorstatic int gcd(int a, int b){    while (a != b) {        if (a > b) {            a -= b;        } else {            b -= a;        }    }    return a;} //// 3rd-order Lagrange interpolation//// Lagrange interpolation is maximally flat near dc and well suited// for further upsampling our heavily-oversampled prototype filter.//static void cubicInterpolation(const float* input, float* output, int inputSize, int outputSize, float gain) {     int64_t step = ((int64_t)inputSize << 32) / outputSize;     // Q32    int64_t offset = (outputSize < inputSize) ? (step/2) : 0;   // offset to improve small integer ratios     // Lagrange interpolation using Farrow structure    for (int j = 0; j < outputSize; j++) {         int32_t i = HI32(offset);        uint32_t f = LO32(offset);         // values outside the window are zero        float x0 = (i - 1 < 0) ? 0.0f : input[i - 1];        float x1 = (i - 0 < 0) ? 0.0f : input[i - 0];        float x2 = (i + 1 < inputSize) ? input[i + 1] : 0.0f;        float x3 = (i + 2 < inputSize) ? input[i + 2] : 0.0f;         // compute the polynomial coefficients        float c0 = (1/6.0f) * (x3 - x0) + (1/2.0f) * (x1 - x2);        float c1 = (1/2.0f) * (x0 + x2) - x1;        float c2 = x2 - (1/3.0f) * x0 - (1/2.0f) * x1 - (1/6.0f) * x3;        float c3 = x1;         // compute the polynomial        float frac = f * Q32_TO_FLOAT;        output[j] = (((c0 * frac + c1) * frac + c2) * frac + c3) * gain;         offset += step;    }} int AudioSRC::createRationalFilter(int upFactor, int downFactor, float gain, Quality quality) {     int prototypeTaps = prototypeFilterTable[quality].taps;    int prototypeCoefs = prototypeFilterTable[quality].coefs;    const float* prototypeFilter = prototypeFilterTable[quality].filter;     int numTaps = prototypeTaps;    int numPhases = upFactor;    int numCoefs = numTaps * numPhases;     //    // When downsampling, we can lower the filter cutoff by downFactor/upFactor using the    // time-scaling property of the Fourier transform.  The gain is adjusted accordingly.    //    if (downFactor > upFactor) {        int oldCoefs = numCoefs;        numCoefs = ((int64_t)oldCoefs * downFactor) / upFactor;        numTaps = (numCoefs + upFactor - 1) / upFactor;        gain *= (float)oldCoefs / numCoefs;    }    numTaps = (numTaps + 7) & ~7;   // SIMD8     // interpolate the coefficients of the prototype filter    float* tempFilter = new float[numTaps * numPhases];    memset(tempFilter, 0, numTaps * numPhases * sizeof(float));     cubicInterpolation(prototypeFilter, tempFilter, prototypeCoefs, numCoefs, gain);     // create the polyphase filter    _polyphaseFilter = (float*)aligned_malloc(numTaps * numPhases * sizeof(float), 32); // SIMD8     // rearrange into polyphase form, ordered by use    for (int i = 0; i < numPhases; i++) {        int phase = (i * downFactor) % upFactor;        for (int j = 0; j < numTaps; j++) {             // the filter taps are reversed, so convolution is implemented as dot-product            float f = tempFilter[(numTaps - j - 1) * numPhases + phase];            _polyphaseFilter[numTaps * i + j] = f;        }    }     delete[] tempFilter;     // precompute the input steps    _stepTable = new int[numPhases];     for (int i = 0; i < numPhases; i++) {        _stepTable[i] = (((int64_t)(i+1) * downFactor) / upFactor) - (((int64_t)(i+0) * downFactor) / upFactor);    }     return numTaps;} int AudioSRC::createIrrationalFilter(int upFactor, int downFactor, float gain, Quality quality) {     int prototypeTaps = prototypeFilterTable[quality].taps;    int prototypeCoefs = prototypeFilterTable[quality].coefs;    const float* prototypeFilter = prototypeFilterTable[quality].filter;     int numTaps = prototypeTaps;    int numPhases = upFactor;    int numCoefs = numTaps * numPhases;     //    // When downsampling, we can lower the filter cutoff by downFactor/upFactor using the    // time-scaling property of the Fourier transform.  The gain is adjusted accordingly.    //    if (downFactor > upFactor) {        int oldCoefs = numCoefs;        numCoefs = ((int64_t)oldCoefs * downFactor) / upFactor;        numTaps = (numCoefs + upFactor - 1) / upFactor;        gain *= (float)oldCoefs / numCoefs;    }    numTaps = (numTaps + 7) & ~7;   // SIMD8     // interpolate the coefficients of the prototype filter    float* tempFilter = new float[numTaps * numPhases];    memset(tempFilter, 0, numTaps * numPhases * sizeof(float));     cubicInterpolation(prototypeFilter, tempFilter, prototypeCoefs, numCoefs, gain);     // create the polyphase filter, with extra phase at the end to simplify coef interpolation    _polyphaseFilter = (float*)aligned_malloc(numTaps * (numPhases + 1) * sizeof(float), 32);   // SIMD8     // rearrange into polyphase form, ordered by fractional delay    for (int phase = 0; phase < numPhases; phase++) {        for (int j = 0; j < numTaps; j++) {             // the filter taps are reversed, so convolution is implemented as dot-product            float f = tempFilter[(numTaps - j - 1) * numPhases + phase];            _polyphaseFilter[numTaps * phase + j] = f;        }    }     delete[] tempFilter;     // by construction, the last tap of the first phase must be zero    assert(_polyphaseFilter[numTaps - 1] == 0.0f);     // so the extra phase is just the first, shifted by one    _polyphaseFilter[numTaps * numPhases + 0] = 0.0f;    for (int j = 1; j < numTaps; j++) {        _polyphaseFilter[numTaps * numPhases + j] = _polyphaseFilter[j-1];    }     return numTaps;} //// scalar reference versions//int AudioSRC::multirateFilter1_ref(const float* input0, float* output0, int inputFrames) {    int outputFrames = 0;     if (_step == 0) {   // rational         int32_t i = HI32(_offset);         while (i < inputFrames) {             const float* c0 = &_polyphaseFilter[_numTaps * _phase];             float acc0 = 0.0f;             for (int j = 0; j < _numTaps; j++) {                 float coef = c0[j];                 acc0 += input0[i + j] * coef;            }             output0[outputFrames] = acc0;            outputFrames += 1;             i += _stepTable[_phase];            if (++_phase == _upFactor) {                _phase = 0;            }        }        _offset = (int64_t)(i - inputFrames) << 32;     } else {    // irrational         while (HI32(_offset) < inputFrames) {             int32_t i = HI32(_offset);            uint32_t f = LO32(_offset);             uint32_t phase = f >> SRC_FRACBITS;            float frac = (f & SRC_FRACMASK) * QFRAC_TO_FLOAT;             const float* c0 = &_polyphaseFilter[_numTaps * (phase + 0)];            const float* c1 = &_polyphaseFilter[_numTaps * (phase + 1)];             float acc0 = 0.0f;             for (int j = 0; j < _numTaps; j++) {                 float coef = c0[j] + frac * (c1[j] - c0[j]);                 acc0 += input0[i + j] * coef;            }             output0[outputFrames] = acc0;            outputFrames += 1;             _offset += _step;        }        _offset -= (int64_t)inputFrames << 32;    }     return outputFrames;} int AudioSRC::multirateFilter2_ref(const float* input0, const float* input1, float* output0, float* output1, int inputFrames) {    int outputFrames = 0;     if (_step == 0) {   // rational         int32_t i = HI32(_offset);         while (i < inputFrames) {             const float* c0 = &_polyphaseFilter[_numTaps * _phase];             float acc0 = 0.0f;            float acc1 = 0.0f;             for (int j = 0; j < _numTaps; j++) {                 float coef = c0[j];                 acc0 += input0[i + j] * coef;                acc1 += input1[i + j] * coef;            }             output0[outputFrames] = acc0;            output1[outputFrames] = acc1;            outputFrames += 1;             i += _stepTable[_phase];            if (++_phase == _upFactor) {                _phase = 0;            }        }        _offset = (int64_t)(i - inputFrames) << 32;     } else {    // irrational         while (HI32(_offset) < inputFrames) {             int32_t i = HI32(_offset);            uint32_t f = LO32(_offset);             uint32_t phase = f >> SRC_FRACBITS;            float frac = (f & SRC_FRACMASK) * QFRAC_TO_FLOAT;             const float* c0 = &_polyphaseFilter[_numTaps * (phase + 0)];            const float* c1 = &_polyphaseFilter[_numTaps * (phase + 1)];             float acc0 = 0.0f;            float acc1 = 0.0f;             for (int j = 0; j < _numTaps; j++) {                 float coef = c0[j] + frac * (c1[j] - c0[j]);                 acc0 += input0[i + j] * coef;                acc1 += input1[i + j] * coef;            }             output0[outputFrames] = acc0;            output1[outputFrames] = acc1;            outputFrames += 1;             _offset += _step;        }        _offset -= (int64_t)inputFrames << 32;    }     return outputFrames;} int AudioSRC::multirateFilter4_ref(const float* input0, const float* input1, const float* input2, const float* input3,                                    float* output0, float* output1, float* output2, float* output3, int inputFrames) {    int outputFrames = 0;     if (_step == 0) {   // rational         int32_t i = HI32(_offset);         while (i < inputFrames) {             const float* c0 = &_polyphaseFilter[_numTaps * _phase];             float acc0 = 0.0f;            float acc1 = 0.0f;            float acc2 = 0.0f;            float acc3 = 0.0f;             for (int j = 0; j < _numTaps; j++) {                 float coef = c0[j];                 acc0 += input0[i + j] * coef;                acc1 += input1[i + j] * coef;                acc2 += input2[i + j] * coef;                acc3 += input3[i + j] * coef;            }             output0[outputFrames] = acc0;            output1[outputFrames] = acc1;            output2[outputFrames] = acc2;            output3[outputFrames] = acc3;            outputFrames += 1;             i += _stepTable[_phase];            if (++_phase == _upFactor) {                _phase = 0;            }        }        _offset = (int64_t)(i - inputFrames) << 32;     } else {    // irrational         while (HI32(_offset) < inputFrames) {             int32_t i = HI32(_offset);            uint32_t f = LO32(_offset);             uint32_t phase = f >> SRC_FRACBITS;            float frac = (f & SRC_FRACMASK) * QFRAC_TO_FLOAT;             const float* c0 = &_polyphaseFilter[_numTaps * (phase + 0)];            const float* c1 = &_polyphaseFilter[_numTaps * (phase + 1)];             float acc0 = 0.0f;            float acc1 = 0.0f;            float acc2 = 0.0f;            float acc3 = 0.0f;             for (int j = 0; j < _numTaps; j++) {                 float coef = c0[j] + frac * (c1[j] - c0[j]);                 acc0 += input0[i + j] * coef;                acc1 += input1[i + j] * coef;                acc2 += input2[i + j] * coef;                acc3 += input3[i + j] * coef;            }             output0[outputFrames] = acc0;            output1[outputFrames] = acc1;            output2[outputFrames] = acc2;            output3[outputFrames] = acc3;            outputFrames += 1;             _offset += _step;        }        _offset -= (int64_t)inputFrames << 32;    }     return outputFrames;} #if defined(_M_IX86) || defined(_M_X64) || defined(__i386__) || defined(__x86_64__) //// Runtime CPU dispatch// #include "CPUDetect.h" int AudioSRC::multirateFilter1(const float* input0, float* output0, int inputFrames) {    static auto f = cpuSupportsAVX2() ? &AudioSRC::multirateFilter1_AVX2 : &AudioSRC::multirateFilter1_ref;    return (this->*f)(input0, output0, inputFrames);    // dispatch} int AudioSRC::multirateFilter2(const float* input0, const float* input1, float* output0, float* output1, int inputFrames) {    static auto f = cpuSupportsAVX2() ? &AudioSRC::multirateFilter2_AVX2 : &AudioSRC::multirateFilter2_ref;    return (this->*f)(input0, input1, output0, output1, inputFrames);   // dispatch} int AudioSRC::multirateFilter4(const float* input0, const float* input1, const float* input2, const float* input3,                                float* output0, float* output1, float* output2, float* output3, int inputFrames) {    static auto f = cpuSupportsAVX2() ? &AudioSRC::multirateFilter4_AVX2 : &AudioSRC::multirateFilter4_ref;    return (this->*f)(input0, input1, input2, input3, output0, output1, output2, output3, inputFrames); // dispatch} #elif defined(__ARM_NEON__) || defined(__ARM_NEON) #include <arm_neon.h> int AudioSRC::multirateFilter1(const float* input0, float* output0, int inputFrames) {    int outputFrames = 0;     assert(_numTaps % 8 == 0);  // SIMD8     if (_step == 0) {   // rational         int32_t i = HI32(_offset);         while (i < inputFrames) {             const float* c0 = &_polyphaseFilter[_numTaps * _phase];             float32x4_t acc0 = vdupq_n_f32(0);            float32x4_t acc1 = vdupq_n_f32(0);                       for (int j = 0; j < _numTaps; j += 8) {                 //float coef = c0[j];                float32x4_t coef0 = vld1q_f32(&c0[j + 0]);  // aligned                float32x4_t coef1 = vld1q_f32(&c0[j + 4]);  // aligned                 //acc += input[i + j] * coef;                acc0 = vmlaq_f32(acc0, vld1q_f32(&input0[i + j + 0]), coef0);                acc1 = vmlaq_f32(acc1, vld1q_f32(&input0[i + j + 4]), coef1);            }            acc0 = vaddq_f32(acc0, acc1);             // horizontal sum            float32x2_t t0 = vadd_f32(vget_low_f32(acc0), vget_high_f32(acc0));            t0 = vpadd_f32(t0, t0);             vst1_lane_f32(&output0[outputFrames], t0, 0);            outputFrames += 1;             i += _stepTable[_phase];            if (++_phase == _upFactor) {                _phase = 0;            }        }        _offset = (int64_t)(i - inputFrames) << 32;     } else {    // irrational 

Lines 1–500 of 1585. Load more