test

src/cdsp/audio.h

@@ -0,0 +1,75 @@
+#pragma once
+#include <thread>
+#include <cdsp/stream.h>
+#include <cdsp/types.h>
+#include <fstream>
+#include <portaudio.h>
+
+namespace cdsp {
+    class AudioSink {
+    public:
+        AudioSink() {
+            
+        }
+
+        AudioSink(stream<float>* in, int bufferSize) {
+            _bufferSize = bufferSize;
+            _input = in;
+            buffer = new float[_bufferSize * 2];
+            _volume = 1.0f;
+            Pa_Initialize();
+        }
+
+        void init(stream<float>* 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<float>* _input;
+        float* buffer;
+        float _volume;
+        PaStream *stream;
+    };
+};

src/cdsp/fast_math.h

@@ -0,0 +1,84 @@
+#ifdef _MSC_VER
+#include <tmmintrin.h>
+#else
+#include <x86intrin.h>
+#endif
+
+#include <cdsp/types.h>
+
+
+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;
+    }
+}

src/vfo.cpp

@@ -0,0 +1,104 @@
+#include <vfo.h>
+#include <numeric>
+
+VFO::VFO() {
+
+}
+
+void VFO::init(cdsp::stream<cdsp::complex_t>* 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;
+}

src/vfo.h

@@ -0,0 +1,43 @@
+#pragma once
+#include <cdsp/math.h>
+#include <cdsp/generator.h>
+#include <cdsp/resampling.h>
+#include <cdsp/filter.h>
+
+// Round up to next 5KHz multiple frequency
+#define OUTPUT_SR_ROUND 5000.0f
+
+class VFO {
+public:
+    VFO();
+    void init(cdsp::stream<cdsp::complex_t>* 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<cdsp::complex_t>* output;
+
+private:
+    cdsp::ComplexSineSource lo;
+    cdsp::Multiplier mixer;
+    cdsp::IQInterpolator interp;
+    cdsp::DecimatingFIRFilter decFir;
+
+    std::vector<float> decimTaps;
+
+    int _interp;
+    int _decim;
+    float _inputSampleRate;
+    float _outputSampleRate;
+    float _bandWidth;
+    int _bufferSize;
+    int outputSampleRate;
+    
+    cdsp::stream<cdsp::complex_t>* _input;
+};