3rdparty/SoundTouch/FIRFilter.cpp

////////////////////////////////////////////////////////////////////////////////
///
/// General FIR digital filter routines with MMX optimization.
///
/// Note : MMX optimized functions reside in a separate, platform-specific file,
/// e.g. 'mmx_win.cpp' or 'mmx_gcc.cpp'
///
/// Author        : Copyright (c) Olli Parviainen
/// Author e-mail : oparviai 'at' iki.fi
/// SoundTouch WWW: http://www.surina.net/soundtouch
///
////////////////////////////////////////////////////////////////////////////////
//
// Last changed  : $Date: 2009-02-25 19:13:51 +0200 (Wed, 25 Feb 2009) $
// File revision : $Revision: 4 $
//
// $Id: FIRFilter.cpp 67 2009-02-25 17:13:51Z oparviai $
//
////////////////////////////////////////////////////////////////////////////////
//
// 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 <stdexcept>
#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 = NULL;
}


FIRFilter::~FIRFilter()
{
    delete[] filterCoeffs;
}

// Usual C-version of the filter routine for stereo sound
uint FIRFilter::evaluateFilterStereo(SAMPLETYPE *dest, const SAMPLETYPE *src, uint numSamples) const
{
    uint i, j, end;
    LONG_SAMPLETYPE suml, sumr;
#ifdef FLOAT_SAMPLES
    // when using floating point samples, use a scaler instead of a divider
    // because division is much slower operation than multiplying.
    double dScaler = 1.0 / (double)resultDivider;
#endif

    assert(length != 0);
    assert(src != NULL);
    assert(dest != NULL);
    assert(filterCoeffs != NULL);

    end = 2 * (numSamples - length);

    for (j = 0; j < end; j += 2)
    {
        const SAMPLETYPE *ptr;

        suml = sumr = 0;
        ptr = src + j;

        for (i = 0; i < length; i += 4)
        {
            // loop is unrolled by factor of 4 here for efficiency
            suml += ptr[2 * i + 0] * filterCoeffs[i + 0] +
                    ptr[2 * i + 2] * filterCoeffs[i + 1] +
                    ptr[2 * i + 4] * filterCoeffs[i + 2] +
                    ptr[2 * i + 6] * filterCoeffs[i + 3];
            sumr += ptr[2 * i + 1] * filterCoeffs[i + 0] +
                    ptr[2 * i + 3] * filterCoeffs[i + 1] +
                    ptr[2 * i + 5] * filterCoeffs[i + 2] +
                    ptr[2 * i + 7] * filterCoeffs[i + 3];
        }

#ifdef 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;
#else
        suml *= dScaler;
        sumr *= dScaler;
#endif // INTEGER_SAMPLES
        dest[j] = (SAMPLETYPE)suml;
        dest[j + 1] = (SAMPLETYPE)sumr;
    }
    return numSamples - length;
}


// Usual C-version of the filter routine for mono sound
uint FIRFilter::evaluateFilterMono(SAMPLETYPE *dest, const SAMPLETYPE *src, uint numSamples) const
{
    uint i, j, end;
    LONG_SAMPLETYPE sum;
#ifdef FLOAT_SAMPLES
    // when using floating point samples, use a scaler instead of a divider
    // because division is much slower operation than multiplying.
    double dScaler = 1.0 / (double)resultDivider;
#endif


    assert(length != 0);

    end = numSamples - length;
    for (j = 0; j < end; j ++)
    {
        sum = 0;
        for (i = 0; i < length; i += 4)
        {
            // loop is unrolled by factor of 4 here for efficiency
            sum += src[i + 0] * filterCoeffs[i + 0] +
                   src[i + 1] * filterCoeffs[i + 1] +
                   src[i + 2] * filterCoeffs[i + 2] +
                   src[i + 3] * filterCoeffs[i + 3];
        }
#ifdef INTEGER_SAMPLES
        sum >>= resultDivFactor;
        // saturate to 16 bit integer limits
        sum = (sum < -32768) ? -32768 : (sum > 32767) ? 32767 : sum;
#else
        sum *= dScaler;
#endif // INTEGER_SAMPLES
        dest[j] = (SAMPLETYPE)sum;
        src ++;
    }
    return end;
}


