diff --git a/core/src/dsp/compression.h b/core/src/dsp/compression.h new file mode 100644 index 00000000..cf56117e --- /dev/null +++ b/core/src/dsp/compression.h @@ -0,0 +1,161 @@ +#pragma once +#include + +namespace dsp { + class DynamicRangeCompressor : public generic_block { + public: + DynamicRangeCompressor() {} + + enum PCMType { + PCM_TYPE_I8, + PCM_TYPE_I16, + PCM_TYPE_F32 + }; + + DynamicRangeCompressor(stream* in, PCMType pcmType) { init(in, pcmType); } + + void init(stream* in, PCMType pcmType) { + _in = in; + _pcmType = pcmType; + generic_block::registerInput(_in); + generic_block::registerOutput(&out); + generic_block::_block_init = true; + } + + void setInput(stream* in) { + assert(generic_block::_block_init); + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + generic_block::unregisterInput(_in); + _in = in; + generic_block::registerInput(_in); + generic_block::tempStart(); + } + + void setPCMType(PCMType pcmType) { + assert(generic_block::_block_init); + std::lock_guard lck(generic_block::ctrlMtx); + _pcmType = pcmType; + } + + int run() { + int count = _in->read(); + if (count < 0) { return -1; } + + float* scaler = (float*)out.writeBuf; + void* dataBuf = &out.writeBuf[4]; + + // If no dynamic range compression is to be done, just pass the data to the output with a null scaler + if (_pcmType == PCM_TYPE_F32) { + *scaler = 0; + memcpy(dataBuf, _in->readBuf, count * sizeof(complex_t)); + _in->flush(); + if (!out.swap(4 + (count * sizeof(complex_t)))) { return -1; } + return count; + } + + // Find maximum value + complex_t val; + float absre; + float absim; + float maxVal = 0; + for (int i = 0; i < count; i++) { + val = _in->readBuf[i]; + absre = fabsf(val.re); + absim = fabsf(val.im); + if (absre > maxVal) { maxVal = absre; } + if (absim > maxVal) { maxVal = absim; } + } + + // Convert to the right type and send it out (sign bit determins pcm type) + if (_pcmType == PCM_TYPE_I8) { + *scaler = maxVal; + volk_32f_s32f_convert_8i((int8_t*)dataBuf, (float*)_in->readBuf, 128.0f / maxVal, count * 2); + _in->flush(); + if (!out.swap(4 + (count * sizeof(int8_t) * 2))) { return -1; } + } + else if (_pcmType == PCM_TYPE_I16) { + *scaler = -maxVal; + volk_32f_s32f_convert_16i((int16_t*)dataBuf, (float*)_in->readBuf, 32768.0f / maxVal, count * 2); + _in->flush(); + if (!out.swap(4 + (count * sizeof(int16_t) * 2))) { return -1; } + } + else { + _in->flush(); + } + + return count; + } + + stream out; + + private: + stream* _in; + PCMType _pcmType; + + }; + + class DynamicRangeDecompressor : public generic_block { + public: + DynamicRangeDecompressor() {} + + DynamicRangeDecompressor(stream* in) { init(in); } + + void init(stream* in) { + _in = in; + generic_block::registerInput(_in); + generic_block::registerOutput(&out); + generic_block::_block_init = true; + } + + void setInput(stream* in) { + assert(generic_block::_block_init); + std::lock_guard lck(generic_block::ctrlMtx); + generic_block::tempStop(); + generic_block::unregisterInput(_in); + _in = in; + generic_block::registerInput(_in); + generic_block::tempStart(); + } + + int run() { + int count = _in->read(); + if (count < 0) { return -1; } + + float* scaler = (float*)_in->readBuf; + void* dataBuf = &_in->readBuf[4]; + + // If the scaler is null, data is F32 + if (*scaler == 0) { + memcpy(out.writeBuf, dataBuf, count - 4); + _in->flush(); + if (!out.swap((count - 4) / sizeof(complex_t))) { return -1; } + return count; + } + + // Convert back to f32 from the pcm type + float absScale = fabsf(*scaler); + if (*scaler > 0) { + spdlog::warn("{0}", absScale); + int outCount = (count - 4) / (sizeof(int8_t) * 2); + volk_8i_s32f_convert_32f((float*)out.writeBuf, (int8_t*)dataBuf, 128.0f / absScale, outCount * 2); + _in->flush(); + if (!out.swap(outCount)) { return -1; } + } + else { + int outCount = (count - 4) / (sizeof(int16_t) * 2); + volk_16i_s32f_convert_32f((float*)out.writeBuf, (int16_t*)dataBuf, 32768.0f / absScale, outCount * 2); + _in->flush(); + if (!out.swap(outCount)) { return -1; } + } + + return count; + } + + stream out; + + private: + stream* _in; + + }; +} \ No newline at end of file