mirror of
https://github.com/overte-org/overte.git
synced 2025-07-23 04:25:49 +02:00
922 lines
28 KiB
C++
922 lines
28 KiB
C++
//
|
|
// 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 <assert.h>
|
|
#include <string.h>
|
|
#include <algorithm>
|
|
|
|
#include "AudioSRC.h"
|
|
#include "AudioSRCData.h"
|
|
|
|
// 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 divisor
|
|
static 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) {
|
|
int numTaps = PROTOTYPE_TAPS;
|
|
int numPhases = upFactor;
|
|
int numCoefs = numTaps * numPhases;
|
|
int oldCoefs = numCoefs;
|
|
int prototypeCoefs = PROTOTYPE_TAPS * PROTOTYPE_PHASES;
|
|
|
|
//
|
|
// 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) {
|
|
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) {
|
|
int numTaps = PROTOTYPE_TAPS;
|
|
int numPhases = upFactor;
|
|
int numCoefs = numTaps * numPhases;
|
|
int oldCoefs = numCoefs;
|
|
int prototypeCoefs = PROTOTYPE_TAPS * PROTOTYPE_PHASES;
|
|
|
|
//
|
|
// 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) {
|
|
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;
|
|
}
|
|
|
|
//
|
|
// on x86 architecture, assume that SSE2 is present
|
|
//
|
|
#if defined(_M_IX86) || defined(_M_X64) || defined(__i386__) || defined(__x86_64__)
|
|
|
|
#include <emmintrin.h>
|
|
|
|
int AudioSRC::multirateFilter1_SSE(const float* input0, float* output0, int inputFrames) {
|
|
int outputFrames = 0;
|
|
|
|
assert(_numTaps % 4 == 0); // SIMD4
|
|
|
|
if (_step == 0) { // rational
|
|
|
|
int32_t i = HI32(_offset);
|
|
|
|
while (i < inputFrames) {
|
|
|
|
const float* c0 = &_polyphaseFilter[_numTaps * _phase];
|
|
|
|
__m128 acc0 = _mm_setzero_ps();
|
|
|
|
for (int j = 0; j < _numTaps; j += 4) {
|
|
|
|
//float coef = c0[j];
|
|
__m128 coef0 = _mm_loadu_ps(&c0[j]);
|
|
|
|
//acc += input[i + j] * coef;
|
|
acc0 = _mm_add_ps(_mm_mul_ps(_mm_loadu_ps(&input0[i + j]), coef0), acc0);
|
|
}
|
|
|
|
// horizontal sum
|
|
acc0 = _mm_add_ps(acc0, _mm_movehl_ps(acc0, acc0));
|
|
acc0 = _mm_add_ss(acc0, _mm_shuffle_ps(acc0, acc0, _MM_SHUFFLE(0,0,0,1)));
|
|
|
|
_mm_store_ss(&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;
|
|
__m128 frac = _mm_set1_ps((f & SRC_FRACMASK) * QFRAC_TO_FLOAT);
|
|
|
|
const float* c0 = &_polyphaseFilter[_numTaps * (phase + 0)];
|
|
const float* c1 = &_polyphaseFilter[_numTaps * (phase + 1)];
|
|
|
|
__m128 acc0 = _mm_setzero_ps();
|
|
|
|
for (int j = 0; j < _numTaps; j += 4) {
|
|
|
|
//float coef = c0[j] + frac * (c1[j] - c0[j]);
|
|
__m128 coef0 = _mm_loadu_ps(&c0[j]);
|
|
__m128 coef1 = _mm_loadu_ps(&c1[j]);
|
|
coef1 = _mm_sub_ps(coef1, coef0);
|
|
coef0 = _mm_add_ps(_mm_mul_ps(coef1, frac), coef0);
|
|
|
|
//acc += input[i + j] * coef;
|
|
acc0 = _mm_add_ps(_mm_mul_ps(_mm_loadu_ps(&input0[i + j]), coef0), acc0);
|
|
}
|
|
|
|
// horizontal sum
|
|
acc0 = _mm_add_ps(acc0, _mm_movehl_ps(acc0, acc0));
|
|
acc0 = _mm_add_ss(acc0, _mm_shuffle_ps(acc0, acc0, _MM_SHUFFLE(0,0,0,1)));
|
|
|
|
_mm_store_ss(&output0[outputFrames], acc0);
|
|
outputFrames += 1;
|
|
|
|
_offset += _step;
|
|
}
|
|
_offset -= (int64_t)inputFrames << 32;
|
|
}
|
|
|
|
return outputFrames;
|
|
}
|
|
|
|
int AudioSRC::multirateFilter2_SSE(const float* input0, const float* input1, float* output0, float* output1, int inputFrames) {
|
|
int outputFrames = 0;
|
|
|
|
assert(_numTaps % 4 == 0); // SIMD4
|
|
|
|
if (_step == 0) { // rational
|
|
|
|
int32_t i = HI32(_offset);
|
|
|
|
while (i < inputFrames) {
|
|
|
|
const float* c0 = &_polyphaseFilter[_numTaps * _phase];
|
|
|
|
__m128 acc0 = _mm_setzero_ps();
|
|
__m128 acc1 = _mm_setzero_ps();
|
|
|
|
for (int j = 0; j < _numTaps; j += 4) {
|
|
|
|
//float coef = c0[j];
|
|
__m128 coef0 = _mm_loadu_ps(&c0[j]);
|
|
|
|
//acc += input[i + j] * coef;
|
|
acc0 = _mm_add_ps(_mm_mul_ps(_mm_loadu_ps(&input0[i + j]), coef0), acc0);
|
|
acc1 = _mm_add_ps(_mm_mul_ps(_mm_loadu_ps(&input1[i + j]), coef0), acc1);
|
|
}
|
|
|
|
// horizontal sum
|
|
acc0 = _mm_add_ps(acc0, _mm_movehl_ps(acc0, acc0));
|
|
acc1 = _mm_add_ps(acc1, _mm_movehl_ps(acc1, acc1));
|
|
acc0 = _mm_add_ss(acc0, _mm_shuffle_ps(acc0, acc0, _MM_SHUFFLE(0,0,0,1)));
|
|
acc1 = _mm_add_ss(acc1, _mm_shuffle_ps(acc1, acc1, _MM_SHUFFLE(0,0,0,1)));
|
|
|
|
_mm_store_ss(&output0[outputFrames], acc0);
|
|
_mm_store_ss(&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;
|
|
__m128 frac = _mm_set1_ps((f & SRC_FRACMASK) * QFRAC_TO_FLOAT);
|
|
|
|
const float* c0 = &_polyphaseFilter[_numTaps * (phase + 0)];
|
|
const float* c1 = &_polyphaseFilter[_numTaps * (phase + 1)];
|
|
|
|
__m128 acc0 = _mm_setzero_ps();
|
|
__m128 acc1 = _mm_setzero_ps();
|
|
|
|
for (int j = 0; j < _numTaps; j += 4) {
|
|
|
|
//float coef = c0[j] + frac * (c1[j] - c0[j]);
|
|
__m128 coef0 = _mm_loadu_ps(&c0[j]);
|
|
__m128 coef1 = _mm_loadu_ps(&c1[j]);
|
|
coef1 = _mm_sub_ps(coef1, coef0);
|
|
coef0 = _mm_add_ps(_mm_mul_ps(coef1, frac), coef0);
|
|
|
|
//acc += input[i + j] * coef;
|
|
acc0 = _mm_add_ps(_mm_mul_ps(_mm_loadu_ps(&input0[i + j]), coef0), acc0);
|
|
acc1 = _mm_add_ps(_mm_mul_ps(_mm_loadu_ps(&input1[i + j]), coef0), acc1);
|
|
}
|
|
|
|
// horizontal sum
|
|
acc0 = _mm_add_ps(acc0, _mm_movehl_ps(acc0, acc0));
|
|
acc1 = _mm_add_ps(acc1, _mm_movehl_ps(acc1, acc1));
|
|
acc0 = _mm_add_ss(acc0, _mm_shuffle_ps(acc0, acc0, _MM_SHUFFLE(0,0,0,1)));
|
|
acc1 = _mm_add_ss(acc1, _mm_shuffle_ps(acc1, acc1, _MM_SHUFFLE(0,0,0,1)));
|
|
|
|
_mm_store_ss(&output0[outputFrames], acc0);
|
|
_mm_store_ss(&output1[outputFrames], acc1);
|
|
outputFrames += 1;
|
|
|
|
_offset += _step;
|
|
}
|
|
_offset -= (int64_t)inputFrames << 32;
|
|
}
|
|
|
|
return outputFrames;
|
|
}
|
|
|
|
//
|
|
// 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_SSE;
|
|
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_SSE;
|
|
return (this->*f)(input0, input1, output0, output1, inputFrames); // dispatch
|
|
}
|
|
|
|
// convert int16_t to float, deinterleave stereo
|
|
void AudioSRC::convertInputFromInt16(const int16_t* input, float** outputs, int numFrames) {
|
|
__m128 scale = _mm_set1_ps(1/32768.0f);
|
|
|
|
if (_numChannels == 1) {
|
|
|
|
int i = 0;
|
|
for (; i < numFrames - 3; i += 4) {
|
|
__m128i a0 = _mm_loadl_epi64((__m128i*)&input[i]);
|
|
|
|
// sign-extend
|
|
a0 = _mm_srai_epi32(_mm_unpacklo_epi16(a0, a0), 16);
|
|
|
|
__m128 f0 = _mm_mul_ps(_mm_cvtepi32_ps(a0), scale);
|
|
|
|
_mm_storeu_ps(&outputs[0][i], f0);
|
|
}
|
|
for (; i < numFrames; i++) {
|
|
__m128i a0 = _mm_insert_epi16(_mm_setzero_si128(), input[i], 0);
|
|
|
|
// sign-extend
|
|
a0 = _mm_srai_epi32(_mm_unpacklo_epi16(a0, a0), 16);
|
|
|
|
__m128 f0 = _mm_mul_ps(_mm_cvtepi32_ps(a0), scale);
|
|
|
|
_mm_store_ss(&outputs[0][i], f0);
|
|
}
|
|
|
|
} else if (_numChannels == 2) {
|
|
|
|
int i = 0;
|
|
for (; i < numFrames - 3; i += 4) {
|
|
__m128i a0 = _mm_loadu_si128((__m128i*)&input[2*i]);
|
|
__m128i a1 = a0;
|
|
|
|
// deinterleave and sign-extend
|
|
a0 = _mm_madd_epi16(a0, _mm_set1_epi32(0x00000001));
|
|
a1 = _mm_madd_epi16(a1, _mm_set1_epi32(0x00010000));
|
|
|
|
__m128 f0 = _mm_mul_ps(_mm_cvtepi32_ps(a0), scale);
|
|
__m128 f1 = _mm_mul_ps(_mm_cvtepi32_ps(a1), scale);
|
|
|
|
_mm_storeu_ps(&outputs[0][i], f0);
|
|
_mm_storeu_ps(&outputs[1][i], f1);
|
|
}
|
|
for (; i < numFrames; i++) {
|
|
__m128i a0 = _mm_cvtsi32_si128(*(int32_t*)&input[2*i]);
|
|
__m128i a1 = a0;
|
|
|
|
// deinterleave and sign-extend
|
|
a0 = _mm_madd_epi16(a0, _mm_set1_epi32(0x00000001));
|
|
a1 = _mm_madd_epi16(a1, _mm_set1_epi32(0x00010000));
|
|
|
|
__m128 f0 = _mm_mul_ps(_mm_cvtepi32_ps(a0), scale);
|
|
__m128 f1 = _mm_mul_ps(_mm_cvtepi32_ps(a1), scale);
|
|
|
|
_mm_store_ss(&outputs[0][i], f0);
|
|
_mm_store_ss(&outputs[1][i], f1);
|
|
}
|
|
}
|
|
}
|
|
|
|
// fast TPDF dither in [-1.0f, 1.0f]
|
|
static inline __m128 dither4() {
|
|
static __m128i rz;
|
|
|
|
// update the 8 different maximum-length LCGs
|
|
rz = _mm_mullo_epi16(rz, _mm_set_epi16(25173, -25511, -5975, -23279, 19445, -27591, 30185, -3495));
|
|
rz = _mm_add_epi16(rz, _mm_set_epi16(13849, -32767, 105, -19675, -7701, -32679, -13225, 28013));
|
|
|
|
// promote to 32-bit
|
|
__m128i r0 = _mm_unpacklo_epi16(rz, _mm_setzero_si128());
|
|
__m128i r1 = _mm_unpackhi_epi16(rz, _mm_setzero_si128());
|
|
|
|
// return (r0 - r1) * (1/65536.0f);
|
|
__m128 d0 = _mm_cvtepi32_ps(_mm_sub_epi32(r0, r1));
|
|
return _mm_mul_ps(d0, _mm_set1_ps(1/65536.0f));
|
|
}
|
|
|
|
// convert float to int16_t, interleave stereo
|
|
void AudioSRC::convertOutputToInt16(float** inputs, int16_t* output, int numFrames) {
|
|
__m128 scale = _mm_set1_ps(32768.0f);
|
|
|
|
if (_numChannels == 1) {
|
|
|
|
int i = 0;
|
|
for (; i < numFrames - 3; i += 4) {
|
|
__m128 f0 = _mm_mul_ps(_mm_loadu_ps(&inputs[0][i]), scale);
|
|
|
|
f0 = _mm_add_ps(f0, dither4());
|
|
|
|
// round and saturate
|
|
__m128i a0 = _mm_cvtps_epi32(f0);
|
|
a0 = _mm_packs_epi32(a0, a0);
|
|
|
|
_mm_storel_epi64((__m128i*)&output[i], a0);
|
|
}
|
|
for (; i < numFrames; i++) {
|
|
__m128 f0 = _mm_mul_ps(_mm_load_ss(&inputs[0][i]), scale);
|
|
|
|
f0 = _mm_add_ps(f0, dither4());
|
|
|
|
// round and saturate
|
|
__m128i a0 = _mm_cvtps_epi32(f0);
|
|
a0 = _mm_packs_epi32(a0, a0);
|
|
|
|
output[i] = (int16_t)_mm_extract_epi16(a0, 0);
|
|
}
|
|
|
|
} else if (_numChannels == 2) {
|
|
|
|
int i = 0;
|
|
for (; i < numFrames - 3; i += 4) {
|
|
__m128 f0 = _mm_mul_ps(_mm_loadu_ps(&inputs[0][i]), scale);
|
|
__m128 f1 = _mm_mul_ps(_mm_loadu_ps(&inputs[1][i]), scale);
|
|
|
|
__m128 d0 = dither4();
|
|
f0 = _mm_add_ps(f0, d0);
|
|
f1 = _mm_add_ps(f1, d0);
|
|
|
|
// round and saturate
|
|
__m128i a0 = _mm_cvtps_epi32(f0);
|
|
__m128i a1 = _mm_cvtps_epi32(f1);
|
|
a0 = _mm_packs_epi32(a0, a0);
|
|
a1 = _mm_packs_epi32(a1, a1);
|
|
|
|
// interleave
|
|
a0 = _mm_unpacklo_epi16(a0, a1);
|
|
_mm_storeu_si128((__m128i*)&output[2*i], a0);
|
|
}
|
|
for (; i < numFrames; i++) {
|
|
__m128 f0 = _mm_mul_ps(_mm_load_ss(&inputs[0][i]), scale);
|
|
__m128 f1 = _mm_mul_ps(_mm_load_ss(&inputs[1][i]), scale);
|
|
|
|
__m128 d0 = dither4();
|
|
f0 = _mm_add_ps(f0, d0);
|
|
f1 = _mm_add_ps(f1, d0);
|
|
|
|
// round and saturate
|
|
__m128i a0 = _mm_cvtps_epi32(f0);
|
|
__m128i a1 = _mm_cvtps_epi32(f1);
|
|
a0 = _mm_packs_epi32(a0, a0);
|
|
a1 = _mm_packs_epi32(a1, a1);
|
|
|
|
// interleave
|
|
a0 = _mm_unpacklo_epi16(a0, a1);
|
|
*(int32_t*)&output[2*i] = _mm_cvtsi128_si32(a0);
|
|
}
|
|
}
|
|
}
|
|
|
|
#else
|
|
|
|
int AudioSRC::multirateFilter1(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(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;
|
|
}
|
|
|
|
// convert int16_t to float, deinterleave stereo
|
|
void AudioSRC::convertInputFromInt16(const int16_t* input, float** outputs, int numFrames) {
|
|
const float scale = 1/32768.0f;
|
|
|
|
if (_numChannels == 1) {
|
|
for (int i = 0; i < numFrames; i++) {
|
|
outputs[0][i] = (float)input[i] * scale;
|
|
}
|
|
} else if (_numChannels == 2) {
|
|
for (int i = 0; i < numFrames; i++) {
|
|
outputs[0][i] = (float)input[2*i + 0] * scale;
|
|
outputs[1][i] = (float)input[2*i + 1] * scale;
|
|
}
|
|
}
|
|
}
|
|
|
|
// fast TPDF dither in [-1.0f, 1.0f]
|
|
static inline float dither() {
|
|
static uint32_t rz = 0;
|
|
rz = rz * 69069 + 1;
|
|
int32_t r0 = rz & 0xffff;
|
|
int32_t r1 = rz >> 16;
|
|
return (int32_t)(r0 - r1) * (1/65536.0f);
|
|
}
|
|
|
|
// convert float to int16_t, interleave stereo
|
|
void AudioSRC::convertOutputToInt16(float** inputs, int16_t* output, int numFrames) {
|
|
const float scale = 32768.0f;
|
|
|
|
if (_numChannels == 1) {
|
|
for (int i = 0; i < numFrames; i++) {
|
|
|
|
float f = inputs[0][i] * scale;
|
|
|
|
f += dither();
|
|
|
|
// round and saturate
|
|
f += (f < 0.0f ? -0.5f : +0.5f);
|
|
f = std::max(std::min(f, 32767.0f), -32768.0f);
|
|
|
|
output[i] = (int16_t)f;
|
|
}
|
|
} else if (_numChannels == 2) {
|
|
for (int i = 0; i < numFrames; i++) {
|
|
|
|
float f0 = inputs[0][i] * scale;
|
|
float f1 = inputs[1][i] * scale;
|
|
|
|
float d = dither();
|
|
f0 += d;
|
|
f1 += d;
|
|
|
|
// round and saturate
|
|
f0 += (f0 < 0.0f ? -0.5f : +0.5f);
|
|
f1 += (f1 < 0.0f ? -0.5f : +0.5f);
|
|
f0 = std::max(std::min(f0, 32767.0f), -32768.0f);
|
|
f1 = std::max(std::min(f1, 32767.0f), -32768.0f);
|
|
|
|
// interleave
|
|
output[2*i + 0] = (int16_t)f0;
|
|
output[2*i + 1] = (int16_t)f1;
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
int AudioSRC::processFloat(float** inputs, float** outputs, int inputFrames) {
|
|
int outputFrames = 0;
|
|
|
|
int nh = std::min(_numHistory, inputFrames); // number of frames from history buffer
|
|
int ni = inputFrames - nh; // number of frames from remaining input
|
|
|
|
if (_numChannels == 1) {
|
|
|
|
// refill history buffers
|
|
memcpy(_history[0] + _numHistory, _inputs[0], nh * sizeof(float));
|
|
|
|
// process history buffer
|
|
outputFrames += multirateFilter1(_history[0], _outputs[0], nh);
|
|
|
|
// process remaining input
|
|
if (ni) {
|
|
outputFrames += multirateFilter1(_inputs[0], _outputs[0] + outputFrames, ni);
|
|
}
|
|
|
|
// shift history buffers
|
|
if (ni) {
|
|
memcpy(_history[0], _inputs[0] + ni, _numHistory * sizeof(float));
|
|
} else {
|
|
memmove(_history[0], _history[0] + nh, _numHistory * sizeof(float));
|
|
}
|
|
|
|
} else if (_numChannels == 2) {
|
|
|
|
// refill history buffers
|
|
memcpy(_history[0] + _numHistory, _inputs[0], nh * sizeof(float));
|
|
memcpy(_history[1] + _numHistory, _inputs[1], nh * sizeof(float));
|
|
|
|
// process history buffer
|
|
outputFrames += multirateFilter2(_history[0], _history[1], _outputs[0], _outputs[1], nh);
|
|
|
|
// process remaining input
|
|
if (ni) {
|
|
outputFrames += multirateFilter2(_inputs[0], _inputs[1], _outputs[0] + outputFrames, _outputs[1] + outputFrames, ni);
|
|
}
|
|
|
|
// shift history buffers
|
|
if (ni) {
|
|
memcpy(_history[0], inputs[0] + ni, _numHistory * sizeof(float));
|
|
memcpy(_history[1], inputs[1] + ni, _numHistory * sizeof(float));
|
|
} else {
|
|
memmove(_history[0], _history[0] + nh, _numHistory * sizeof(float));
|
|
memmove(_history[1], _history[1] + nh, _numHistory * sizeof(float));
|
|
}
|
|
}
|
|
|
|
return outputFrames;
|
|
}
|
|
|
|
AudioSRC::AudioSRC(int inputSampleRate, int outputSampleRate, int numChannels) {
|
|
assert(inputSampleRate > 0);
|
|
assert(outputSampleRate > 0);
|
|
assert(numChannels > 0);
|
|
assert(numChannels <= SRC_MAX_CHANNELS);
|
|
|
|
_inputSampleRate = inputSampleRate;
|
|
_outputSampleRate = outputSampleRate;
|
|
_numChannels = numChannels;
|
|
|
|
// reduce to the smallest rational fraction
|
|
int divisor = gcd(inputSampleRate, outputSampleRate);
|
|
_upFactor = outputSampleRate / divisor;
|
|
_downFactor = inputSampleRate / divisor;
|
|
_step = 0; // rational mode
|
|
|
|
// if the number of phases is too large, use irrational mode
|
|
if (_upFactor > 640) {
|
|
_upFactor = SRC_PHASES;
|
|
_downFactor = ((int64_t)SRC_PHASES * _inputSampleRate) / _outputSampleRate;
|
|
_step = ((int64_t)_inputSampleRate << 32) / _outputSampleRate;
|
|
}
|
|
|
|
_polyphaseFilter = nullptr;
|
|
_stepTable = nullptr;
|
|
|
|
// create the polyphase filter
|
|
if (_step == 0) {
|
|
_numTaps = createRationalFilter(_upFactor, _downFactor, 1.0f);
|
|
} else {
|
|
_numTaps = createIrrationalFilter(_upFactor, _downFactor, 1.0f);
|
|
}
|
|
|
|
//printf("up=%d down=%.3f taps=%d\n", _upFactor, _downFactor + (LO32(_step)<<SRC_PHASEBITS) * Q32_TO_FLOAT, _numTaps);
|
|
|
|
// filter history buffers
|
|
_numHistory = _numTaps - 1;
|
|
_history[0] = new float[2 * _numHistory];
|
|
_history[1] = new float[2 * _numHistory];
|
|
|
|
// format conversion buffers
|
|
_inputs[0] = (float*)aligned_malloc(4*SRC_BLOCK * sizeof(float), 16); // SIMD4
|
|
_inputs[1] = _inputs[0] + 1*SRC_BLOCK;
|
|
_outputs[0] = _inputs[0] + 2*SRC_BLOCK;
|
|
_outputs[1] = _inputs[0] + 3*SRC_BLOCK;
|
|
|
|
// input blocking size, such that input and output are both guaranteed not to exceed SRC_BLOCK frames
|
|
_inputBlock = std::min(SRC_BLOCK, getMaxInput(SRC_BLOCK));
|
|
|
|
// reset the state
|
|
_offset = 0;
|
|
_phase = 0;
|
|
memset(_history[0], 0, 2 * _numHistory * sizeof(float));
|
|
memset(_history[1], 0, 2 * _numHistory * sizeof(float));
|
|
}
|
|
|
|
AudioSRC::~AudioSRC() {
|
|
aligned_free(_polyphaseFilter);
|
|
delete[] _stepTable;
|
|
|
|
delete[] _history[0];
|
|
delete[] _history[1];
|
|
|
|
aligned_free(_inputs[0]);
|
|
}
|
|
|
|
//
|
|
// This version handles input/output as interleaved int16_t
|
|
//
|
|
int AudioSRC::render(const int16_t* input, int16_t* output, int inputFrames) {
|
|
int outputFrames = 0;
|
|
|
|
while (inputFrames) {
|
|
|
|
int ni = std::min(inputFrames, _inputBlock);
|
|
|
|
convertInputFromInt16(input, _inputs, ni);
|
|
|
|
int no = processFloat(_inputs, _outputs, ni);
|
|
assert(no <= SRC_BLOCK);
|
|
|
|
convertOutputToInt16(_outputs, output, no);
|
|
|
|
input += _numChannels * ni;
|
|
output += _numChannels * no;
|
|
inputFrames -= ni;
|
|
outputFrames += no;
|
|
}
|
|
|
|
return outputFrames;
|
|
}
|
|
|
|
// the min output frames that will be produced by inputFrames
|
|
int AudioSRC::getMinOutput(int inputFrames) {
|
|
if (_step == 0) {
|
|
return (int)(((int64_t)inputFrames * _upFactor) / _downFactor);
|
|
} else {
|
|
return (int)(((int64_t)inputFrames << 32) / _step);
|
|
}
|
|
}
|
|
|
|
// the max output frames that will be produced by inputFrames
|
|
int AudioSRC::getMaxOutput(int inputFrames) {
|
|
if (_step == 0) {
|
|
return (int)(((int64_t)inputFrames * _upFactor + _downFactor-1) / _downFactor);
|
|
} else {
|
|
return (int)((((int64_t)inputFrames << 32) + _step-1) / _step);
|
|
}
|
|
}
|
|
|
|
// the min input frames that will produce at least outputFrames
|
|
int AudioSRC::getMinInput(int outputFrames) {
|
|
if (_step == 0) {
|
|
return (int)(((int64_t)outputFrames * _downFactor + _upFactor-1) / _upFactor);
|
|
} else {
|
|
return (int)(((int64_t)outputFrames * _step + 0xffffffffu) >> 32);
|
|
}
|
|
}
|
|
|
|
// the max input frames that will produce at most outputFrames
|
|
int AudioSRC::getMaxInput(int outputFrames) {
|
|
if (_step == 0) {
|
|
return (int)(((int64_t)outputFrames * _downFactor) / _upFactor);
|
|
} else {
|
|
return (int)(((int64_t)outputFrames * _step) >> 32);
|
|
}
|
|
}
|