diff --git a/src/cdsp/audio.h b/src/cdsp/audio.h new file mode 100644 index 0000000..bc82ee1 --- /dev/null +++ b/src/cdsp/audio.h @@ -0,0 +1,75 @@ +#pragma once +#include +#include +#include +#include +#include + +namespace cdsp { + class AudioSink { + public: + AudioSink() { + + } + + AudioSink(stream* in, int bufferSize) { + _bufferSize = bufferSize; + _input = in; + buffer = new float[_bufferSize * 2]; + _volume = 1.0f; + Pa_Initialize(); + } + + void init(stream* in, int bufferSize) { + _bufferSize = bufferSize; + _input = in; + buffer = new float[_bufferSize * 2]; + _volume = 1.0f; + Pa_Initialize(); + } + + void setVolume(float volume) { + _volume = volume; + } + + void start() { + PaStreamParameters outputParams; + outputParams.channelCount = 2; + outputParams.sampleFormat = paFloat32; + outputParams.hostApiSpecificStreamInfo = NULL; + outputParams.device = Pa_GetDefaultOutputDevice(); + outputParams.suggestedLatency = Pa_GetDeviceInfo(outputParams.device)->defaultLowOutputLatency; + PaError err = Pa_OpenStream(&stream, NULL, &outputParams, 40000.0f, 320, paClipOff, _callback, this); + printf("%s\n", Pa_GetErrorText(err)); + err = Pa_StartStream(stream); + printf("%s\n", Pa_GetErrorText(err)); + } + + void stop() { + Pa_CloseStream(stream); + } + + private: + static int _callback(const void *input, + void *output, + unsigned long frameCount, + const PaStreamCallbackTimeInfo* timeInfo, + PaStreamCallbackFlags statusFlags, void *userData ) { + AudioSink* _this = (AudioSink*)userData; + float* outbuf = (float*)output; + _this->_input->read(_this->buffer, frameCount); + float vol = powf(_this->_volume, 2); + for (int i = 0; i < frameCount; i++) { + outbuf[(i * 2) + 0] = _this->buffer[i] * vol; + outbuf[(i * 2) + 1] = _this->buffer[i] * vol; + } + return 0; + } + + int _bufferSize; + stream* _input; + float* buffer; + float _volume; + PaStream *stream; + }; +}; \ No newline at end of file diff --git a/src/cdsp/fast_math.h b/src/cdsp/fast_math.h new file mode 100644 index 0000000..eb08e9e --- /dev/null +++ b/src/cdsp/fast_math.h @@ -0,0 +1,84 @@ +#ifdef _MSC_VER +#include +#else +#include +#endif + +#include + + +inline void cm_mul(__m128& ab, const __m128& xy) +{ + //const __m128 aa = _mm_shuffle_ps(ab, ab, _MM_SHUFFLE(2, 2, 0, 0)); + const __m128 aa = _mm_moveldup_ps(ab); + const __m128 bb = _mm_movehdup_ps(ab); + //const __m128 bb = _mm_shuffle_ps(ab, ab, _MM_SHUFFLE(3, 3, 1, 1)); + const __m128 yx = _mm_shuffle_ps(xy, xy, _MM_SHUFFLE(2, 3, 0, 1)); + + const __m128 tmp = _mm_addsub_ps(_mm_mul_ps(aa, xy), _mm_mul_ps(bb, yx)); + ab = tmp; +} + +inline void do_mul(cdsp::complex_t* a, const cdsp::complex_t* b, int n) +{ + const int vector_size = 16; + int simd_iterations = n - (n % vector_size); + //assert(simd_iterations % vector_size == 0); + + for (int i = 0; i < simd_iterations; i += vector_size) + { + //__builtin_prefetch(a + i*4 + 64, 0); + //__builtin_prefetch(b + i*4 + 64, 0); + + __m128 vec_a = _mm_load_ps((float*)&a[i]); + __m128 vec_b = _mm_load_ps((float*)&b[i]); + + __m128 vec_a2 = _mm_load_ps((float*)&a[i+2]); + __m128 vec_b2 = _mm_load_ps((float*)&b[i+2]); + + __m128 vec_a3 = _mm_load_ps((float*)&a[i+4]); + __m128 vec_b3 = _mm_load_ps((float*)&b[i+4]); + + __m128 vec_a4 = _mm_load_ps((float*)&a[i+6]); + __m128 vec_b4 = _mm_load_ps((float*)&b[i+6]); + + __m128 vec_a5 = _mm_load_ps((float*)&a[i+8]); + __m128 vec_b5 = _mm_load_ps((float*)&b[i+8]); + + __m128 vec_a6 = _mm_load_ps((float*)&a[i+10]); + __m128 vec_b6 = _mm_load_ps((float*)&b[i+10]); + + __m128 vec_a7 = _mm_load_ps((float*)&a[i+12]); + __m128 vec_b7 = _mm_load_ps((float*)&b[i+12]); + + __m128 vec_a8 = _mm_load_ps((float*)&a[i+14]); + __m128 vec_b8 = _mm_load_ps((float*)&b[i+14]); + + cm_mul(vec_a, vec_b); + _mm_store_ps((float*)&a[i], vec_a); + cm_mul(vec_a2, vec_b2); + _mm_store_ps((float*)&a[i+2], vec_a2); + cm_mul(vec_a3, vec_b3); + _mm_store_ps((float*)&a[i+4], vec_a3); + cm_mul(vec_a4, vec_b4); + _mm_store_ps((float*)&a[i+6], vec_a4); + + cm_mul(vec_a5, vec_b5); + _mm_store_ps((float*)&a[i+8], vec_a5); + cm_mul(vec_a6, vec_b6); + _mm_store_ps((float*)&a[i+10], vec_a6); + cm_mul(vec_a7, vec_b7); + _mm_store_ps((float*)&a[i+12], vec_a7); + cm_mul(vec_a8, vec_b8); + _mm_store_ps((float*)&a[i+14], vec_a8); + } + + // finish with scalar + for (int i = simd_iterations; i < n; i++) + { + cdsp::complex_t cm; + cm.q = a[i].q*b[i].q - a[i].i*b[i].i; + cm.i = a[i].q*b[i].i + b[i].q*a[i].i; + a[i] = cm; + } +} \ No newline at end of file diff --git a/src/vfo.cpp b/src/vfo.cpp new file mode 100644 index 0000000..5611fdc --- /dev/null +++ b/src/vfo.cpp @@ -0,0 +1,104 @@ +#include +#include + +VFO::VFO() { + +} + +void VFO::init(cdsp::stream* input, float offset, float inputSampleRate, float bandWidth, int bufferSize) { + _input = input; + outputSampleRate = ceilf(bandWidth / OUTPUT_SR_ROUND) * OUTPUT_SR_ROUND; + _inputSampleRate = inputSampleRate; + int _gcd = std::gcd((int)inputSampleRate, (int)outputSampleRate); + _interp = outputSampleRate / _gcd; + _decim = inputSampleRate / _gcd; + _bandWidth = bandWidth; + _bufferSize = bufferSize; + lo.init(offset, inputSampleRate, bufferSize); + mixer.init(&lo.output, input, bufferSize); + interp.init(&mixer.output, _interp, bufferSize); + + BlackmanWindow(decimTaps, inputSampleRate * _interp, bandWidth / 2.0f, bandWidth / 2.0f); + + if (_interp != 1) { + printf("Interpolation needed\n"); + decFir.init(&interp.output, decimTaps, bufferSize * _interp, _decim); + } + else { + decFir.init(&mixer.output, decimTaps, bufferSize, _decim); + printf("Interpolation NOT needed: %d %d %d\n", bufferSize / _decim, _decim, _interp); + } + + output = &decFir.output; +} + + +void VFO::start() { + lo.start(); + mixer.start(); + if (_interp != 1) { + interp.start(); + } + decFir.start(); +} + +void VFO::stop() { + // TODO: Stop LO + mixer.stop(); + interp.stop(); + decFir.stop(); +} + + +void VFO::setOffset(float freq) { + lo.setFrequency(-freq); +} + +void VFO::setBandwidth(float bandWidth) { + if (bandWidth == _bandWidth) { + return; + } + outputSampleRate = ceilf(bandWidth / OUTPUT_SR_ROUND) * OUTPUT_SR_ROUND; + int _gcd = std::gcd((int)_inputSampleRate, (int)outputSampleRate); + int interpol = outputSampleRate / _gcd; + int decim = _inputSampleRate / _gcd; + _bandWidth = bandWidth; + + BlackmanWindow(decimTaps, _inputSampleRate * _interp, bandWidth / 2, bandWidth); + + decFir.stop(); + decFir.setTaps(decimTaps); + decFir.setDecimation(decim); + + if (interpol != _interp) { + interp.stop(); + if (interpol == 1) { + decFir.setBufferSize(_bufferSize); + decFir.setInput(&mixer.output); + } + else if (_interp == 1) { + decFir.setInput(&interp.output); + decFir.setBufferSize(_bufferSize * _interp); + interp.setInterpolation(interpol); + interp.start(); + } + else { + decFir.setBufferSize(_bufferSize * _interp); + interp.setInterpolation(interpol); + interp.start(); + } + } + + _interp = interpol; + _decim = decim; + + decFir.start(); +} + +void VFO::setSampleRate(int sampleRate) { + +} + +int VFO::getOutputSampleRate() { + return outputSampleRate; +} \ No newline at end of file diff --git a/src/vfo.h b/src/vfo.h new file mode 100644 index 0000000..9f9c9cd --- /dev/null +++ b/src/vfo.h @@ -0,0 +1,43 @@ +#pragma once +#include +#include +#include +#include + +// Round up to next 5KHz multiple frequency +#define OUTPUT_SR_ROUND 5000.0f + +class VFO { +public: + VFO(); + void init(cdsp::stream* input, float offset, float sampleRate, float bandWidth, int bufferSize); + + void start(); + void stop(); + + void setOffset(float freq); + void setBandwidth(float bandwidth); + void setSampleRate(int sampleRate); + + int getOutputSampleRate(); + + cdsp::stream* output; + +private: + cdsp::ComplexSineSource lo; + cdsp::Multiplier mixer; + cdsp::IQInterpolator interp; + cdsp::DecimatingFIRFilter decFir; + + std::vector decimTaps; + + int _interp; + int _decim; + float _inputSampleRate; + float _outputSampleRate; + float _bandWidth; + int _bufferSize; + int outputSampleRate; + + cdsp::stream* _input; +}; \ No newline at end of file