diff --git a/CMakeLists.txt b/CMakeLists.txt index 36a5bef26..f3b26d126 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,7 +30,7 @@ option(DEBUG_OUTPUT "Print debug messages" OFF) option(SANITIZE_ADDRESS "Activate memory address sanitization" OFF) option(HOST_RPI "Compiling on RPi" OFF) option(RX_SAMPLE_24BIT "Internal 24 bit Rx DSP" OFF) -option(NO_DSP_SIMD "Do not ese SIMD instructions for DSP even if available" OFF) +option(NO_DSP_SIMD "Do not use SIMD instructions for DSP even if available" OFF) list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/Modules) diff --git a/sdrbase/dsp/decimators.h b/sdrbase/dsp/decimators.h index 0e2d3874c..44680cbd8 100644 --- a/sdrbase/dsp/decimators.h +++ b/sdrbase/dsp/decimators.h @@ -19,19 +19,11 @@ #include "dsp/dsptypes.h" -#ifdef NO_DSP_SIMD -#include "dsp/inthalfbandfilterdb.h" -#else // NO_DSP_SIMD #ifdef SDR_RX_SAMPLE_24BIT -#include "dsp/inthalfbandfilterdb.h" +#include "dsp/inthalfbandfiltereo2.h" #else // SDR_RX_SAMPLE_24BIT -#ifdef USE_SSE4_1 #include "dsp/inthalfbandfiltereo1.h" -#else // USE_SSE4_1 -#include "dsp/inthalfbandfilterdb.h" -#endif // USE_SSE4_1 #endif // SDR_RX_SAMPLE_24BIT -#endif // NO_DSP_SIMD #define DECIMATORS_HB_FILTER_ORDER 64 @@ -336,48 +328,21 @@ public: void decimate64_cen(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len); private: -#ifdef NO_DSP_SIMD #ifdef SDR_RX_SAMPLE_24BIT - IntHalfbandFilterDB m_decimator2; // 1st stages - IntHalfbandFilterDB m_decimator4; // 2nd stages - IntHalfbandFilterDB m_decimator8; // 3rd stages - IntHalfbandFilterDB m_decimator16; // 4th stages - IntHalfbandFilterDB m_decimator32; // 5th stages - IntHalfbandFilterDB m_decimator64; // 6th stages + IntHalfbandFilterEO2 m_decimator2; // 1st stages + IntHalfbandFilterEO2 m_decimator4; // 2nd stages + IntHalfbandFilterEO2 m_decimator8; // 3rd stages + IntHalfbandFilterEO2 m_decimator16; // 4th stages + IntHalfbandFilterEO2 m_decimator32; // 5th stages + IntHalfbandFilterEO2 m_decimator64; // 6th stages #else // SDR_RX_SAMPLE_24BIT - IntHalfbandFilterDB m_decimator2; // 1st stages - IntHalfbandFilterDB m_decimator4; // 2nd stages - IntHalfbandFilterDB m_decimator8; // 3rd stages - IntHalfbandFilterDB m_decimator16; // 4th stages - IntHalfbandFilterDB m_decimator32; // 5th stages - IntHalfbandFilterDB m_decimator64; // 6th stages -#endif // SDR_RX_SAMPLE_24BIT -#else // NO_DSP_SIMD -#ifdef SDR_RX_SAMPLE_24BIT - IntHalfbandFilterDB m_decimator2; // 1st stages - IntHalfbandFilterDB m_decimator4; // 2nd stages - IntHalfbandFilterDB m_decimator8; // 3rd stages - IntHalfbandFilterDB m_decimator16; // 4th stages - IntHalfbandFilterDB m_decimator32; // 5th stages - IntHalfbandFilterDB m_decimator64; // 6th stages -#else // SDR_RX_SAMPLE_24BIT -#ifdef USE_SSE4_1 IntHalfbandFilterEO1 m_decimator2; // 1st stages IntHalfbandFilterEO1 m_decimator4; // 2nd stages IntHalfbandFilterEO1 m_decimator8; // 3rd stages IntHalfbandFilterEO1 m_decimator16; // 4th stages IntHalfbandFilterEO1 m_decimator32; // 5th stages IntHalfbandFilterEO1 m_decimator64; // 6th stages -#else // USE_SSE4_1 - IntHalfbandFilterDB m_decimator2; // 1st stages - IntHalfbandFilterDB m_decimator4; // 2nd stages - IntHalfbandFilterDB m_decimator8; // 3rd stages - IntHalfbandFilterDB m_decimator16; // 4th stages - IntHalfbandFilterDB m_decimator32; // 5th stages - IntHalfbandFilterDB m_decimator64; // 6th stages -#endif // USE_SSE4_1 #endif // SDR_RX_SAMPLE_24BIT -#endif // NO_DSP_SIMD }; template diff --git a/sdrbase/dsp/inthalfbandfiltereo1.h b/sdrbase/dsp/inthalfbandfiltereo1.h index e76f3a871..b2b8f1793 100644 --- a/sdrbase/dsp/inthalfbandfiltereo1.h +++ b/sdrbase/dsp/inthalfbandfiltereo1.h @@ -681,7 +681,7 @@ protected: int32_t iAcc = 0; int32_t qAcc = 0; -#ifdef USE_SSE4_1 +#if defined(USE_SSE4_1) && !defined(NO_DSP_SIMD) IntHalfbandFilterEO1Intrisics::work( m_ptr, m_even, @@ -731,7 +731,7 @@ protected: int32_t iAcc = 0; int32_t qAcc = 0; -#ifdef USE_SSE4_1 +#if defined(USE_SSE4_1) && !defined(NO_DSP_SIMD) IntHalfbandFilterEO1Intrisics::work( m_ptr, m_even, diff --git a/sdrbase/dsp/inthalfbandfiltereo2.h b/sdrbase/dsp/inthalfbandfiltereo2.h new file mode 100644 index 000000000..15619d1eb --- /dev/null +++ b/sdrbase/dsp/inthalfbandfiltereo2.h @@ -0,0 +1,853 @@ +/////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2018 F4EXB // +// written by Edouard Griffiths // +// // +// Integer half-band FIR based interpolator and decimator // +// This is the even/odd double buffer variant. Really useful only when SIMD is // +// used // +// // +// This program 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 as version 3 of the License, or // +// // +// This program 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 V3 for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with this program. If not, see . // +/////////////////////////////////////////////////////////////////////////////////// + +#ifndef SDRBASE_DSP_INTHALFBANDFILTEREO2_H_ +#define SDRBASE_DSP_INTHALFBANDFILTEREO2_H_ + +#include +#include +#include "dsp/dsptypes.h" +#include "dsp/hbfiltertraits.h" +#include "dsp/inthalfbandfiltereo2i.h" +#include "export.h" + +template +class SDRBASE_API IntHalfbandFilterEO2 { +public: + IntHalfbandFilterEO2(); + + // downsample by 2, return center part of original spectrum + bool workDecimateCenter(Sample* sample) + { + // insert sample into ring-buffer + storeSample((FixReal) sample->real(), (FixReal) sample->imag()); + + switch(m_state) + { + case 0: + // advance write-pointer + advancePointer(); + // next state + m_state = 1; + // tell caller we don't have a new sample + return false; + + default: + // save result + doFIR(sample); + // advance write-pointer + advancePointer(); + // next state + m_state = 0; + + // tell caller we have a new sample + return true; + } + } + + // upsample by 2, return center part of original spectrum - double buffer variant + bool workInterpolateCenterZeroStuffing(Sample* sampleIn, Sample *SampleOut) + { + switch(m_state) + { + case 0: + // insert sample into ring-buffer + storeSample((FixReal) 0, (FixReal) 0); + // save result + doFIR(SampleOut); + // advance write-pointer + advancePointer(); + // next state + m_state = 1; + // tell caller we didn't consume the sample + return false; + + default: + // insert sample into ring-buffer + storeSample((FixReal) sampleIn->real(), (FixReal) sampleIn->imag()); + // save result + doFIR(SampleOut); + // advance write-pointer + advancePointer(); + // next state + m_state = 0; + // tell caller we consumed the sample + return true; + } + } + + /** Optimized upsampler by 2 not calculating FIR with inserted null samples */ + bool workInterpolateCenter(Sample* sampleIn, Sample *SampleOut) + { + switch(m_state) + { + case 0: + // return the middle peak + SampleOut->setReal(m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][0]); + SampleOut->setImag(m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][1]); + m_state = 1; // next state + return false; // tell caller we didn't consume the sample + + default: + // calculate with non null samples + doInterpolateFIR(SampleOut); + + // insert sample into ring double buffer + m_samples[m_ptr][0] = sampleIn->real(); + m_samples[m_ptr][1] = sampleIn->imag(); + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][0] = sampleIn->real(); + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][1] = sampleIn->imag(); + + // advance pointer + if (m_ptr < (HBFIRFilterTraits::hbOrder/2) - 1) { + m_ptr++; + } else { + m_ptr = 0; + } + + m_state = 0; // next state + return true; // tell caller we consumed the sample + } + } + + bool workDecimateCenter(int32_t *x, int32_t *y) + { + // insert sample into ring-buffer + storeSample32(*x, *y); + + switch(m_state) + { + case 0: + // advance write-pointer + advancePointer(); + // next state + m_state = 1; + // tell caller we don't have a new sample + return false; + + default: + // save result + doFIR(x, y); + // advance write-pointer + advancePointer(); + // next state + m_state = 0; + // tell caller we have a new sample + return true; + } + } + + // downsample by 2, return lower half of original spectrum + bool workDecimateLowerHalf(Sample* sample) + { + switch(m_state) + { + case 0: + // insert sample into ring-buffer + storeSample((FixReal) -sample->imag(), (FixReal) sample->real()); + // advance write-pointer + advancePointer(); + // next state + m_state = 1; + // tell caller we don't have a new sample + return false; + + case 1: + // insert sample into ring-buffer + storeSample((FixReal) -sample->real(), (FixReal) -sample->imag()); + // save result + doFIR(sample); + // advance write-pointer + advancePointer(); + // next state + m_state = 2; + // tell caller we have a new sample + return true; + + case 2: + // insert sample into ring-buffer + storeSample((FixReal) sample->imag(), (FixReal) -sample->real()); + // advance write-pointer + advancePointer(); + // next state + m_state = 3; + // tell caller we don't have a new sample + return false; + + default: + // insert sample into ring-buffer + storeSample((FixReal) sample->real(), (FixReal) sample->imag()); + // save result + doFIR(sample); + // advance write-pointer + advancePointer(); + // next state + m_state = 0; + // tell caller we have a new sample + return true; + } + } + + // upsample by 2, from lower half of original spectrum - double buffer variant + bool workInterpolateLowerHalfZeroStuffing(Sample* sampleIn, Sample *sampleOut) + { + Sample s; + + switch(m_state) + { + case 0: + // insert sample into ring-buffer + storeSample((FixReal) 0, (FixReal) 0); + + // save result + doFIR(&s); + sampleOut->setReal(s.imag()); + sampleOut->setImag(-s.real()); + + // advance write-pointer + advancePointer(); + + // next state + m_state = 1; + + // tell caller we didn't consume the sample + return false; + + case 1: + // insert sample into ring-buffer + storeSample((FixReal) sampleIn->real(), (FixReal) sampleIn->imag()); + + // save result + doFIR(&s); + sampleOut->setReal(-s.real()); + sampleOut->setImag(-s.imag()); + + // advance write-pointer + advancePointer(); + + // next state + m_state = 2; + + // tell caller we consumed the sample + return true; + + case 2: + // insert sample into ring-buffer + storeSample((FixReal) 0, (FixReal) 0); + + // save result + doFIR(&s); + sampleOut->setReal(-s.imag()); + sampleOut->setImag(s.real()); + + // advance write-pointer + advancePointer(); + + // next state + m_state = 3; + + // tell caller we didn't consume the sample + return false; + + default: + // insert sample into ring-buffer + storeSample((FixReal) sampleIn->real(), (FixReal) sampleIn->imag()); + + // save result + doFIR(&s); + sampleOut->setReal(s.real()); + sampleOut->setImag(s.imag()); + + // advance write-pointer + advancePointer(); + + // next state + m_state = 0; + + // tell caller we consumed the sample + return true; + } + } + + /** Optimized upsampler by 2 not calculating FIR with inserted null samples */ + bool workInterpolateLowerHalf(Sample* sampleIn, Sample *sampleOut) + { + Sample s; + + switch(m_state) + { + case 0: + // return the middle peak + sampleOut->setReal(m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][1]); // imag + sampleOut->setImag(-m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][0]); // - real + m_state = 1; // next state + return false; // tell caller we didn't consume the sample + + case 1: + // calculate with non null samples + doInterpolateFIR(&s); + sampleOut->setReal(-s.real()); + sampleOut->setImag(-s.imag()); + + // insert sample into ring double buffer + m_samples[m_ptr][0] = sampleIn->real(); + m_samples[m_ptr][1] = sampleIn->imag(); + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][0] = sampleIn->real(); + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][1] = sampleIn->imag(); + + // advance pointer + if (m_ptr < (HBFIRFilterTraits::hbOrder/2) - 1) { + m_ptr++; + } else { + m_ptr = 0; + } + + m_state = 2; // next state + return true; // tell caller we consumed the sample + + case 2: + // return the middle peak + sampleOut->setReal(-m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][1]); // - imag + sampleOut->setImag(m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][0]); // real + m_state = 3; // next state + return false; // tell caller we didn't consume the sample + + default: + // calculate with non null samples + doInterpolateFIR(&s); + sampleOut->setReal(s.real()); + sampleOut->setImag(s.imag()); + + // insert sample into ring double buffer + m_samples[m_ptr][0] = sampleIn->real(); + m_samples[m_ptr][1] = sampleIn->imag(); + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][0] = sampleIn->real(); + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][1] = sampleIn->imag(); + + // advance pointer + if (m_ptr < (HBFIRFilterTraits::hbOrder/2) - 1) { + m_ptr++; + } else { + m_ptr = 0; + } + + m_state = 0; // next state + return true; // tell caller we consumed the sample + } + } + + // downsample by 2, return upper half of original spectrum + bool workDecimateUpperHalf(Sample* sample) + { + switch(m_state) + { + case 0: + // insert sample into ring-buffer + storeSample((FixReal) sample->imag(), (FixReal) -sample->real()); + // advance write-pointer + advancePointer(); + // next state + m_state = 1; + // tell caller we don't have a new sample + return false; + + case 1: + // insert sample into ring-buffer + storeSample((FixReal) -sample->real(), (FixReal) -sample->imag()); + // save result + doFIR(sample); + // advance write-pointer + advancePointer(); + // next state + m_state = 2; + // tell caller we have a new sample + return true; + + case 2: + // insert sample into ring-buffer + storeSample((FixReal) -sample->imag(), (FixReal) sample->real()); + // advance write-pointer + advancePointer(); + // next state + m_state = 3; + // tell caller we don't have a new sample + return false; + + default: + // insert sample into ring-buffer + storeSample((FixReal) sample->real(), (FixReal) sample->imag()); + // save result + doFIR(sample); + // advance write-pointer + advancePointer(); + // next state + m_state = 0; + // tell caller we have a new sample + return true; + } + } + + // upsample by 2, move original spectrum to upper half - double buffer variant + bool workInterpolateUpperHalfZeroStuffing(Sample* sampleIn, Sample *sampleOut) + { + Sample s; + + switch(m_state) + { + case 0: + // insert sample into ring-buffer + storeSample((FixReal) 0, (FixReal) 0); + + // save result + doFIR(&s); + sampleOut->setReal(-s.imag()); + sampleOut->setImag(s.real()); + + // advance write-pointer + advancePointer(); + + // next state + m_state = 1; + + // tell caller we didn't consume the sample + return false; + + case 1: + // insert sample into ring-buffer + storeSample((FixReal) sampleIn->real(), (FixReal) sampleIn->imag()); + + // save result + doFIR(&s); + sampleOut->setReal(-s.real()); + sampleOut->setImag(-s.imag()); + + // advance write-pointer + advancePointer(); + + // next state + m_state = 2; + + // tell caller we consumed the sample + return true; + + case 2: + // insert sample into ring-buffer + storeSample((FixReal) 0, (FixReal) 0); + + // save result + doFIR(&s); + sampleOut->setReal(s.imag()); + sampleOut->setImag(-s.real()); + + // advance write-pointer + advancePointer(); + + // next state + m_state = 3; + + // tell caller we didn't consume the sample + return false; + + default: + // insert sample into ring-buffer + storeSample((FixReal) sampleIn->real(), (FixReal) sampleIn->imag()); + + // save result + doFIR(&s); + sampleOut->setReal(s.real()); + sampleOut->setImag(s.imag()); + + // advance write-pointer + advancePointer(); + + // next state + m_state = 0; + + // tell caller we consumed the sample + return true; + } + } + + /** Optimized upsampler by 2 not calculating FIR with inserted null samples */ + bool workInterpolateUpperHalf(Sample* sampleIn, Sample *sampleOut) + { + Sample s; + + switch(m_state) + { + case 0: + // return the middle peak + sampleOut->setReal(-m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][1]); // - imag + sampleOut->setImag(m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][0]); // + real + m_state = 1; // next state + return false; // tell caller we didn't consume the sample + + case 1: + // calculate with non null samples + doInterpolateFIR(&s); + sampleOut->setReal(-s.real()); + sampleOut->setImag(-s.imag()); + + // insert sample into ring double buffer + m_samples[m_ptr][0] = sampleIn->real(); + m_samples[m_ptr][1] = sampleIn->imag(); + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][0] = sampleIn->real(); + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][1] = sampleIn->imag(); + + // advance pointer + if (m_ptr < (HBFIRFilterTraits::hbOrder/2) - 1) { + m_ptr++; + } else { + m_ptr = 0; + } + + m_state = 2; // next state + return true; // tell caller we consumed the sample + + case 2: + // return the middle peak + sampleOut->setReal(m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][1]); // + imag + sampleOut->setImag(-m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][0]); // - real + m_state = 3; // next state + return false; // tell caller we didn't consume the sample + + default: + // calculate with non null samples + doInterpolateFIR(&s); + sampleOut->setReal(s.real()); + sampleOut->setImag(s.imag()); + + // insert sample into ring double buffer + m_samples[m_ptr][0] = sampleIn->real(); + m_samples[m_ptr][1] = sampleIn->imag(); + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][0] = sampleIn->real(); + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][1] = sampleIn->imag(); + + // advance pointer + if (m_ptr < (HBFIRFilterTraits::hbOrder/2) - 1) { + m_ptr++; + } else { + m_ptr = 0; + } + + m_state = 0; // next state + return true; // tell caller we consumed the sample + } + } + + void myDecimate(const Sample* sample1, Sample* sample2) + { + storeSample((FixReal) sample1->real(), (FixReal) sample1->imag()); + advancePointer(); + + storeSample((FixReal) sample2->real(), (FixReal) sample2->imag()); + doFIR(sample2); + advancePointer(); + } + + void myDecimate(qint64 x1, qint64 y1, qint64 *x2, qint64 *y2) + { + storeSample64(x1, y1); + advancePointer(); + + storeSample64(*x2, *y2); + doFIR(x2, y2); + advancePointer(); + } + + /** Simple zero stuffing and filter */ + void myInterpolateZeroStuffing(Sample* sample1, Sample* sample2) + { + storeSample((FixReal) sample1->real(), (FixReal) sample1->imag()); + doFIR(sample1); + advancePointer(); + + storeSample((FixReal) 0, (FixReal) 0); + doFIR(sample2); + advancePointer(); + } + + /** Simple zero stuffing and filter */ + void myInterpolateZeroStuffing(qint64 *x1, qint64 *y1, qint64 *x2, qint64 *y2) + { + storeSample64(*x1, *y1); + doFIR(x1, y1); + advancePointer(); + + storeSample64(0, 0); + doFIR(x2, y2); + advancePointer(); + } + + /** Optimized upsampler by 2 not calculating FIR with inserted null samples */ + void myInterpolate(qint32 *x1, qint32 *y1, qint32 *x2, qint32 *y2) + { + // insert sample into ring double buffer + m_samples[m_ptr][0] = *x1; + m_samples[m_ptr][1] = *y1; + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][0] = *x1; + m_samples[m_ptr + HBFIRFilterTraits::hbOrder/2][1] = *y1; + + // advance pointer + if (m_ptr < (HBFIRFilterTraits::hbOrder/2) - 1) { + m_ptr++; + } else { + m_ptr = 0; + } + + // first output sample calculated with the middle peak + *x1 = m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][0]; + *y1 = m_samples[m_ptr + (HBFIRFilterTraits::hbOrder/4) - 1][1]; + + // second sample calculated with the filter + doInterpolateFIR(x2, y2); + } + +protected: + qint64 m_even[2][HBFIRFilterTraits::hbOrder]; // double buffer technique + qint64 m_odd[2][HBFIRFilterTraits::hbOrder]; // double buffer technique + int32_t m_samples[HBFIRFilterTraits::hbOrder][2]; // double buffer technique + + int m_ptr; + int m_size; + int m_state; + + void storeSample(const FixReal& sampleI, const FixReal& sampleQ) + { + if ((m_ptr % 2) == 0) + { + m_even[0][m_ptr/2] = sampleI; + m_even[1][m_ptr/2] = sampleQ; + m_even[0][m_ptr/2 + m_size] = sampleI; + m_even[1][m_ptr/2 + m_size] = sampleQ; + } + else + { + m_odd[0][m_ptr/2] = sampleI; + m_odd[1][m_ptr/2] = sampleQ; + m_odd[0][m_ptr/2 + m_size] = sampleI; + m_odd[1][m_ptr/2 + m_size] = sampleQ; + } + } + + void storeSample32(int32_t x, int32_t y) + { + if ((m_ptr % 2) == 0) + { + m_even[0][m_ptr/2] = x; + m_even[1][m_ptr/2] = y; + m_even[0][m_ptr/2 + m_size] = x; + m_even[1][m_ptr/2 + m_size] = y; + } + else + { + m_odd[0][m_ptr/2] = x; + m_odd[1][m_ptr/2] = y; + m_odd[0][m_ptr/2 + m_size] = x; + m_odd[1][m_ptr/2 + m_size] = y; + } + } + + void storeSample64(qint64 x, qint64 y) + { + if ((m_ptr % 2) == 0) + { + m_even[0][m_ptr/2] = x; + m_even[1][m_ptr/2] = y; + m_even[0][m_ptr/2 + m_size] = x; + m_even[1][m_ptr/2 + m_size] = y; + } + else + { + m_odd[0][m_ptr/2] = x; + m_odd[1][m_ptr/2] = y; + m_odd[0][m_ptr/2 + m_size] = x; + m_odd[1][m_ptr/2 + m_size] = y; + } + } + + void advancePointer() + { + m_ptr = m_ptr + 1 < 2*m_size ? m_ptr + 1: 0; + } + + void doFIR(Sample* sample) + { + qint64 iAcc = 0; + qint64 qAcc = 0; + +//#if defined(USE_SSE4_1) && !defined(NO_DSP_SIMD) +// IntHalfbandFilterEO2Intrisics::work( +// m_ptr, +// m_even, +// m_odd, +// iAcc, +// qAcc +// ); +//#else + int a = m_ptr/2 + m_size; // tip pointer + int b = m_ptr/2 + 1; // tail pointer + + for (int i = 0; i < HBFIRFilterTraits::hbOrder / 4; i++) + { + if ((m_ptr % 2) == 0) + { + iAcc += (m_even[0][a] + m_even[0][b]) * HBFIRFilterTraits::hbCoeffs[i]; + qAcc += (m_even[1][a] + m_even[1][b]) * HBFIRFilterTraits::hbCoeffs[i]; + } + else + { + iAcc += (m_odd[0][a] + m_odd[0][b]) * HBFIRFilterTraits::hbCoeffs[i]; + qAcc += (m_odd[1][a] + m_odd[1][b]) * HBFIRFilterTraits::hbCoeffs[i]; + } + + a -= 1; + b += 1; + } +//#endif + + if ((m_ptr % 2) == 0) + { + iAcc += ((int32_t)m_odd[0][m_ptr/2 + m_size/2]) << (HBFIRFilterTraits::hbShift - 1); + qAcc += ((int32_t)m_odd[1][m_ptr/2 + m_size/2]) << (HBFIRFilterTraits::hbShift - 1); + } + else + { + iAcc += ((int32_t)m_even[0][m_ptr/2 + m_size/2 + 1]) << (HBFIRFilterTraits::hbShift - 1); + qAcc += ((int32_t)m_even[1][m_ptr/2 + m_size/2 + 1]) << (HBFIRFilterTraits::hbShift - 1); + } + + sample->setReal(iAcc >> (HBFIRFilterTraits::hbShift -1)); + sample->setImag(qAcc >> (HBFIRFilterTraits::hbShift -1)); + } + + void doFIR(qint64 *x, qint64 *y) + { + qint64 iAcc = 0; + qint64 qAcc = 0; + +//#if defined(USE_SSE4_1) && !defined(NO_DSP_SIMD) +// IntHalfbandFilterEO2Intrisics::work( +// m_ptr, +// m_even, +// m_odd, +// iAcc, +// qAcc +// ); +//#else + int a = m_ptr/2 + m_size; // tip pointer + int b = m_ptr/2 + 1; // tail pointer + + for (int i = 0; i < HBFIRFilterTraits::hbOrder / 4; i++) + { + if ((m_ptr % 2) == 0) + { + iAcc += (m_even[0][a] + m_even[0][b]) * HBFIRFilterTraits::hbCoeffs[i]; + qAcc += (m_even[1][a] + m_even[1][b]) * HBFIRFilterTraits::hbCoeffs[i]; + } + else + { + iAcc += (m_odd[0][a] + m_odd[0][b]) * HBFIRFilterTraits::hbCoeffs[i]; + qAcc += (m_odd[1][a] + m_odd[1][b]) * HBFIRFilterTraits::hbCoeffs[i]; + } + + a -= 1; + b += 1; + } +//#endif + if ((m_ptr % 2) == 0) + { + iAcc += ((int32_t)m_odd[0][m_ptr/2 + m_size/2]) << (HBFIRFilterTraits::hbShift - 1); + qAcc += ((int32_t)m_odd[1][m_ptr/2 + m_size/2]) << (HBFIRFilterTraits::hbShift - 1); + } + else + { + iAcc += ((int32_t)m_even[0][m_ptr/2 + m_size/2 + 1]) << (HBFIRFilterTraits::hbShift - 1); + qAcc += ((int32_t)m_even[1][m_ptr/2 + m_size/2 + 1]) << (HBFIRFilterTraits::hbShift - 1); + } + + *x = iAcc >> (HBFIRFilterTraits::hbShift -1); // HB_SHIFT incorrect do not loose the gained bit + *y = qAcc >> (HBFIRFilterTraits::hbShift -1); + } + + void doInterpolateFIR(Sample* sample) + { + qint64 iAcc = 0; + qint64 qAcc = 0; + + qint16 a = m_ptr; + qint16 b = m_ptr + (HBFIRFilterTraits::hbOrder / 2) - 1; + + // go through samples in buffer + for (int i = 0; i < HBFIRFilterTraits::hbOrder / 4; i++) + { + iAcc += (m_samples[a][0] + m_samples[b][0]) * HBFIRFilterTraits::hbCoeffs[i]; + qAcc += (m_samples[a][1] + m_samples[b][1]) * HBFIRFilterTraits::hbCoeffs[i]; + a++; + b--; + } + + sample->setReal(iAcc >> (HBFIRFilterTraits::hbShift -1)); + sample->setImag(qAcc >> (HBFIRFilterTraits::hbShift -1)); + } + + void doInterpolateFIR(qint32 *x, qint32 *y) + { + qint64 iAcc = 0; + qint64 qAcc = 0; + + qint16 a = m_ptr; + qint16 b = m_ptr + (HBFIRFilterTraits::hbOrder / 2) - 1; + + // go through samples in buffer + for (int i = 0; i < HBFIRFilterTraits::hbOrder / 4; i++) + { + iAcc += (m_samples[a][0] + m_samples[b][0]) * HBFIRFilterTraits::hbCoeffs[i]; + qAcc += (m_samples[a][1] + m_samples[b][1]) * HBFIRFilterTraits::hbCoeffs[i]; + a++; + b--; + } + + *x = iAcc >> (HBFIRFilterTraits::hbShift -1); + *y = qAcc >> (HBFIRFilterTraits::hbShift -1); + } +}; + +template +IntHalfbandFilterEO2::IntHalfbandFilterEO2() +{ + m_size = HBFIRFilterTraits::hbOrder/2; + + for (int i = 0; i < 2*m_size; i++) + { + m_even[0][i] = 0; + m_even[1][i] = 0; + m_odd[0][i] = 0; + m_odd[1][i] = 0; + m_samples[i][0] = 0; + m_samples[i][1] = 0; + } + + m_ptr = 0; + m_state = 0; +} + +#endif /* SDRBASE_DSP_INTHALFBANDFILTEREO2_H_ */