// 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) throw std::runtime_error("FIR filter length not divisible by 8");

    lengthDiv8 = newLength / 8;
    length = lengthDiv8 * 8;
    assert(length == newLength);

    resultDivFactor = uResultDivFactor;
    resultDivider = (SAMPLETYPE)::pow(2.0, (int)resultDivFactor);

    delete[] filterCoeffs;
    filterCoeffs = new SAMPLETYPE[length];
    memcpy(filterCoeffs, coeffs, length * sizeof(SAMPLETYPE));
}


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.
uint FIRFilter::evaluate(SAMPLETYPE *dest, const SAMPLETYPE *src, uint numSamples, uint numChannels) const
{
    assert(numChannels == 1 || numChannels == 2);

    assert(length > 0);
    assert(lengthDiv8 * 8 == length);
    if (numSamples < length) return 0;
    if (numChannels == 2)
    {
        return evaluateFilterStereo(dest, src, numSamples);
    } else {
        return evaluateFilterMono(dest, src, numSamples);
    }
}


// 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 s)
{
    // Notice! don't use "new FIRFilter" directly, use "newInstance" to create a new instance instead!
    throw std::runtime_error("Error in FIRFilter::new: Don't use 'new FIRFilter', use 'newInstance' member instead!");
    return NULL;
}


