:Added SIMD to polyphase resampler

This commit is contained in:
zakrent 2020-12-26 21:33:33 +01:00
parent 09498f3b18
commit 69bcbf6f27

View File

@ -16,6 +16,7 @@ namespace dsp {
generic_block<PolyphaseResampler<T>>::stop(); generic_block<PolyphaseResampler<T>>::stop();
volk_free(buffer); volk_free(buffer);
volk_free(taps); volk_free(taps);
freeTapPhases();
} }
void init(stream<T>* in, dsp::filter_window::generic_window* window, float inSampleRate, float outSampleRate) { void init(stream<T>* in, dsp::filter_window::generic_window* window, float inSampleRate, float outSampleRate) {
@ -32,9 +33,10 @@ namespace dsp {
taps = (float*)volk_malloc(tapCount * sizeof(float), volk_get_alignment()); taps = (float*)volk_malloc(tapCount * sizeof(float), volk_get_alignment());
_window->createTaps(taps, tapCount, _interp); _window->createTaps(taps, tapCount, _interp);
buildTapPhases();
buffer = (T*)volk_malloc(STREAM_BUFFER_SIZE * sizeof(T) * 2, volk_get_alignment()); buffer = (T*)volk_malloc(STREAM_BUFFER_SIZE * sizeof(T) * 2, volk_get_alignment());
memset(buffer, 0, STREAM_BUFFER_SIZE * sizeof(T) * 2); memset(buffer, 0, STREAM_BUFFER_SIZE * sizeof(T) * 2);
bufStart = &buffer[tapCount];
generic_block<PolyphaseResampler<T>>::registerInput(_in); generic_block<PolyphaseResampler<T>>::registerInput(_in);
generic_block<PolyphaseResampler<T>>::registerOutput(&out); generic_block<PolyphaseResampler<T>>::registerOutput(&out);
} }
@ -55,6 +57,7 @@ namespace dsp {
int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate); int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate);
_interp = _outSampleRate / _gcd; _interp = _outSampleRate / _gcd;
_decim = _inSampleRate / _gcd; _decim = _inSampleRate / _gcd;
buildTapPhases();
generic_block<PolyphaseResampler<T>>::tempStart(); generic_block<PolyphaseResampler<T>>::tempStart();
} }
@ -65,6 +68,7 @@ namespace dsp {
int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate); int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate);
_interp = _outSampleRate / _gcd; _interp = _outSampleRate / _gcd;
_decim = _inSampleRate / _gcd; _decim = _inSampleRate / _gcd;
buildTapPhases();
generic_block<PolyphaseResampler<T>>::tempStart(); generic_block<PolyphaseResampler<T>>::tempStart();
} }
@ -84,7 +88,7 @@ namespace dsp {
tapCount = window->getTapCount(); tapCount = window->getTapCount();
taps = (float*)volk_malloc(tapCount * sizeof(float), volk_get_alignment()); taps = (float*)volk_malloc(tapCount * sizeof(float), volk_get_alignment());
window->createTaps(taps, tapCount, _interp); window->createTaps(taps, tapCount, _interp);
bufStart = &buffer[tapCount]; buildTapPhases();
generic_block<PolyphaseResampler<T>>::tempStart(); generic_block<PolyphaseResampler<T>>::tempStart();
} }
@ -100,34 +104,28 @@ namespace dsp {
int outCount = calcOutSize(count); int outCount = calcOutSize(count);
memcpy(&buffer[tapCount], _in->readBuf, count * sizeof(T)); memcpy(&buffer[tapsPerPhase], _in->readBuf, count * sizeof(T));
_in->flush(); _in->flush();
// Write to output // Write to output
int outIndex = 0; int outIndex = 0;
if constexpr (std::is_same_v<T, float>) { if constexpr (std::is_same_v<T, float>) {
for (int i = 0; outIndex < outCount; i += _decim) { for (int i = 0; outIndex < outCount; i += _decim) {
out.writeBuf[outIndex] = 0; int phase = i % _interp;
for (int j = i % _interp; j < tapCount; j += _interp) { volk_32f_x2_dot_prod_32f(&out.writeBuf[outIndex], &buffer[i / _interp], tapPhases[phase], tapsPerPhase);
out.writeBuf[outIndex] += buffer[((i - j) / _interp) + tapCount] * taps[j];
}
outIndex++; outIndex++;
} }
} }
if constexpr (std::is_same_v<T, complex_t>) { if constexpr (std::is_same_v<T, complex_t>) {
for (int i = 0; outIndex < outCount; i += _decim) { for (int i = 0; outIndex < outCount; i += _decim) {
out.writeBuf[outIndex].i = 0; int phase = i % _interp;
out.writeBuf[outIndex].q = 0; volk_32fc_32f_dot_prod_32fc((lv_32fc_t*)&out.writeBuf[outIndex], (lv_32fc_t*)&buffer[(i / _interp)], tapPhases[phase], tapsPerPhase);
for (int j = i % _interp; j < tapCount; j += _interp) {
out.writeBuf[outIndex].i += buffer[((i - j) / _interp) + tapCount].i * taps[j];
out.writeBuf[outIndex].q += buffer[((i - j) / _interp) + tapCount].q * taps[j];
}
outIndex++; outIndex++;
} }
} }
if (!out.swap(outCount)) { return -1; } if (!out.swap(outCount)) { return -1; }
memmove(buffer, &buffer[count], tapCount * sizeof(T)); memmove(buffer, &buffer[count], tapsPerPhase * sizeof(T));
return count; return count;
} }
@ -135,6 +133,44 @@ namespace dsp {
stream<T> out; stream<T> out;
private: private:
void buildTapPhases(){
if(!taps){
return;
}
if(!tapPhases.empty()){
freeTapPhases();
}
int phases = _interp;
tapsPerPhase = (tapCount+phases-1)/phases; //Integer division ceiling
bufStart = &buffer[tapsPerPhase];
for(int i = 0; i < phases; i++){
tapPhases.push_back((float*)volk_malloc(tapsPerPhase * sizeof(float), volk_get_alignment()));
}
int currentTap = 0;
for(int tap = 0; tap < tapsPerPhase; tap++) {
for (int phase = 0; phase < phases; phase++) {
if(currentTap < tapCount) {
tapPhases[phase][tap] = taps[currentTap++];
}
else{
tapPhases[phase][tap] = 0;
}
}
}
}
void freeTapPhases(){
for(auto & tapPhase : tapPhases){
volk_free(tapPhase);
}
tapPhases.clear();
}
int count; int count;
stream<T>* _in; stream<T>* _in;
@ -147,5 +183,8 @@ namespace dsp {
float _inSampleRate, _outSampleRate; float _inSampleRate, _outSampleRate;
float* taps; float* taps;
int tapsPerPhase;
std::vector<float*> tapPhases;
}; };
} }