dolphin/Externals/soundtouch/FIRFilter.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

315 lines
9.2 KiB
C++
Raw Normal View History

2013-06-22 18:19:27 +00:00
////////////////////////////////////////////////////////////////////////////////
///
/// General FIR digital filter routines with MMX optimization.
///
/// Notes : MMX optimized functions reside in a separate, platform-specific file,
2013-06-22 18:19:27 +00:00
/// e.g. 'mmx_win.cpp' or 'mmx_gcc.cpp'
///
/// This source file contains OpenMP optimizations that allow speeding up the
/// corss-correlation algorithm by executing it in several threads / CPU cores
/// in parallel. See the following article link for more detailed discussion
/// about SoundTouch OpenMP optimizations:
/// http://www.softwarecoven.com/parallel-computing-in-embedded-mobile-devices
///
2013-06-22 18:19:27 +00:00
/// Author : Copyright (c) Olli Parviainen
/// Author e-mail : oparviai 'at' iki.fi
/// SoundTouch WWW: http://www.surina.net/soundtouch
///
////////////////////////////////////////////////////////////////////////////////
//
// License :
//
// SoundTouch audio processing library
// Copyright (c) Olli Parviainen
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
//
////////////////////////////////////////////////////////////////////////////////
#include <memory.h>
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include "FIRFilter.h"
#include "cpu_detect.h"
using namespace soundtouch;
/*****************************************************************************
*
* Implementation of the class 'FIRFilter'
*
*****************************************************************************/
FIRFilter::FIRFilter()
{
resultDivFactor = 0;
resultDivider = 0;
length = 0;
lengthDiv8 = 0;
filterCoeffs = nullptr;
filterCoeffsStereo = nullptr;
2013-06-22 18:19:27 +00:00
}
FIRFilter::~FIRFilter()
{
delete[] filterCoeffs;
delete[] filterCoeffsStereo;
2013-06-22 18:19:27 +00:00
}
2013-06-22 18:19:27 +00:00
// Usual C-version of the filter routine for stereo sound
uint FIRFilter::evaluateFilterStereo(SAMPLETYPE *dest, const SAMPLETYPE *src, uint numSamples) const
{
2015-12-28 12:07:53 +00:00
int j, end;
// hint compiler autovectorization that loop length is divisible by 8
uint ilength = length & -8;
2013-06-22 18:19:27 +00:00
assert((length != 0) && (length == ilength) && (src != nullptr) && (dest != nullptr) && (filterCoeffs != nullptr));
assert(numSamples > ilength);
2013-06-22 18:19:27 +00:00
end = 2 * (numSamples - ilength);
2013-06-22 18:19:27 +00:00
2015-12-28 12:07:53 +00:00
#pragma omp parallel for
2013-06-22 18:19:27 +00:00
for (j = 0; j < end; j += 2)
{
const SAMPLETYPE *ptr;
2015-12-28 12:07:53 +00:00
LONG_SAMPLETYPE suml, sumr;
2013-06-22 18:19:27 +00:00
suml = sumr = 0;
ptr = src + j;
for (uint i = 0; i < ilength; i ++)
2013-06-22 18:19:27 +00:00
{
suml += ptr[2 * i] * filterCoeffsStereo[2 * i];
sumr += ptr[2 * i + 1] * filterCoeffsStereo[2 * i + 1];
2013-06-22 18:19:27 +00:00
}
#ifdef SOUNDTOUCH_INTEGER_SAMPLES
suml >>= resultDivFactor;
sumr >>= resultDivFactor;
// saturate to 16 bit integer limits
suml = (suml < -32768) ? -32768 : (suml > 32767) ? 32767 : suml;
// saturate to 16 bit integer limits
sumr = (sumr < -32768) ? -32768 : (sumr > 32767) ? 32767 : sumr;
#endif // SOUNDTOUCH_INTEGER_SAMPLES
dest[j] = (SAMPLETYPE)suml;
dest[j + 1] = (SAMPLETYPE)sumr;
}
return numSamples - ilength;
2013-06-22 18:19:27 +00:00
}
// Usual C-version of the filter routine for mono sound
uint FIRFilter::evaluateFilterMono(SAMPLETYPE *dest, const SAMPLETYPE *src, uint numSamples) const
{
2015-12-28 12:07:53 +00:00
int j, end;
2013-06-22 18:19:27 +00:00
// hint compiler autovectorization that loop length is divisible by 8
int ilength = length & -8;
assert(ilength != 0);
2013-06-22 18:19:27 +00:00
end = numSamples - ilength;
2015-12-28 12:07:53 +00:00
#pragma omp parallel for
for (j = 0; j < end; j ++)
2013-06-22 18:19:27 +00:00
{
2015-12-28 12:07:53 +00:00
const SAMPLETYPE *pSrc = src + j;
LONG_SAMPLETYPE sum;
int i;
2015-12-28 12:07:53 +00:00
2013-06-22 18:19:27 +00:00
sum = 0;
for (i = 0; i < ilength; i ++)
2013-06-22 18:19:27 +00:00
{
sum += pSrc[i] * filterCoeffs[i];
2013-06-22 18:19:27 +00:00
}
#ifdef SOUNDTOUCH_INTEGER_SAMPLES
sum >>= resultDivFactor;
// saturate to 16 bit integer limits
sum = (sum < -32768) ? -32768 : (sum > 32767) ? 32767 : sum;
#endif // SOUNDTOUCH_INTEGER_SAMPLES
dest[j] = (SAMPLETYPE)sum;
}
return end;
}
2015-12-28 12:07:53 +00:00
uint FIRFilter::evaluateFilterMulti(SAMPLETYPE *dest, const SAMPLETYPE *src, uint numSamples, uint numChannels)
2013-06-22 18:19:27 +00:00
{
2015-12-28 12:07:53 +00:00
int j, end;
2013-06-22 18:19:27 +00:00
assert(length != 0);
assert(src != nullptr);
assert(dest != nullptr);
assert(filterCoeffs != nullptr);
2015-12-28 12:07:53 +00:00
assert(numChannels < 16);
2013-06-22 18:19:27 +00:00
// hint compiler autovectorization that loop length is divisible by 8
int ilength = length & -8;
end = numChannels * (numSamples - ilength);
2013-06-22 18:19:27 +00:00
2015-12-28 12:07:53 +00:00
#pragma omp parallel for
2013-06-22 18:19:27 +00:00
for (j = 0; j < end; j += numChannels)
{
const SAMPLETYPE *ptr;
2015-12-28 12:07:53 +00:00
LONG_SAMPLETYPE sums[16];
uint c;
int i;
2015-12-28 12:07:53 +00:00
for (c = 0; c < numChannels; c ++)
{
sums[c] = 0;
}
2013-06-22 18:19:27 +00:00
ptr = src + j;
for (i = 0; i < ilength; i ++)
2013-06-22 18:19:27 +00:00
{
SAMPLETYPE coef=filterCoeffs[i];
for (c = 0; c < numChannels; c ++)
{
2015-12-28 12:07:53 +00:00
sums[c] += ptr[0] * coef;
2013-06-22 18:19:27 +00:00
ptr ++;
}
}
for (c = 0; c < numChannels; c ++)
{
#ifdef SOUNDTOUCH_INTEGER_SAMPLES
2015-12-28 12:07:53 +00:00
sums[c] >>= resultDivFactor;
2013-06-22 18:19:27 +00:00
#endif // SOUNDTOUCH_INTEGER_SAMPLES
2015-12-28 12:07:53 +00:00
dest[j+c] = (SAMPLETYPE)sums[c];
2013-06-22 18:19:27 +00:00
}
}
return numSamples - ilength;
2013-06-22 18:19:27 +00:00
}
// Set filter coeffiecients and length.
//
// Throws an exception if filter length isn't divisible by 8
void FIRFilter::setCoefficients(const SAMPLETYPE *coeffs, uint newLength, uint uResultDivFactor)
{
assert(newLength > 0);
if (newLength % 8) ST_THROW_RT_ERROR("FIR filter length not divisible by 8");
#ifdef SOUNDTOUCH_FLOAT_SAMPLES
// scale coefficients already here if using floating samples
double scale = 1.0 / resultDivider;
#else
short scale = 1;
#endif
2013-06-22 18:19:27 +00:00
lengthDiv8 = newLength / 8;
length = lengthDiv8 * 8;
assert(length == newLength);
resultDivFactor = uResultDivFactor;
resultDivider = (SAMPLETYPE)::pow(2.0, (int)resultDivFactor);
delete[] filterCoeffs;
filterCoeffs = new SAMPLETYPE[length];
delete[] filterCoeffsStereo;
filterCoeffsStereo = new SAMPLETYPE[length*2];
for (uint i = 0; i < length; i ++)
{
filterCoeffs[i] = (SAMPLETYPE)(coeffs[i] * scale);
// create also stereo set of filter coefficients: this allows compiler
// to autovectorize filter evaluation much more efficiently
filterCoeffsStereo[2 * i] = (SAMPLETYPE)(coeffs[i] * scale);
filterCoeffsStereo[2 * i + 1] = (SAMPLETYPE)(coeffs[i] * scale);
}
2013-06-22 18:19:27 +00:00
}
uint FIRFilter::getLength() const
{
return length;
}
// Applies the filter to the given sequence of samples.
//
// Note : The amount of outputted samples is by value of 'filter_length'
// smaller than the amount of input samples.
2015-12-28 12:07:53 +00:00
uint FIRFilter::evaluate(SAMPLETYPE *dest, const SAMPLETYPE *src, uint numSamples, uint numChannels)
2013-06-22 18:19:27 +00:00
{
assert(length > 0);
assert(lengthDiv8 * 8 == length);
if (numSamples < length) return 0;
#ifndef USE_MULTICH_ALWAYS
if (numChannels == 1)
{
return evaluateFilterMono(dest, src, numSamples);
}
else if (numChannels == 2)
{
return evaluateFilterStereo(dest, src, numSamples);
}
else
#endif // USE_MULTICH_ALWAYS
{
assert(numChannels > 0);
return evaluateFilterMulti(dest, src, numSamples, numChannels);
}
}
// Operator 'new' is overloaded so that it automatically creates a suitable instance
// depending on if we've a MMX-capable CPU available or not.
void * FIRFilter::operator new(size_t)
2013-06-22 18:19:27 +00:00
{
// Notice! don't use "new FIRFilter" directly, use "newInstance" to create a new instance instead!
ST_THROW_RT_ERROR("Error in FIRFilter::new: Don't use 'new FIRFilter', use 'newInstance' member instead!");
return newInstance();
}
FIRFilter * FIRFilter::newInstance()
{
uint uExtensions;
uExtensions = detectCPUextensions();
// Check if MMX/SSE instruction set extensions supported by CPU
#ifdef SOUNDTOUCH_ALLOW_MMX
// MMX routines available only with integer sample types
if (uExtensions & SUPPORT_MMX)
{
return ::new FIRFilterMMX;
}
else
#endif // SOUNDTOUCH_ALLOW_MMX
#ifdef SOUNDTOUCH_ALLOW_SSE
if (uExtensions & SUPPORT_SSE)
{
// SSE support
return ::new FIRFilterSSE;
}
else
#endif // SOUNDTOUCH_ALLOW_SSE
{
// ISA optimizations not supported, use plain C version
return ::new FIRFilter;
}
}