This commit is contained in:
Ryzerth 2020-06-15 16:11:22 +02:00
parent fa2f6a0a9b
commit 61ba7f1420
4 changed files with 306 additions and 0 deletions

75
src/cdsp/audio.h Normal file
View File

@ -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;
};
};

84
src/cdsp/fast_math.h Normal file
View File

@ -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;
}
}

104
src/vfo.cpp Normal file
View File

@ -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;
}

43
src/vfo.h Normal file
View File

@ -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;
};