even more stuff
This commit is contained in:
AlexandreRouma
18
core/src/dsp/buffer/buffer.h
Normal file
18
core/src/dsp/buffer/buffer.h
Normal file
@@ -0,0 +1,18 @@
|
||||
#pragma once
|
||||
#include <volk/volk.h>
|
||||
|
||||
namespace dsp::buffer {
|
||||
template<class T>
|
||||
inline T* alloc(int count) {
|
||||
return (T*)volk_malloc(count * sizeof(T), volk_get_alignment());
|
||||
}
|
||||
|
||||
template<class T>
|
||||
inline void clear(T* buffer, int count, int offset = 0) {
|
||||
memset(&buffer[offset], 0, count * sizeof(T));
|
||||
}
|
||||
|
||||
inline void free(void* buffer) {
|
||||
volk_free(buffer);
|
||||
}
|
||||
}
|
||||
136
core/src/dsp/buffer/frame_buffer.h
Normal file
136
core/src/dsp/buffer/frame_buffer.h
Normal file
@@ -0,0 +1,136 @@
|
||||
#pragma once
|
||||
#include "../block.h"
|
||||
#define TEST_BUFFER_SIZE 32
|
||||
|
||||
// IMPORTANT: THIS IS TRASH AND MUST BE REWRITTEN IN THE FUTURE
|
||||
|
||||
namespace dsp::buffer {
|
||||
template <class T>
|
||||
class SampleFrameBuffer : public block {
|
||||
using base_type = block;
|
||||
public:
|
||||
SampleFrameBuffer() {}
|
||||
|
||||
SampleFrameBuffer(stream<T>* in) { init(in); }
|
||||
|
||||
~SampleFrameBuffer() {
|
||||
if (!base_type::_block_init) { return; }
|
||||
base_type::stop();
|
||||
for (int i = 0; i < TEST_BUFFER_SIZE; i++) {
|
||||
buffer::free(buffers[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void init(stream<T>* in) {
|
||||
_in = in;
|
||||
|
||||
for (int i = 0; i < TEST_BUFFER_SIZE; i++) {
|
||||
buffers[i] = buffer::alloc<T>(STREAM_BUFFER_SIZE);
|
||||
}
|
||||
|
||||
base_type::registerInput(in);
|
||||
base_type::registerOutput(&out);
|
||||
base_type::_block_init = true;
|
||||
}
|
||||
|
||||
void setInput(stream<T>* in) {
|
||||
assert(base_type::_block_init);
|
||||
std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
|
||||
base_type::tempStop();
|
||||
base_type::unregisterInput(_in);
|
||||
_in = in;
|
||||
base_type::registerInput(_in);
|
||||
base_type::tempStart();
|
||||
}
|
||||
|
||||
void flush() {
|
||||
std::unique_lock lck(bufMtx);
|
||||
readCur = writeCur;
|
||||
}
|
||||
|
||||
int run() {
|
||||
// Wait for data
|
||||
int count = _in->read();
|
||||
if (count < 0) { return -1; }
|
||||
|
||||
if (bypass) {
|
||||
memcpy(out.writeBuf, _in->readBuf, count * sizeof(T));
|
||||
_in->flush();
|
||||
if (!out.swap(count)) { return -1; }
|
||||
return count;
|
||||
}
|
||||
|
||||
// Push it on the ring buffer
|
||||
{
|
||||
std::lock_guard<std::mutex> lck(bufMtx);
|
||||
memcpy(buffers[writeCur], _in->readBuf, count * sizeof(T));
|
||||
sizes[writeCur] = count;
|
||||
writeCur++;
|
||||
writeCur = ((writeCur) % TEST_BUFFER_SIZE);
|
||||
|
||||
// if (((writeCur - readCur + TEST_BUFFER_SIZE) % TEST_BUFFER_SIZE) >= (TEST_BUFFER_SIZE-2)) {
|
||||
// spdlog::warn("Overflow");
|
||||
// }
|
||||
}
|
||||
cnd.notify_all();
|
||||
_in->flush();
|
||||
return count;
|
||||
}
|
||||
|
||||
void worker() {
|
||||
while (true) {
|
||||
// Wait for data
|
||||
std::unique_lock lck(bufMtx);
|
||||
cnd.wait(lck, [this]() { return (((writeCur - readCur + TEST_BUFFER_SIZE) % TEST_BUFFER_SIZE) > 0) || stopWorker; });
|
||||
if (stopWorker) { break; }
|
||||
|
||||
// Write one to output buffer and unlock in preparation to swap buffers
|
||||
int count = sizes[readCur];
|
||||
memcpy(out.writeBuf, buffers[readCur], count * sizeof(T));
|
||||
readCur++;
|
||||
readCur = ((readCur) % TEST_BUFFER_SIZE);
|
||||
lck.unlock();
|
||||
|
||||
// Swap
|
||||
if (!out.swap(count)) { break; }
|
||||
}
|
||||
}
|
||||
|
||||
stream<T> out;
|
||||
|
||||
int writeCur = 0;
|
||||
int readCur = 0;
|
||||
|
||||
bool bypass = false;
|
||||
|
||||
private:
|
||||
void doStart() {
|
||||
base_type::workerThread = std::thread(&SampleFrameBuffer<T>::workerLoop, this);
|
||||
readWorkerThread = std::thread(&SampleFrameBuffer<T>::worker, this);
|
||||
}
|
||||
|
||||
void doStop() {
|
||||
_in->stopReader();
|
||||
out.stopWriter();
|
||||
stopWorker = true;
|
||||
cnd.notify_all();
|
||||
|
||||
if (base_type::workerThread.joinable()) { base_type::workerThread.join(); }
|
||||
if (readWorkerThread.joinable()) { readWorkerThread.join(); }
|
||||
|
||||
_in->clearReadStop();
|
||||
out.clearWriteStop();
|
||||
stopWorker = false;
|
||||
}
|
||||
|
||||
stream<T>* _in;
|
||||
|
||||
std::thread readWorkerThread;
|
||||
std::mutex bufMtx;
|
||||
std::condition_variable cnd;
|
||||
T* buffers[TEST_BUFFER_SIZE];
|
||||
int sizes[TEST_BUFFER_SIZE];
|
||||
|
||||
bool stopWorker = false;
|
||||
};
|
||||
}
|
||||
69
core/src/dsp/buffer/packer.h
Normal file
69
core/src/dsp/buffer/packer.h
Normal file
@@ -0,0 +1,69 @@
|
||||
#include "../block.h"
|
||||
|
||||
|
||||
namespace dsp::buffer {
|
||||
template <class T>
|
||||
class Packer : public block {
|
||||
public:
|
||||
Packer() {}
|
||||
|
||||
Packer(stream<T>* in, int count) { init(in, count); }
|
||||
|
||||
void init(stream<T>* in, int count) {
|
||||
_in = in;
|
||||
samples = count;
|
||||
block::registerInput(_in);
|
||||
block::registerOutput(&out);
|
||||
block::_block_init = true;
|
||||
}
|
||||
|
||||
void setInput(stream<T>* in) {
|
||||
assert(block::_block_init);
|
||||
std::lock_guard<std::recursive_mutex> lck(block::ctrlMtx);
|
||||
block::tempStop();
|
||||
block::unregisterInput(_in);
|
||||
_in = in;
|
||||
block::registerInput(_in);
|
||||
block::tempStart();
|
||||
}
|
||||
|
||||
void setSampleCount(int count) {
|
||||
assert(block::_block_init);
|
||||
std::lock_guard<std::recursive_mutex> lck(block::ctrlMtx);
|
||||
block::tempStop();
|
||||
samples = count;
|
||||
block::tempStart();
|
||||
}
|
||||
|
||||
int run() {
|
||||
int count = _in->read();
|
||||
if (count < 0) {
|
||||
read = 0;
|
||||
return -1;
|
||||
}
|
||||
|
||||
for (int i = 0; i < count; i++) {
|
||||
out.writeBuf[read++] = _in->readBuf[i];
|
||||
if (read >= samples) {
|
||||
read = 0;
|
||||
if (!out.swap(samples)) {
|
||||
_in->flush();
|
||||
read = 0;
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_in->flush();
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
stream<T> out;
|
||||
|
||||
private:
|
||||
int samples = 1;
|
||||
int read = 0;
|
||||
stream<T>* _in;
|
||||
};
|
||||
}
|
||||
137
core/src/dsp/buffer/reshaper.h
Normal file
137
core/src/dsp/buffer/reshaper.h
Normal file
@@ -0,0 +1,137 @@
|
||||
#pragma once
|
||||
#include "../block.h"
|
||||
#include "ring_buffer.h"
|
||||
|
||||
// IMPORTANT: THIS IS TRASH AND MUST BE REWRITTEN IN THE FUTURE
|
||||
|
||||
namespace dsp::buffer {
|
||||
// NOTE: I'm not proud of this, it's BAD and just taken from the previous DSP, but it works...
|
||||
template <class T>
|
||||
class Reshaper : public block {
|
||||
using base_type = block;
|
||||
public:
|
||||
Reshaper() {}
|
||||
|
||||
Reshaper(stream<T>* in, int keep, int skip) { init(in, keep, skip); }
|
||||
|
||||
// NOTE: For some reason, the base class destructor doesn't get called.... this is a temporary fix I guess
|
||||
// I also don't check for _block_init for the exact sample reason, something's weird
|
||||
~Reshaper() {
|
||||
if (!base_type::_block_init) { return; }
|
||||
base_type::stop();
|
||||
}
|
||||
|
||||
void init(stream<T>* in, int keep, int skip) {
|
||||
_in = in;
|
||||
_keep = keep;
|
||||
_skip = skip;
|
||||
ringBuf.init(keep * 2);
|
||||
base_type::registerInput(_in);
|
||||
base_type::registerOutput(&out);
|
||||
base_type::_block_init = true;
|
||||
}
|
||||
|
||||
void setInput(stream<T>* in) {
|
||||
assert(base_type::_block_init);
|
||||
std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
|
||||
base_type::tempStop();
|
||||
base_type::unregisterInput(_in);
|
||||
_in = in;
|
||||
base_type::registerInput(_in);
|
||||
base_type::tempStart();
|
||||
}
|
||||
|
||||
void setKeep(int keep) {
|
||||
assert(base_type::_block_init);
|
||||
std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
|
||||
base_type::tempStop();
|
||||
_keep = keep;
|
||||
ringBuf.setMaxLatency(keep * 2);
|
||||
base_type::tempStart();
|
||||
}
|
||||
|
||||
void setSkip(int skip) {
|
||||
assert(base_type::_block_init);
|
||||
std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
|
||||
base_type::tempStop();
|
||||
_skip = skip;
|
||||
base_type::tempStart();
|
||||
}
|
||||
|
||||
int run() {
|
||||
int count = _in->read();
|
||||
if (count < 0) { return -1; }
|
||||
ringBuf.write(_in->readBuf, count);
|
||||
_in->flush();
|
||||
return count;
|
||||
}
|
||||
|
||||
stream<T> out;
|
||||
|
||||
private:
|
||||
void doStart() override {
|
||||
workThread = std::thread(&Reshaper<T>::loop, this);
|
||||
bufferWorkerThread = std::thread(&Reshaper<T>::bufferWorker, this);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
while (run() >= 0)
|
||||
;
|
||||
}
|
||||
|
||||
void doStop() override {
|
||||
_in->stopReader();
|
||||
ringBuf.stopReader();
|
||||
out.stopWriter();
|
||||
ringBuf.stopWriter();
|
||||
|
||||
if (workThread.joinable()) {
|
||||
workThread.join();
|
||||
}
|
||||
if (bufferWorkerThread.joinable()) {
|
||||
bufferWorkerThread.join();
|
||||
}
|
||||
|
||||
_in->clearReadStop();
|
||||
ringBuf.clearReadStop();
|
||||
out.clearWriteStop();
|
||||
ringBuf.clearWriteStop();
|
||||
}
|
||||
|
||||
void bufferWorker() {
|
||||
T* buf = new T[_keep];
|
||||
bool delay = _skip < 0;
|
||||
|
||||
int readCount = std::min<int>(_keep + _skip, _keep);
|
||||
int skip = std::max<int>(_skip, 0);
|
||||
int delaySize = (-_skip) * sizeof(T);
|
||||
int delayCount = (-_skip);
|
||||
|
||||
T* start = &buf[std::max<int>(-_skip, 0)];
|
||||
T* delayStart = &buf[_keep + _skip];
|
||||
|
||||
while (true) {
|
||||
if (delay) {
|
||||
memmove(buf, delayStart, delaySize);
|
||||
if constexpr (std::is_same_v<T, complex_t> || std::is_same_v<T, stereo_t>) {
|
||||
for (int i = 0; i < delayCount; i++) {
|
||||
buf[i].re /= 10.0f;
|
||||
buf[i].im /= 10.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (ringBuf.readAndSkip(start, readCount, skip) < 0) { break; };
|
||||
memcpy(out.writeBuf, buf, _keep * sizeof(T));
|
||||
if (!out.swap(_keep)) { break; }
|
||||
}
|
||||
delete[] buf;
|
||||
}
|
||||
|
||||
stream<T>* _in;
|
||||
int _outBlockSize;
|
||||
RingBuffer<T> ringBuf;
|
||||
std::thread bufferWorkerThread;
|
||||
std::thread workThread;
|
||||
int _keep, _skip;
|
||||
};
|
||||
}
|
||||
239
core/src/dsp/buffer/ring_buffer.h
Normal file
239
core/src/dsp/buffer/ring_buffer.h
Normal file
@@ -0,0 +1,239 @@
|
||||
#pragma once
|
||||
#include "buffer.h"
|
||||
|
||||
#define RING_BUF_SZ 1000000
|
||||
|
||||
// IMPORTANT: THIS IS TRASH AND MUST BE REWRITTEN IN THE FUTURE
|
||||
|
||||
namespace dsp::buffer {
|
||||
template <class T>
|
||||
class RingBuffer {
|
||||
public:
|
||||
RingBuffer() {}
|
||||
|
||||
RingBuffer(int maxLatency) { init(maxLatency); }
|
||||
|
||||
~RingBuffer() {
|
||||
if (!_init) { return; }
|
||||
buffer::free(_buffer);
|
||||
_init = false;
|
||||
}
|
||||
|
||||
void init(int maxLatency) {
|
||||
size = RING_BUF_SZ;
|
||||
_stopReader = false;
|
||||
_stopWriter = false;
|
||||
this->maxLatency = maxLatency;
|
||||
writec = 0;
|
||||
readc = 0;
|
||||
readable = 0;
|
||||
writable = size;
|
||||
_buffer = buffer::alloc<T>(size);
|
||||
buffer::clear(_buffer, size);
|
||||
_init = true;
|
||||
}
|
||||
|
||||
int read(T* data, int len) {
|
||||
assert(_init);
|
||||
int dataRead = 0;
|
||||
int toRead = 0;
|
||||
while (dataRead < len) {
|
||||
toRead = std::min<int>(waitUntilReadable(), len - dataRead);
|
||||
if (toRead < 0) { return -1; };
|
||||
|
||||
if ((toRead + readc) > size) {
|
||||
memcpy(&data[dataRead], &_buffer[readc], (size - readc) * sizeof(T));
|
||||
memcpy(&data[dataRead + (size - readc)], &_buffer[0], (toRead - (size - readc)) * sizeof(T));
|
||||
}
|
||||
else {
|
||||
memcpy(&data[dataRead], &_buffer[readc], toRead * sizeof(T));
|
||||
}
|
||||
|
||||
dataRead += toRead;
|
||||
|
||||
_readable_mtx.lock();
|
||||
readable -= toRead;
|
||||
_readable_mtx.unlock();
|
||||
_writable_mtx.lock();
|
||||
writable += toRead;
|
||||
_writable_mtx.unlock();
|
||||
readc = (readc + toRead) % size;
|
||||
canWriteVar.notify_one();
|
||||
}
|
||||
return len;
|
||||
}
|
||||
|
||||
int readAndSkip(T* data, int len, int skip) {
|
||||
assert(_init);
|
||||
int dataRead = 0;
|
||||
int toRead = 0;
|
||||
while (dataRead < len) {
|
||||
toRead = std::min<int>(waitUntilReadable(), len - dataRead);
|
||||
if (toRead < 0) { return -1; };
|
||||
|
||||
if ((toRead + readc) > size) {
|
||||
memcpy(&data[dataRead], &_buffer[readc], (size - readc) * sizeof(T));
|
||||
memcpy(&data[dataRead + (size - readc)], &_buffer[0], (toRead - (size - readc)) * sizeof(T));
|
||||
}
|
||||
else {
|
||||
memcpy(&data[dataRead], &_buffer[readc], toRead * sizeof(T));
|
||||
}
|
||||
|
||||
dataRead += toRead;
|
||||
|
||||
_readable_mtx.lock();
|
||||
readable -= toRead;
|
||||
_readable_mtx.unlock();
|
||||
_writable_mtx.lock();
|
||||
writable += toRead;
|
||||
_writable_mtx.unlock();
|
||||
readc = (readc + toRead) % size;
|
||||
canWriteVar.notify_one();
|
||||
}
|
||||
dataRead = 0;
|
||||
while (dataRead < skip) {
|
||||
toRead = std::min<int>(waitUntilReadable(), skip - dataRead);
|
||||
if (toRead < 0) { return -1; };
|
||||
|
||||
dataRead += toRead;
|
||||
|
||||
_readable_mtx.lock();
|
||||
readable -= toRead;
|
||||
_readable_mtx.unlock();
|
||||
_writable_mtx.lock();
|
||||
writable += toRead;
|
||||
_writable_mtx.unlock();
|
||||
readc = (readc + toRead) % size;
|
||||
canWriteVar.notify_one();
|
||||
}
|
||||
return len;
|
||||
}
|
||||
|
||||
int waitUntilReadable() {
|
||||
assert(_init);
|
||||
if (_stopReader) { return -1; }
|
||||
int _r = getReadable();
|
||||
if (_r != 0) { return _r; }
|
||||
std::unique_lock<std::mutex> lck(_readable_mtx);
|
||||
canReadVar.wait(lck, [=]() { return ((this->getReadable(false) > 0) || this->getReadStop()); });
|
||||
if (_stopReader) { return -1; }
|
||||
return getReadable(false);
|
||||
}
|
||||
|
||||
int getReadable(bool lock = true) {
|
||||
assert(_init);
|
||||
if (lock) { _readable_mtx.lock(); };
|
||||
int _r = readable;
|
||||
if (lock) { _readable_mtx.unlock(); };
|
||||
return _r;
|
||||
}
|
||||
|
||||
int write(T* data, int len) {
|
||||
assert(_init);
|
||||
int dataWritten = 0;
|
||||
int toWrite = 0;
|
||||
while (dataWritten < len) {
|
||||
toWrite = std::min<int>(waitUntilwritable(), len - dataWritten);
|
||||
if (toWrite < 0) { return -1; };
|
||||
|
||||
if ((toWrite + writec) > size) {
|
||||
memcpy(&_buffer[writec], &data[dataWritten], (size - writec) * sizeof(T));
|
||||
memcpy(&_buffer[0], &data[dataWritten + (size - writec)], (toWrite - (size - writec)) * sizeof(T));
|
||||
}
|
||||
else {
|
||||
memcpy(&_buffer[writec], &data[dataWritten], toWrite * sizeof(T));
|
||||
}
|
||||
|
||||
dataWritten += toWrite;
|
||||
|
||||
_readable_mtx.lock();
|
||||
readable += toWrite;
|
||||
_readable_mtx.unlock();
|
||||
_writable_mtx.lock();
|
||||
writable -= toWrite;
|
||||
_writable_mtx.unlock();
|
||||
writec = (writec + toWrite) % size;
|
||||
|
||||
canReadVar.notify_one();
|
||||
}
|
||||
return len;
|
||||
}
|
||||
|
||||
int waitUntilwritable() {
|
||||
assert(_init);
|
||||
if (_stopWriter) { return -1; }
|
||||
int _w = getWritable();
|
||||
if (_w != 0) { return _w; }
|
||||
std::unique_lock<std::mutex> lck(_writable_mtx);
|
||||
canWriteVar.wait(lck, [=]() { return ((this->getWritable(false) > 0) || this->getWriteStop()); });
|
||||
if (_stopWriter) { return -1; }
|
||||
return getWritable(false);
|
||||
}
|
||||
|
||||
int getWritable(bool lock = true) {
|
||||
assert(_init);
|
||||
if (lock) { _writable_mtx.lock(); };
|
||||
int _w = writable;
|
||||
if (lock) {
|
||||
_writable_mtx.unlock();
|
||||
_readable_mtx.lock();
|
||||
};
|
||||
int _r = readable;
|
||||
if (lock) { _readable_mtx.unlock(); };
|
||||
return std::max<int>(std::min<int>(_w, maxLatency - _r), 0);
|
||||
}
|
||||
|
||||
void stopReader() {
|
||||
assert(_init);
|
||||
_stopReader = true;
|
||||
canReadVar.notify_one();
|
||||
}
|
||||
|
||||
void stopWriter() {
|
||||
assert(_init);
|
||||
_stopWriter = true;
|
||||
canWriteVar.notify_one();
|
||||
}
|
||||
|
||||
bool getReadStop() {
|
||||
assert(_init);
|
||||
return _stopReader;
|
||||
}
|
||||
|
||||
bool getWriteStop() {
|
||||
assert(_init);
|
||||
return _stopWriter;
|
||||
}
|
||||
|
||||
void clearReadStop() {
|
||||
assert(_init);
|
||||
_stopReader = false;
|
||||
}
|
||||
|
||||
void clearWriteStop() {
|
||||
assert(_init);
|
||||
_stopWriter = false;
|
||||
}
|
||||
|
||||
void setMaxLatency(int maxLatency) {
|
||||
assert(_init);
|
||||
this->maxLatency = maxLatency;
|
||||
}
|
||||
|
||||
private:
|
||||
bool _init = false;
|
||||
T* _buffer;
|
||||
int size;
|
||||
int readc;
|
||||
int writec;
|
||||
int readable;
|
||||
int writable;
|
||||
int maxLatency;
|
||||
bool _stopReader;
|
||||
bool _stopWriter;
|
||||
std::mutex _readable_mtx;
|
||||
std::mutex _writable_mtx;
|
||||
std::condition_variable canReadVar;
|
||||
std::condition_variable canWriteVar;
|
||||
};
|
||||
}
|
||||
Reference in New Issue
Block a user