diff options
Diffstat (limited to 'src/Resampler.cpp')
| -rw-r--r-- | src/Resampler.cpp | 318 |
1 files changed, 318 insertions, 0 deletions
diff --git a/src/Resampler.cpp b/src/Resampler.cpp new file mode 100644 index 0000000..67bdf4f --- /dev/null +++ b/src/Resampler.cpp @@ -0,0 +1,318 @@ +/* + Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011 Her Majesty + the Queen in Right of Canada (Communications Research Center Canada) + */ +/* + This file is part of CRC-DADMOD. + + CRC-DADMOD is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + CRC-DADMOD 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 General Public License for more details. + + You should have received a copy of the GNU General Public License + along with CRC-DADMOD. If not, see <http://www.gnu.org/licenses/>. + */ + +#include "Resampler.h" +#include "PcDebug.h" + + +#ifdef __ppc__ +# define memalign(a, b) malloc(b) +#else // !__ppc__ +# include <mm_malloc.h> +#endif +#include <sys/types.h> +#include <string.h> +#include <stdexcept> +#include <assert.h> + + +unsigned gcd(unsigned a, unsigned b) +{ + if (b == 0) { + return a; + } + + return gcd(b, a % b); +} + + +Resampler::Resampler(size_t inputRate, size_t outputRate, size_t resolution) : + ModCodec(ModFormat(inputRate * sizeof(complexf)), + ModFormat(outputRate * sizeof(complexf))), + myFftPlan1(NULL), + myFftPlan2(NULL), + myFftIn(NULL), + myFftOut(NULL), + myBufferIn(NULL), + myBufferOut(NULL), + myFront(NULL), + myBack(NULL), + myWindow(NULL) +{ + PDEBUG("Resampler::Resampler(%zu, %zu) @ %p\n", inputRate, outputRate, this); + + fprintf(stderr, "This software uses KISS FFT.\n\n"); + fprintf(stderr, "Copyright (c) 2003-2004 Mark Borgerding\n" + "\n" + "All rights reserved.\n" + "\n" + "Redistribution and use in source and binary forms, with or " + "without modification, are permitted provided that the following " + "conditions are met:\n" + "\n" + " * Redistributions of source code must retain the above " + "copyright notice, this list of conditions and the following " + "disclaimer.\n" + " * Redistributions in binary form must reproduce the above " + "copyright notice, this list of conditions and the following " + "disclaimer in the documentation and/or other materials provided " + "with the distribution.\n" + " * Neither the author nor the names of any contributors may be " + "used to endorse or promote products derived from this software " + "without specific prior written permission.\n" + "\n" + "THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND " + "CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, " + "INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF " + "MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE " + "DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS " + "BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, " + "EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED " + "TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, " + "DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON " + "ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, " + "OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY " + "OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE " + "POSSIBILITY OF SUCH DAMAGE.\n"); + + size_t divisor = gcd(inputRate, outputRate); + L = outputRate / divisor; + M = inputRate / divisor; + PDEBUG(" gcd: %zu, L: %zu, M: %zu\n", divisor, L, M); + { + unsigned factor = resolution * 2 / M; + if (factor & 1) { + ++factor; + } + myFftSizeIn = factor * M; + myFftSizeOut = factor * L; + } + PDEBUG(" FFT size in: %zu, FFT size out: %zu\n", myFftSizeIn, myFftSizeOut); + + if (myFftSizeIn > myFftSizeOut) { + myFactor = 1.0f / myFftSizeIn; + } else { + myFactor = 1.0f / myFftSizeOut; + } + + myWindow = (float*)memalign(16, myFftSizeIn * sizeof(float)); + for (size_t i = 0; i < myFftSizeIn; ++i) { + myWindow[i] = 0.5f * (1.0f - cosf(2.0f * M_PI * i / (myFftSizeIn - 1))); + PDEBUG("Window[%zu] = %f\n", i, myWindow[i]); + } + + myFftIn = (FFT_TYPE*)memalign(16, myFftSizeIn * sizeof(FFT_TYPE)); + myFftOut = (FFT_TYPE*)memalign(16, myFftSizeOut * sizeof(FFT_TYPE)); + myBufferIn = (complexf*)memalign(16, myFftSizeIn / 2 * sizeof(FFT_TYPE)); + myBufferOut = (complexf*)memalign(16, myFftSizeOut / 2 * sizeof(FFT_TYPE)); + myFront = (FFT_TYPE*)memalign(16, myFftSizeIn * sizeof(FFT_TYPE)); + myBack = (FFT_TYPE*)memalign(16, myFftSizeOut * sizeof(FFT_TYPE)); + myFftPlan1 = kiss_fft_alloc(myFftSizeIn, 0, NULL, NULL); + myFftPlan2 = kiss_fft_alloc(myFftSizeOut, 1, NULL, NULL); + + memset(myBufferIn, 0, myFftSizeIn / 2 * sizeof(FFT_TYPE)); + memset(myBufferOut, 0, myFftSizeOut / 2 * sizeof(FFT_TYPE)); +} + + +Resampler::~Resampler() +{ + PDEBUG("Resampler::~Resampler() @ %p\n", this); + + if (myFftPlan1 != NULL) { + free(myFftPlan1); + } + if (myFftPlan2 != NULL) { + free(myFftPlan2); + } + if (myFftIn != NULL) { + free(myFftIn); + } + if (myFftOut != NULL) { + free(myFftOut); + } + if (myBufferIn != NULL) { + free(myBufferIn); + } + if (myBufferOut != NULL) { + free(myBufferOut); + } + if (myFront != NULL) { + free(myFront); + } + if (myBack != NULL) { + free(myBack); + } + if (myWindow != NULL) { + free(myWindow); + } + kiss_fft_cleanup(); +} + + +int Resampler::process(Buffer* const dataIn, Buffer* dataOut) +{ + PDEBUG("Resampler::process(dataIn: %p, dataOut: %p)\n", + dataIn, dataOut); + + dataOut->setLength(dataIn->getLength() * L / M); + + FFT_TYPE* in = reinterpret_cast<FFT_TYPE*>(dataIn->getData()); + FFT_TYPE* out = reinterpret_cast<FFT_TYPE*>(dataOut->getData()); + size_t sizeIn = dataIn->getLength() / sizeof(complexf); + +#ifdef USE_SIMD + size_t sizeOut = dataOut->getLength() / sizeof(complexf); + + typedef struct { + float r[4]; + float i[4]; + } fft_data; + assert(sizeof(FFT_TYPE) == sizeof(fft_data)); + fft_data *fftDataIn = (fft_data*)myFftIn; + fft_data *fftDataOut = (fft_data*)myFftOut; + complexf *cplxIn = (complexf*)in; + complexf *cplxOut = (complexf*)out; + for (size_t i = 0, j = 0; i < sizeIn; ) { + for (int k = 0; k < 4; ++k) { + if (i < sizeIn) { + for (size_t l = 0; l < myFftSizeIn / 2; ++l) { + fftDataIn[l].r[k] = myBufferIn[l].real(); + fftDataIn[l].i[k] = myBufferIn[l].imag(); + fftDataIn[myFftSizeIn / 2 + l].r[k] = cplxIn[i + l].real(); + fftDataIn[myFftSizeIn / 2 + l].i[k] = cplxIn[i + l].imag(); + } + memcpy(myBufferIn, cplxIn + i, myFftSizeIn / 2 * sizeof(complexf)); + i += myFftSizeIn / 2; + } else { + for (size_t l = 0; l < myFftSizeIn; ++l) { + fftDataIn[l].r[k] = 0.0f; + fftDataIn[l].i[k] = 0.0f; + } + } + } + for (size_t k = 0; k < myFftSizeIn; ++ k) { + FFT_REAL(myFftIn[k]) = _mm_mul_ps(FFT_REAL(myFftIn[k]), _mm_set_ps1(myWindow[k])); + FFT_IMAG(myFftIn[k]) = _mm_mul_ps(FFT_IMAG(myFftIn[k]), _mm_set_ps1(myWindow[k])); + } + + kiss_fft(myFftPlan1, myFftIn, myFront); + + if (myFftSizeOut > myFftSizeIn) { + memset(myBack, 0, myFftSizeOut * sizeof(FFT_TYPE)); + memcpy(myBack, myFront, myFftSizeIn / 2 * sizeof(FFT_TYPE)); + memcpy(&myBack[myFftSizeOut - (myFftSizeIn / 2)], + &myFront[myFftSizeIn / 2], + myFftSizeIn / 2 * sizeof(FFT_TYPE)); + // Copy input Fs + FFT_REAL(myBack[myFftSizeIn / 2]) = + FFT_REAL(myFront[myFftSizeIn / 2]); + FFT_IMAG(myBack[myFftSizeIn / 2]) = + FFT_IMAG(myFront[myFftSizeIn / 2]); + } else { + memcpy(myBack, myFront, myFftSizeOut / 2 * sizeof(FFT_TYPE)); + memcpy(&myBack[myFftSizeOut / 2], + &myFront[myFftSizeIn - (myFftSizeOut / 2)], + myFftSizeOut / 2 * sizeof(FFT_TYPE)); + // Average output Fs from input + FFT_REAL(myBack[myFftSizeOut / 2]) = + _mm_add_ps(FFT_REAL(myBack[myFftSizeOut / 2]), + FFT_REAL(myFront[myFftSizeOut / 2])); + FFT_IMAG(myBack[myFftSizeOut / 2]) = + _mm_add_ps(FFT_IMAG(myBack[myFftSizeOut / 2]), + FFT_IMAG(myFront[myFftSizeOut / 2])); + FFT_REAL(myBack[myFftSizeOut / 2]) = + _mm_mul_ps(FFT_REAL(myBack[myFftSizeOut / 2]), _mm_set_ps1(0.5f)); + FFT_IMAG(myBack[myFftSizeOut / 2]) = + _mm_mul_ps(FFT_IMAG(myBack[myFftSizeOut / 2]), _mm_set_ps1(0.5f)); + } + for (size_t k = 0; k < myFftSizeOut; ++k) { + FFT_REAL(myBack[k]) = _mm_mul_ps(FFT_REAL(myBack[k]), _mm_set_ps1(myFactor)); + FFT_IMAG(myBack[k]) = _mm_mul_ps(FFT_IMAG(myBack[k]), _mm_set_ps1(myFactor)); + } + + kiss_fft(myFftPlan2, myBack, myFftOut); + + for (size_t k = 0; k < 4; ++k) { + if (j < sizeOut) { + for (size_t l = 0; l < myFftSizeOut / 2; ++l) { + cplxOut[j + l].real() = myBufferOut[l].real() + fftDataOut[l].r[k]; + cplxOut[j + l].imag() = myBufferOut[l].imag() + fftDataOut[l].i[k]; + myBufferOut[l].real() = fftDataOut[myFftSizeOut / 2 + l].r[k]; + myBufferOut[l].imag() = fftDataOut[myFftSizeOut / 2 + l].i[k]; + } + } + j += myFftSizeOut / 2; + } + } +#else + for (size_t i = 0, j = 0; i < sizeIn; i += myFftSizeIn / 2, j += myFftSizeOut / 2) { + memcpy(myFftIn, myBufferIn, myFftSizeIn / 2 * sizeof(FFT_TYPE)); + memcpy(myFftIn + (myFftSizeIn / 2), in + i, myFftSizeIn / 2 * sizeof(FFT_TYPE)); + memcpy(myBufferIn, in + i, myFftSizeIn / 2 * sizeof(FFT_TYPE)); + for (size_t k = 0; k < myFftSizeIn; ++k) { + FFT_REAL(myFftIn[k]) *= myWindow[k]; + FFT_IMAG(myFftIn[k]) *= myWindow[k]; + } + + kiss_fft(myFftPlan1, myFftIn, myFront); + + if (myFftSizeOut > myFftSizeIn) { + memset(myBack, 0, myFftSizeOut * sizeof(FFT_TYPE)); + memcpy(myBack, myFront, myFftSizeIn / 2 * sizeof(FFT_TYPE)); + memcpy(&myBack[myFftSizeOut - (myFftSizeIn / 2)], + &myFront[myFftSizeIn / 2], + myFftSizeIn / 2 * sizeof(FFT_TYPE)); + // Copy input Fs + FFT_REAL(myBack[myFftSizeIn / 2]) = + FFT_REAL(myFront[myFftSizeIn / 2]); + FFT_IMAG(myBack[myFftSizeIn / 2]) = + FFT_IMAG(myFront[myFftSizeIn / 2]); + } else { + memcpy(myBack, myFront, myFftSizeOut / 2 * sizeof(FFT_TYPE)); + memcpy(&myBack[myFftSizeOut / 2], + &myFront[myFftSizeIn - (myFftSizeOut / 2)], + myFftSizeOut / 2 * sizeof(FFT_TYPE)); + // Average output Fs from input + FFT_REAL(myBack[myFftSizeOut / 2]) += + FFT_REAL(myFront[myFftSizeOut / 2]); + FFT_IMAG(myBack[myFftSizeOut / 2]) += + FFT_IMAG(myFront[myFftSizeOut / 2]); + FFT_REAL(myBack[myFftSizeOut / 2]) *= 0.5f; + FFT_IMAG(myBack[myFftSizeOut / 2]) *= 0.5f; + } + for (size_t k = 0; k < myFftSizeOut; ++k) { + FFT_REAL(myBack[k]) *= myFactor; + FFT_IMAG(myBack[k]) *= myFactor; + } + + kiss_fft(myFftPlan2, myBack, myFftOut); + + for (size_t k = 0; k < myFftSizeOut / 2; ++k) { + FFT_REAL(out[j + k]) = myBufferOut[k].real() + FFT_REAL(myFftOut[k]); + FFT_IMAG(out[j + k]) = myBufferOut[k].imag() + FFT_IMAG(myFftOut[k]); + } + memcpy(myBufferOut, myFftOut + (myFftSizeOut / 2), (myFftSizeOut / 2) * sizeof(FFT_TYPE)); + } +#endif + + return 1; +} |