Merge pull request #52 from zakrent/master

Added SIMD to polyphase resampler
This commit is contained in:
AlexandreRouma 2020-12-26 23:08:45 +01:00 committed by GitHub
commit 9bbf634f5d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23

View File

@ -16,6 +16,7 @@ namespace dsp {
generic_block<PolyphaseResampler<T>>::stop();
volk_free(buffer);
volk_free(taps);
freeTapPhases();
}
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());
_window->createTaps(taps, tapCount, _interp);
buildTapPhases();
buffer = (T*)volk_malloc(STREAM_BUFFER_SIZE * sizeof(T) * 2, volk_get_alignment());
memset(buffer, 0, STREAM_BUFFER_SIZE * sizeof(T) * 2);
bufStart = &buffer[tapCount];
generic_block<PolyphaseResampler<T>>::registerInput(_in);
generic_block<PolyphaseResampler<T>>::registerOutput(&out);
}
@ -55,6 +57,7 @@ namespace dsp {
int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate);
_interp = _outSampleRate / _gcd;
_decim = _inSampleRate / _gcd;
buildTapPhases();
generic_block<PolyphaseResampler<T>>::tempStart();
}
@ -65,6 +68,7 @@ namespace dsp {
int _gcd = std::gcd((int)_inSampleRate, (int)_outSampleRate);
_interp = _outSampleRate / _gcd;
_decim = _inSampleRate / _gcd;
buildTapPhases();
generic_block<PolyphaseResampler<T>>::tempStart();
}
@ -84,7 +88,7 @@ namespace dsp {
tapCount = window->getTapCount();
taps = (float*)volk_malloc(tapCount * sizeof(float), volk_get_alignment());
window->createTaps(taps, tapCount, _interp);
bufStart = &buffer[tapCount];
buildTapPhases();
generic_block<PolyphaseResampler<T>>::tempStart();
}
@ -100,34 +104,28 @@ namespace dsp {
int outCount = calcOutSize(count);
memcpy(&buffer[tapCount], _in->readBuf, count * sizeof(T));
memcpy(&buffer[tapsPerPhase], _in->readBuf, count * sizeof(T));
_in->flush();
// Write to output
int outIndex = 0;
if constexpr (std::is_same_v<T, float>) {
for (int i = 0; outIndex < outCount; i += _decim) {
out.writeBuf[outIndex] = 0;
for (int j = i % _interp; j < tapCount; j += _interp) {
out.writeBuf[outIndex] += buffer[((i - j) / _interp) + tapCount] * taps[j];
}
int phase = i % _interp;
volk_32f_x2_dot_prod_32f(&out.writeBuf[outIndex], &buffer[i / _interp], tapPhases[phase], tapsPerPhase);
outIndex++;
}
}
if constexpr (std::is_same_v<T, complex_t>) {
for (int i = 0; outIndex < outCount; i += _decim) {
out.writeBuf[outIndex].i = 0;
out.writeBuf[outIndex].q = 0;
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];
}
int phase = i % _interp;
volk_32fc_32f_dot_prod_32fc((lv_32fc_t*)&out.writeBuf[outIndex], (lv_32fc_t*)&buffer[(i / _interp)], tapPhases[phase], tapsPerPhase);
outIndex++;
}
}
if (!out.swap(outCount)) { return -1; }
memmove(buffer, &buffer[count], tapCount * sizeof(T));
memmove(buffer, &buffer[count], tapsPerPhase * sizeof(T));
return count;
}
@ -135,6 +133,44 @@ namespace dsp {
stream<T> out;
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;
stream<T>* _in;
@ -147,5 +183,8 @@ namespace dsp {
float _inSampleRate, _outSampleRate;
float* taps;
int tapsPerPhase;
std::vector<float*> tapPhases;
};
}