FIRFilter * FIRFilter::newInstance()
{
    uint uExtensions;

    uExtensions = detectCPUextensions();

    // Check if MMX/SSE/3DNow! instruction set extensions supported by CPU

#ifdef ALLOW_MMX
    // MMX routines available only with integer sample types
    if (uExtensions & SUPPORT_MMX)
    {
        return ::new FIRFilterMMX;
    }
    else
#endif // ALLOW_MMX

#ifdef ALLOW_SSE
    if (uExtensions & SUPPORT_SSE)
    {
        // SSE support
        return ::new FIRFilterSSE;
    }
    else
#endif // ALLOW_SSE

#ifdef ALLOW_3DNOW
    if (uExtensions & SUPPORT_3DNOW)
    {
        // 3DNow! support
        return ::new FIRFilter3DNow;
    }
    else
#endif // ALLOW_3DNOW

    {
        // ISA optimizations not supported, use plain C version
        return ::new FIRFilter;
    }
}
1	////////////////////////////////////////////////////////////////////////////////
2	///
3	/// General FIR digital filter routines with MMX optimization.
4	///
5	/// Note : MMX optimized functions reside in a separate, platform-specific file,
6	/// e.g. 'mmx_win.cpp' or 'mmx_gcc.cpp'
7	///
8	/// Author : Copyright (c) Olli Parviainen
9	/// Author e-mail : oparviai 'at' iki.fi
10	/// SoundTouch WWW: http://www.surina.net/soundtouch
11	///
12	////////////////////////////////////////////////////////////////////////////////
13	//
14	// Last changed : $Date: 2009-02-25 19:13:51 +0200 (Wed, 25 Feb 2009) $
15	// File revision : $Revision: 4 $
16	//
17	// $Id: FIRFilter.cpp 67 2009-02-25 17:13:51Z oparviai $
18	//
19	////////////////////////////////////////////////////////////////////////////////
20	//
21	// License :
22	//
23	// SoundTouch audio processing library
24	// Copyright (c) Olli Parviainen
25	//
26	// This library is free software; you can redistribute it and/or
27	// modify it under the terms of the GNU Lesser General Public
28	// License as published by the Free Software Foundation; either
29	// version 2.1 of the License, or (at your option) any later version.
30	//
31	// This library is distributed in the hope that it will be useful,
32	// but WITHOUT ANY WARRANTY; without even the implied warranty of
33	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
34	// Lesser General Public License for more details.
35	//
36	// You should have received a copy of the GNU Lesser General Public
37	// License along with this library; if not, write to the Free Software
38	// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
39	//
40	////////////////////////////////////////////////////////////////////////////////
41
42	#include <memory.h>
43	#include <assert.h>
44	#include <math.h>
45	#include <stdlib.h>
46	#include <stdexcept>
47	#include "FIRFilter.h"
48	#include "cpu_detect.h"
49
50	using namespace soundtouch;
51
52	/*****************************************************************************
53	*
54	* Implementation of the class 'FIRFilter'
55	*
56	*****************************************************************************/
57
58	FIRFilter::FIRFilter()
59	{
60	resultDivFactor = 0;
61	resultDivider = 0;
62	length = 0;
63	lengthDiv8 = 0;
64	filterCoeffs = NULL;
65	}
66
67
68	FIRFilter::~FIRFilter()
69	{
70	delete[] filterCoeffs;
71	}
72
73	// Usual C-version of the filter routine for stereo sound
74	uint FIRFilter::evaluateFilterStereo(SAMPLETYPE dest, const SAMPLETYPE src, uint numSamples) const
75	{
76	uint i, j, end;
77	LONG_SAMPLETYPE suml, sumr;
78	#ifdef FLOAT_SAMPLES
79	// when using floating point samples, use a scaler instead of a divider
80	// because division is much slower operation than multiplying.
81	double dScaler = 1.0 / (double)resultDivider;
82	#endif
83
84	assert(length != 0);
85	assert(src != NULL);
86	assert(dest != NULL);
87	assert(filterCoeffs != NULL);
88
89	end = 2 * (numSamples - length);
90
91	for (j = 0; j < end; j += 2)
92	{
93	const SAMPLETYPE *ptr;
94
95	suml = sumr = 0;
96	ptr = src + j;
97
98	for (i = 0; i < length; i += 4)
99	{
100	// loop is unrolled by factor of 4 here for efficiency
101	suml += ptr[2 * i + 0] * filterCoeffs[i + 0] +
102	ptr[2 * i + 2] * filterCoeffs[i + 1] +
103	ptr[2 * i + 4] * filterCoeffs[i + 2] +
104	ptr[2 * i + 6] * filterCoeffs[i + 3];
105	sumr += ptr[2 * i + 1] * filterCoeffs[i + 0] +
106	ptr[2 * i + 3] * filterCoeffs[i + 1] +
107	ptr[2 * i + 5] * filterCoeffs[i + 2] +
108	ptr[2 * i + 7] * filterCoeffs[i + 3];
109	}
110
111	#ifdef INTEGER_SAMPLES
112	suml >>= resultDivFactor;
113	sumr >>= resultDivFactor;
114	// saturate to 16 bit integer limits
115	suml = (suml < -32768) ? -32768 : (suml > 32767) ? 32767 : suml;
116	// saturate to 16 bit integer limits
117	sumr = (sumr < -32768) ? -32768 : (sumr > 32767) ? 32767 : sumr;
118	#else
119	suml *= dScaler;
120	sumr *= dScaler;
121	#endif // INTEGER_SAMPLES
122	dest[j] = (SAMPLETYPE)suml;
123	dest[j + 1] = (SAMPLETYPE)sumr;
124	}
125	return numSamples - length;
126	}
127
128
129
130
131	// Usual C-version of the filter routine for mono sound
132	uint FIRFilter::evaluateFilterMono(SAMPLETYPE dest, const SAMPLETYPE src, uint numSamples) const
133	{
134	uint i, j, end;
135	LONG_SAMPLETYPE sum;
136	#ifdef FLOAT_SAMPLES
137	// when using floating point samples, use a scaler instead of a divider
138	// because division is much slower operation than multiplying.
139	double dScaler = 1.0 / (double)resultDivider;
140	#endif
141
142
143	assert(length != 0);
144
145	end = numSamples - length;
146	for (j = 0; j < end; j ++)
147	{
148	sum = 0;
149	for (i = 0; i < length; i += 4)
150	{
151	// loop is unrolled by factor of 4 here for efficiency
152	sum += src[i + 0] * filterCoeffs[i + 0] +
153	src[i + 1] * filterCoeffs[i + 1] +
154	src[i + 2] * filterCoeffs[i + 2] +
155	src[i + 3] * filterCoeffs[i + 3];
156	}
157	#ifdef INTEGER_SAMPLES
158	sum >>= resultDivFactor;
159	// saturate to 16 bit integer limits
160	sum = (sum < -32768) ? -32768 : (sum > 32767) ? 32767 : sum;
161	#else
162	sum *= dScaler;
163	#endif // INTEGER_SAMPLES
164	dest[j] = (SAMPLETYPE)sum;
165	src ++;
166	}
167	return end;
168	}
169
170
171	// Set filter coeffiecients and length.
172	//
173	// Throws an exception if filter length isn't divisible by 8
174	void FIRFilter::setCoefficients(const SAMPLETYPE *coeffs, uint newLength, uint uResultDivFactor)
175	{
176	assert(newLength > 0);
177	if (newLength % 8) throw std::runtime_error("FIR filter length not divisible by 8");
178
179	lengthDiv8 = newLength / 8;
180	length = lengthDiv8 * 8;
181	assert(length == newLength);
182
183	resultDivFactor = uResultDivFactor;
184	resultDivider = (SAMPLETYPE)::pow(2.0, (int)resultDivFactor);
185
186	delete[] filterCoeffs;
187	filterCoeffs = new SAMPLETYPE[length];
188	memcpy(filterCoeffs, coeffs, length * sizeof(SAMPLETYPE));
189	}
190
191
192	uint FIRFilter::getLength() const
193	{
194	return length;
195	}
196
197
198
199	// Applies the filter to the given sequence of samples.
200	//
201	// Note : The amount of outputted samples is by value of 'filter_length'
202	// smaller than the amount of input samples.
203	uint FIRFilter::evaluate(SAMPLETYPE dest, const SAMPLETYPE src, uint numSamples, uint numChannels) const
204	{
205	assert(numChannels == 1 \|\| numChannels == 2);
206
207	assert(length > 0);
208	assert(lengthDiv8 * 8 == length);
209	if (numSamples < length) return 0;
210	if (numChannels == 2)
211	{
212	return evaluateFilterStereo(dest, src, numSamples);
213	} else {
214	return evaluateFilterMono(dest, src, numSamples);
215	}
216	}
217
218
219
220	// Operator 'new' is overloaded so that it automatically creates a suitable instance
221	// depending on if we've a MMX-capable CPU available or not.
222	void * FIRFilter::operator new(size_t s)
223	{
224	// Notice! don't use "new FIRFilter" directly, use "newInstance" to create a new instance instead!
225	throw std::runtime_error("Error in FIRFilter::new: Don't use 'new FIRFilter', use 'newInstance' member instead!");
226	return NULL;
227	}
228
229
230	FIRFilter * FIRFilter::newInstance()
231	{
232	uint uExtensions;
233
234	uExtensions = detectCPUextensions();
235
236	// Check if MMX/SSE/3DNow! instruction set extensions supported by CPU
237
238	#ifdef ALLOW_MMX
239	// MMX routines available only with integer sample types
240	if (uExtensions & SUPPORT_MMX)
241	{
242	return ::new FIRFilterMMX;
243	}
244	else
245	#endif // ALLOW_MMX
246
247	#ifdef ALLOW_SSE
248	if (uExtensions & SUPPORT_SSE)
249	{
250	// SSE support
251	return ::new FIRFilterSSE;
252	}
253	else
254	#endif // ALLOW_SSE
255
256	#ifdef ALLOW_3DNOW
257	if (uExtensions & SUPPORT_3DNOW)
258	{
259	// 3DNow! support
260	return ::new FIRFilter3DNow;
261	}
262	else
263	#endif // ALLOW_3DNOW
264
265	{
266	// ISA optimizations not supported, use plain C version
267	return ::new FIRFilter;
268	}
269	}