1005 lines
20 KiB
C++
1005 lines
20 KiB
C++
// ----------------------------------------------------------------------------
|
|
//
|
|
// Copyright (C) 2006-2018 Fons Adriaensen <fons@linuxaudio.org>
|
|
//
|
|
// This program is free software; you can redistribute it and/or modify
|
|
// it under the terms of the GNU General Public License as published by
|
|
// the Free Software Foundation; either version 3 of the License, or
|
|
// (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful,
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
// GNU General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
//
|
|
// ----------------------------------------------------------------------------
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
|
|
#ifdef _MSC_VER
|
|
#include <windows.h> // Needed for MSVC 'Sleep()'
|
|
#endif
|
|
|
|
#include "zita-convolver/zita-convolver.h"
|
|
|
|
using namespace ArdourZita;
|
|
|
|
float Convproc::_mac_cost = 1.0f;
|
|
float Convproc::_fft_cost = 5.0f;
|
|
|
|
static float*
|
|
calloc_real (uint32_t k)
|
|
{
|
|
float* p = fftwf_alloc_real (k);
|
|
if (!p)
|
|
throw (Converror (Converror::MEM_ALLOC));
|
|
memset (p, 0, k * sizeof (float));
|
|
return p;
|
|
}
|
|
|
|
static fftwf_complex*
|
|
calloc_complex (uint32_t k)
|
|
{
|
|
fftwf_complex* p = fftwf_alloc_complex (k);
|
|
if (!p)
|
|
throw (Converror (Converror::MEM_ALLOC));
|
|
memset (p, 0, k * sizeof (fftwf_complex));
|
|
return p;
|
|
}
|
|
|
|
Convproc::Convproc (void)
|
|
: _state (ST_IDLE)
|
|
, _options (0)
|
|
, _skipcnt (0)
|
|
, _ninp (0)
|
|
, _nout (0)
|
|
, _quantum (0)
|
|
, _minpart (0)
|
|
, _maxpart (0)
|
|
, _nlevels (0)
|
|
, _latecnt (0)
|
|
{
|
|
memset (_inpbuff, 0, MAXINP * sizeof (float*));
|
|
memset (_outbuff, 0, MAXOUT * sizeof (float*));
|
|
memset (_convlev, 0, MAXLEV * sizeof (Convlevel*));
|
|
}
|
|
|
|
Convproc::~Convproc (void)
|
|
{
|
|
stop_process ();
|
|
cleanup ();
|
|
}
|
|
|
|
void
|
|
Convproc::set_options (uint32_t options)
|
|
{
|
|
_options = options;
|
|
}
|
|
|
|
void
|
|
Convproc::set_skipcnt (uint32_t skipcnt)
|
|
{
|
|
if ((_quantum == _minpart) && (_quantum == _maxpart))
|
|
_skipcnt = skipcnt;
|
|
}
|
|
|
|
int
|
|
Convproc::configure (uint32_t ninp,
|
|
uint32_t nout,
|
|
uint32_t maxsize,
|
|
uint32_t quantum,
|
|
uint32_t minpart,
|
|
uint32_t maxpart,
|
|
float density)
|
|
{
|
|
uint32_t offs, npar, size, pind, nmin, i;
|
|
int prio, step, d, r, s;
|
|
float cfft, cmac;
|
|
|
|
if (_state != ST_IDLE)
|
|
return Converror::BAD_STATE;
|
|
if ( (ninp < 1) || (ninp > MAXINP)
|
|
|| (nout < 1) || (nout > MAXOUT)
|
|
|| (quantum & (quantum - 1))
|
|
|| (quantum < MINQUANT)
|
|
|| (quantum > MAXQUANT)
|
|
|| (minpart & (minpart - 1))
|
|
|| (minpart < MINPART)
|
|
|| (minpart < quantum)
|
|
|| (minpart > MAXDIVIS * quantum)
|
|
|| (maxpart & (maxpart - 1))
|
|
|| (maxpart > MAXPART)
|
|
|| (maxpart < minpart))
|
|
return Converror::BAD_PARAM;
|
|
|
|
nmin = (ninp < nout) ? ninp : nout;
|
|
if (density <= 0.0f)
|
|
density = 1.0f / nmin;
|
|
if (density > 1.0f)
|
|
density = 1.0f;
|
|
cfft = _fft_cost * (ninp + nout);
|
|
cmac = _mac_cost * ninp * nout * density;
|
|
step = (cfft < 4 * cmac) ? 1 : 2;
|
|
if (step == 2) {
|
|
r = maxpart / minpart;
|
|
s = (r & 0xAAAA) ? 1 : 2;
|
|
} else
|
|
s = 1;
|
|
nmin = (s == 1) ? 2 : 6;
|
|
if (minpart == quantum)
|
|
nmin++;
|
|
prio = 0;
|
|
size = quantum;
|
|
while (size < minpart) {
|
|
prio -= 1;
|
|
size <<= 1;
|
|
}
|
|
|
|
try {
|
|
for (offs = pind = 0; offs < maxsize; pind++) {
|
|
npar = (maxsize - offs + size - 1) / size;
|
|
if ((size < maxpart) && (npar > nmin)) {
|
|
r = 1 << s;
|
|
d = npar - nmin;
|
|
d = d - (d + r - 1) / r;
|
|
if (cfft < d * cmac)
|
|
npar = nmin;
|
|
}
|
|
_convlev[pind] = new Convlevel ();
|
|
_convlev[pind]->configure (prio, offs, npar, size, _options);
|
|
offs += size * npar;
|
|
if (offs < maxsize) {
|
|
prio -= s;
|
|
size <<= s;
|
|
s = step;
|
|
nmin = (s == 1) ? 2 : 6;
|
|
}
|
|
}
|
|
|
|
_ninp = ninp;
|
|
_nout = nout;
|
|
_quantum = quantum;
|
|
_minpart = minpart;
|
|
_maxpart = size;
|
|
_nlevels = pind;
|
|
_latecnt = 0;
|
|
_inpsize = 2 * size;
|
|
|
|
for (i = 0; i < ninp; i++)
|
|
_inpbuff[i] = new float[_inpsize];
|
|
for (i = 0; i < nout; i++)
|
|
_outbuff[i] = new float[_minpart];
|
|
} catch (...) {
|
|
cleanup ();
|
|
return Converror::MEM_ALLOC;
|
|
}
|
|
|
|
_state = ST_STOP;
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
Convproc::impdata_create (uint32_t inp,
|
|
uint32_t out,
|
|
int32_t step,
|
|
float* data,
|
|
int32_t ind0,
|
|
int32_t ind1)
|
|
{
|
|
uint32_t j;
|
|
|
|
if (_state != ST_STOP)
|
|
return Converror::BAD_STATE;
|
|
if ((inp >= _ninp) || (out >= _nout))
|
|
return Converror::BAD_PARAM;
|
|
try {
|
|
for (j = 0; j < _nlevels; j++) {
|
|
_convlev[j]->impdata_write (inp, out, step, data, ind0, ind1, true);
|
|
}
|
|
} catch (...) {
|
|
cleanup ();
|
|
return Converror::MEM_ALLOC;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
Convproc::impdata_clear (uint32_t inp, uint32_t out)
|
|
{
|
|
uint32_t k;
|
|
|
|
if (_state < ST_STOP)
|
|
return Converror::BAD_STATE;
|
|
for (k = 0; k < _nlevels; k++)
|
|
_convlev[k]->impdata_clear (inp, out);
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
Convproc::impdata_update (uint32_t inp,
|
|
uint32_t out,
|
|
int32_t step,
|
|
float* data,
|
|
int32_t ind0,
|
|
int32_t ind1)
|
|
{
|
|
uint32_t j;
|
|
|
|
if (_state < ST_STOP)
|
|
return Converror::BAD_STATE;
|
|
if ((inp >= _ninp) || (out >= _nout))
|
|
return Converror::BAD_PARAM;
|
|
for (j = 0; j < _nlevels; j++) {
|
|
_convlev[j]->impdata_write (inp, out, step, data, ind0, ind1, false);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
Convproc::impdata_link (uint32_t inp1,
|
|
uint32_t out1,
|
|
uint32_t inp2,
|
|
uint32_t out2)
|
|
{
|
|
uint32_t j;
|
|
|
|
if ((inp1 >= _ninp) || (out1 >= _nout))
|
|
return Converror::BAD_PARAM;
|
|
if ((inp2 >= _ninp) || (out2 >= _nout))
|
|
return Converror::BAD_PARAM;
|
|
if ((inp1 == inp2) && (out1 == out2))
|
|
return Converror::BAD_PARAM;
|
|
if (_state != ST_STOP)
|
|
return Converror::BAD_STATE;
|
|
try {
|
|
for (j = 0; j < _nlevels; j++) {
|
|
_convlev[j]->impdata_link (inp1, out1, inp2, out2);
|
|
}
|
|
} catch (...) {
|
|
cleanup ();
|
|
return Converror::MEM_ALLOC;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
Convproc::reset (void)
|
|
{
|
|
uint32_t k;
|
|
|
|
if (_state == ST_IDLE)
|
|
return Converror::BAD_STATE;
|
|
for (k = 0; k < _ninp; k++)
|
|
memset (_inpbuff[k], 0, _inpsize * sizeof (float));
|
|
for (k = 0; k < _nout; k++)
|
|
memset (_outbuff[k], 0, _minpart * sizeof (float));
|
|
for (k = 0; k < _nlevels; k++)
|
|
_convlev[k]->reset (_inpsize, _minpart, _inpbuff, _outbuff);
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
Convproc::start_process (int abspri, int policy)
|
|
{
|
|
uint32_t k;
|
|
|
|
if (_state != ST_STOP)
|
|
return Converror::BAD_STATE;
|
|
_latecnt = 0;
|
|
_inpoffs = 0;
|
|
_outoffs = 0;
|
|
reset ();
|
|
|
|
for (k = (_minpart == _quantum) ? 1 : 0; k < _nlevels; k++) {
|
|
_convlev[k]->start (abspri, policy);
|
|
}
|
|
_state = ST_PROC;
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
Convproc::process (bool sync)
|
|
{
|
|
uint32_t k;
|
|
int f = 0;
|
|
|
|
if (_state != ST_PROC)
|
|
return 0;
|
|
_inpoffs += _quantum;
|
|
if (_inpoffs == _inpsize)
|
|
_inpoffs = 0;
|
|
_outoffs += _quantum;
|
|
if (_outoffs == _minpart) {
|
|
_outoffs = 0;
|
|
for (k = 0; k < _nout; k++)
|
|
memset (_outbuff[k], 0, _minpart * sizeof (float));
|
|
for (k = 0; k < _nlevels; k++)
|
|
f |= _convlev[k]->readout (sync, _skipcnt);
|
|
if (_skipcnt < _minpart)
|
|
_skipcnt = 0;
|
|
else
|
|
_skipcnt -= _minpart;
|
|
if (f) {
|
|
if (++_latecnt >= 5) {
|
|
if (~_options & OPT_LATE_CONTIN)
|
|
stop_process ();
|
|
f |= FL_LOAD;
|
|
}
|
|
} else
|
|
_latecnt = 0;
|
|
}
|
|
return f;
|
|
}
|
|
|
|
int
|
|
Convproc::stop_process (void)
|
|
{
|
|
uint32_t k;
|
|
|
|
if (_state != ST_PROC)
|
|
return Converror::BAD_STATE;
|
|
for (k = 0; k < _nlevels; k++)
|
|
_convlev[k]->stop ();
|
|
_state = ST_WAIT;
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
Convproc::cleanup (void)
|
|
{
|
|
uint32_t k;
|
|
|
|
while (!check_stop ()) {
|
|
#ifdef _MSC_VER
|
|
Sleep (100);
|
|
#else
|
|
usleep (100000);
|
|
#endif
|
|
}
|
|
for (k = 0; k < _ninp; k++) {
|
|
delete[] _inpbuff[k];
|
|
_inpbuff[k] = 0;
|
|
}
|
|
for (k = 0; k < _nout; k++) {
|
|
delete[] _outbuff[k];
|
|
_outbuff[k] = 0;
|
|
}
|
|
for (k = 0; k < _nlevels; k++) {
|
|
delete _convlev[k];
|
|
_convlev[k] = 0;
|
|
}
|
|
|
|
_state = ST_IDLE;
|
|
_options = 0;
|
|
_skipcnt = 0;
|
|
_ninp = 0;
|
|
_nout = 0;
|
|
_quantum = 0;
|
|
_minpart = 0;
|
|
_maxpart = 0;
|
|
_nlevels = 0;
|
|
_latecnt = 0;
|
|
return 0;
|
|
}
|
|
|
|
bool
|
|
Convproc::check_stop (void)
|
|
{
|
|
uint32_t k;
|
|
|
|
for (k = 0; (k < _nlevels) && (_convlev[k]->_stat == Convlevel::ST_IDLE); k++)
|
|
;
|
|
if (k == _nlevels) {
|
|
_state = ST_STOP;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void
|
|
Convproc::print (FILE* F)
|
|
{
|
|
uint32_t k;
|
|
|
|
for (k = 0; k < _nlevels; k++)
|
|
_convlev[k]->print (F);
|
|
}
|
|
|
|
#ifdef ENABLE_VECTOR_MODE
|
|
typedef float FV4 __attribute__ ((vector_size (16)));
|
|
#endif
|
|
|
|
Convlevel::Convlevel (void)
|
|
: _stat (ST_IDLE)
|
|
, _npar (0)
|
|
, _parsize (0)
|
|
, _options (0)
|
|
#ifndef PTW32_VERSION
|
|
, _pthr (0)
|
|
#endif
|
|
, _inp_list (0)
|
|
, _out_list (0)
|
|
, _plan_r2c (0)
|
|
, _plan_c2r (0)
|
|
, _time_data (0)
|
|
, _prep_data (0)
|
|
, _freq_data (0)
|
|
{
|
|
}
|
|
|
|
Convlevel::~Convlevel (void)
|
|
{
|
|
cleanup ();
|
|
}
|
|
|
|
void
|
|
Convlevel::configure (int prio,
|
|
uint32_t offs,
|
|
uint32_t npar,
|
|
uint32_t parsize,
|
|
uint32_t options)
|
|
{
|
|
int fftwopt = (options & OPT_FFTW_MEASURE) ? FFTW_MEASURE : FFTW_ESTIMATE;
|
|
|
|
_prio = prio;
|
|
_offs = offs;
|
|
_npar = npar;
|
|
_parsize = parsize;
|
|
_options = options;
|
|
|
|
_time_data = calloc_real (2 * _parsize);
|
|
_prep_data = calloc_real (2 * _parsize);
|
|
_freq_data = calloc_complex (_parsize + 1);
|
|
_plan_r2c = fftwf_plan_dft_r2c_1d (2 * _parsize, _time_data, _freq_data, fftwopt);
|
|
_plan_c2r = fftwf_plan_dft_c2r_1d (2 * _parsize, _freq_data, _time_data, fftwopt);
|
|
if (_plan_r2c && _plan_c2r)
|
|
return;
|
|
throw (Converror (Converror::MEM_ALLOC));
|
|
}
|
|
|
|
void
|
|
Convlevel::impdata_write (uint32_t inp,
|
|
uint32_t out,
|
|
int32_t step,
|
|
float* data,
|
|
int32_t i0,
|
|
int32_t i1,
|
|
bool create)
|
|
{
|
|
uint32_t k;
|
|
int32_t j, j0, j1, n;
|
|
float norm;
|
|
fftwf_complex* fftb;
|
|
Macnode* M;
|
|
|
|
n = i1 - i0;
|
|
i0 = _offs - i0;
|
|
i1 = i0 + _npar * _parsize;
|
|
if ((i0 >= n) || (i1 <= 0))
|
|
return;
|
|
|
|
if (create) {
|
|
M = findmacnode (inp, out, true);
|
|
if (M == 0 || M->_link)
|
|
return;
|
|
if (M->_fftb == 0)
|
|
M->alloc_fftb (_npar);
|
|
} else {
|
|
M = findmacnode (inp, out, false);
|
|
if (M == 0 || M->_link || M->_fftb == 0)
|
|
return;
|
|
}
|
|
|
|
norm = 0.5f / _parsize;
|
|
for (k = 0; k < _npar; k++) {
|
|
i1 = i0 + _parsize;
|
|
if ((i0 < n) && (i1 > 0)) {
|
|
fftb = M->_fftb[k];
|
|
if (fftb == 0 && create) {
|
|
M->_fftb[k] = fftb = calloc_complex (_parsize + 1);
|
|
}
|
|
if (fftb && data) {
|
|
memset (_prep_data, 0, 2 * _parsize * sizeof (float));
|
|
j0 = (i0 < 0) ? 0 : i0;
|
|
j1 = (i1 > n) ? n : i1;
|
|
for (j = j0; j < j1; j++)
|
|
_prep_data[j - i0] = norm * data[j * step];
|
|
fftwf_execute_dft_r2c (_plan_r2c, _prep_data, _freq_data);
|
|
#ifdef ENABLE_VECTOR_MODE
|
|
if (_options & OPT_VECTOR_MODE)
|
|
fftswap (_freq_data);
|
|
#endif
|
|
for (j = 0; j <= (int)_parsize; j++) {
|
|
fftb[j][0] += _freq_data[j][0];
|
|
fftb[j][1] += _freq_data[j][1];
|
|
}
|
|
}
|
|
}
|
|
i0 = i1;
|
|
}
|
|
}
|
|
|
|
void
|
|
Convlevel::impdata_clear (uint32_t inp, uint32_t out)
|
|
{
|
|
uint32_t i;
|
|
Macnode* M;
|
|
|
|
M = findmacnode (inp, out, false);
|
|
if (M == 0 || M->_link || M->_fftb == 0)
|
|
return;
|
|
for (i = 0; i < _npar; i++) {
|
|
if (M->_fftb[i]) {
|
|
memset (M->_fftb[i], 0, (_parsize + 1) * sizeof (fftwf_complex));
|
|
}
|
|
}
|
|
}
|
|
|
|
void
|
|
Convlevel::impdata_link (uint32_t inp1,
|
|
uint32_t out1,
|
|
uint32_t inp2,
|
|
uint32_t out2)
|
|
{
|
|
Macnode* M1;
|
|
Macnode* M2;
|
|
|
|
M1 = findmacnode (inp1, out1, false);
|
|
if (!M1)
|
|
return;
|
|
M2 = findmacnode (inp2, out2, true);
|
|
M2->free_fftb ();
|
|
M2->_link = M1;
|
|
}
|
|
|
|
void
|
|
Convlevel::reset (uint32_t inpsize,
|
|
uint32_t outsize,
|
|
float** inpbuff,
|
|
float** outbuff)
|
|
{
|
|
uint32_t i;
|
|
Inpnode* X;
|
|
Outnode* Y;
|
|
|
|
_inpsize = inpsize;
|
|
_outsize = outsize;
|
|
_inpbuff = inpbuff;
|
|
_outbuff = outbuff;
|
|
for (X = _inp_list; X; X = X->_next) {
|
|
for (i = 0; i < _npar; i++) {
|
|
memset (X->_ffta[i], 0, (_parsize + 1) * sizeof (fftwf_complex));
|
|
}
|
|
}
|
|
for (Y = _out_list; Y; Y = Y->_next) {
|
|
for (i = 0; i < 3; i++) {
|
|
memset (Y->_buff[i], 0, _parsize * sizeof (float));
|
|
}
|
|
}
|
|
if (_parsize == _outsize) {
|
|
_outoffs = 0;
|
|
_inpoffs = 0;
|
|
} else {
|
|
_outoffs = _parsize / 2;
|
|
_inpoffs = _inpsize - _outoffs;
|
|
}
|
|
_bits = _parsize / _outsize;
|
|
_wait = 0;
|
|
_ptind = 0;
|
|
_opind = 0;
|
|
_trig.init (0, 0);
|
|
_done.init (0, 0);
|
|
}
|
|
|
|
void
|
|
Convlevel::start (int abspri, int policy)
|
|
{
|
|
int min, max;
|
|
pthread_attr_t attr;
|
|
struct sched_param parm;
|
|
|
|
#ifndef PTW32_VERSION
|
|
_pthr = 0;
|
|
#endif
|
|
min = sched_get_priority_min (policy);
|
|
max = sched_get_priority_max (policy);
|
|
abspri += _prio;
|
|
if (abspri > max)
|
|
abspri = max;
|
|
if (abspri < min)
|
|
abspri = min;
|
|
parm.sched_priority = abspri;
|
|
pthread_attr_init (&attr);
|
|
pthread_attr_setdetachstate (&attr, PTHREAD_CREATE_DETACHED);
|
|
pthread_attr_setschedpolicy (&attr, policy);
|
|
pthread_attr_setschedparam (&attr, &parm);
|
|
pthread_attr_setscope (&attr, PTHREAD_SCOPE_SYSTEM);
|
|
pthread_attr_setinheritsched (&attr, PTHREAD_EXPLICIT_SCHED);
|
|
pthread_attr_setstacksize (&attr, 0x10000); // 64kB
|
|
pthread_create (&_pthr, &attr, static_main, this);
|
|
pthread_attr_destroy (&attr);
|
|
}
|
|
|
|
void
|
|
Convlevel::stop (void)
|
|
{
|
|
if (_stat != ST_IDLE) {
|
|
_stat = ST_TERM;
|
|
_trig.post ();
|
|
}
|
|
}
|
|
|
|
void
|
|
Convlevel::cleanup (void)
|
|
{
|
|
Inpnode *X, *X1;
|
|
Outnode *Y, *Y1;
|
|
Macnode *M, *M1;
|
|
|
|
X = _inp_list;
|
|
while (X) {
|
|
X1 = X->_next;
|
|
delete X;
|
|
X = X1;
|
|
}
|
|
_inp_list = 0;
|
|
|
|
Y = _out_list;
|
|
while (Y) {
|
|
M = Y->_list;
|
|
while (M) {
|
|
M1 = M->_next;
|
|
delete M;
|
|
M = M1;
|
|
}
|
|
Y1 = Y->_next;
|
|
delete Y;
|
|
Y = Y1;
|
|
}
|
|
_out_list = 0;
|
|
|
|
fftwf_destroy_plan (_plan_r2c);
|
|
fftwf_destroy_plan (_plan_c2r);
|
|
fftwf_free (_time_data);
|
|
fftwf_free (_prep_data);
|
|
fftwf_free (_freq_data);
|
|
_plan_r2c = 0;
|
|
_plan_c2r = 0;
|
|
_time_data = 0;
|
|
_prep_data = 0;
|
|
_freq_data = 0;
|
|
}
|
|
|
|
void*
|
|
Convlevel::static_main (void* arg)
|
|
{
|
|
((Convlevel*)arg)->main ();
|
|
#if !defined PTW32_VERSION && defined _GNU_SOURCE
|
|
pthread_setname_np (pthread_self(), "ZConvlevel");
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
void
|
|
Convlevel::main (void)
|
|
{
|
|
_stat = ST_PROC;
|
|
while (true) {
|
|
_trig.wait ();
|
|
if (_stat == ST_TERM) {
|
|
_stat = ST_IDLE;
|
|
#ifndef PTW32_VERSION
|
|
_pthr = 0;
|
|
#endif
|
|
return;
|
|
}
|
|
process (false);
|
|
_done.post ();
|
|
}
|
|
}
|
|
|
|
void
|
|
Convlevel::process (bool skip)
|
|
{
|
|
uint32_t i, i1, j, k, n1, n2, opi1, opi2;
|
|
Inpnode* X;
|
|
Macnode* M;
|
|
Outnode* Y;
|
|
fftwf_complex* ffta;
|
|
fftwf_complex* fftb;
|
|
float* inpd;
|
|
float* outd;
|
|
|
|
i1 = _inpoffs;
|
|
n1 = _parsize;
|
|
n2 = 0;
|
|
_inpoffs = i1 + n1;
|
|
if (_inpoffs >= _inpsize) {
|
|
_inpoffs -= _inpsize;
|
|
n2 = _inpoffs;
|
|
n1 -= n2;
|
|
}
|
|
|
|
opi1 = (_opind + 1) % 3;
|
|
opi2 = (_opind + 2) % 3;
|
|
|
|
for (X = _inp_list; X; X = X->_next) {
|
|
inpd = _inpbuff[X->_inp];
|
|
if (n1)
|
|
memcpy (_time_data, inpd + i1, n1 * sizeof (float));
|
|
if (n2)
|
|
memcpy (_time_data + n1, inpd, n2 * sizeof (float));
|
|
memset (_time_data + _parsize, 0, _parsize * sizeof (float));
|
|
fftwf_execute_dft_r2c (_plan_r2c, _time_data, X->_ffta[_ptind]);
|
|
#ifdef ENABLE_VECTOR_MODE
|
|
if (_options & OPT_VECTOR_MODE)
|
|
fftswap (X->_ffta[_ptind]);
|
|
#endif
|
|
}
|
|
|
|
if (skip) {
|
|
for (Y = _out_list; Y; Y = Y->_next) {
|
|
outd = Y->_buff[opi2];
|
|
memset (outd, 0, _parsize * sizeof (float));
|
|
}
|
|
} else {
|
|
for (Y = _out_list; Y; Y = Y->_next) {
|
|
memset (_freq_data, 0, (_parsize + 1) * sizeof (fftwf_complex));
|
|
for (M = Y->_list; M; M = M->_next) {
|
|
X = M->_inpn;
|
|
i = _ptind;
|
|
for (j = 0; j < _npar; j++) {
|
|
ffta = X->_ffta[i];
|
|
fftb = M->_link ? M->_link->_fftb[j] : M->_fftb[j];
|
|
if (fftb) {
|
|
#ifdef ENABLE_VECTOR_MODE
|
|
if (_options & OPT_VECTOR_MODE) {
|
|
FV4* A = (FV4*)ffta;
|
|
FV4* B = (FV4*)fftb;
|
|
FV4* D = (FV4*)_freq_data;
|
|
for (k = 0; k < _parsize; k += 4) {
|
|
D[0] += A[0] * B[0] - A[1] * B[1];
|
|
D[1] += A[0] * B[1] + A[1] * B[0];
|
|
A += 2;
|
|
B += 2;
|
|
D += 2;
|
|
}
|
|
_freq_data[_parsize][0] += ffta[_parsize][0] * fftb[_parsize][0];
|
|
_freq_data[_parsize][1] = 0;
|
|
} else
|
|
#endif
|
|
{
|
|
for (k = 0; k <= _parsize; k++) {
|
|
_freq_data[k][0] += ffta[k][0] * fftb[k][0] - ffta[k][1] * fftb[k][1];
|
|
_freq_data[k][1] += ffta[k][0] * fftb[k][1] + ffta[k][1] * fftb[k][0];
|
|
}
|
|
}
|
|
}
|
|
if (i == 0)
|
|
i = _npar;
|
|
i--;
|
|
}
|
|
}
|
|
|
|
#ifdef ENABLE_VECTOR_MODE
|
|
if (_options & OPT_VECTOR_MODE)
|
|
fftswap (_freq_data);
|
|
#endif
|
|
fftwf_execute_dft_c2r (_plan_c2r, _freq_data, _time_data);
|
|
outd = Y->_buff[opi1];
|
|
for (k = 0; k < _parsize; k++)
|
|
outd[k] += _time_data[k];
|
|
outd = Y->_buff[opi2];
|
|
memcpy (outd, _time_data + _parsize, _parsize * sizeof (float));
|
|
}
|
|
}
|
|
|
|
_ptind++;
|
|
if (_ptind == _npar)
|
|
_ptind = 0;
|
|
}
|
|
|
|
int
|
|
Convlevel::readout (bool sync, uint32_t skipcnt)
|
|
{
|
|
uint32_t i;
|
|
float * p, *q;
|
|
Outnode* Y;
|
|
|
|
_outoffs += _outsize;
|
|
if (_outoffs == _parsize) {
|
|
_outoffs = 0;
|
|
if (_stat == ST_PROC) {
|
|
while (_wait) {
|
|
if (sync)
|
|
_done.wait ();
|
|
else if (_done.trywait ())
|
|
break;
|
|
_wait--;
|
|
}
|
|
if (++_opind == 3)
|
|
_opind = 0;
|
|
_trig.post ();
|
|
_wait++;
|
|
} else {
|
|
process (skipcnt >= 2 * _parsize);
|
|
if (++_opind == 3)
|
|
_opind = 0;
|
|
}
|
|
}
|
|
|
|
for (Y = _out_list; Y; Y = Y->_next) {
|
|
p = Y->_buff[_opind] + _outoffs;
|
|
q = _outbuff[Y->_out];
|
|
for (i = 0; i < _outsize; i++)
|
|
q[i] += p[i];
|
|
}
|
|
|
|
return (_wait > 1) ? _bits : 0;
|
|
}
|
|
|
|
void
|
|
Convlevel::print (FILE* F)
|
|
{
|
|
fprintf (F, "prio = %4d, offs = %6d, parsize = %5d, npar = %3d\n", _prio, _offs, _parsize, _npar);
|
|
}
|
|
|
|
Macnode*
|
|
Convlevel::findmacnode (uint32_t inp, uint32_t out, bool create)
|
|
{
|
|
Inpnode* X;
|
|
Outnode* Y;
|
|
Macnode* M;
|
|
|
|
for (X = _inp_list; X && (X->_inp != inp); X = X->_next)
|
|
;
|
|
if (!X) {
|
|
if (!create)
|
|
return 0;
|
|
X = new Inpnode (inp);
|
|
X->_next = _inp_list;
|
|
_inp_list = X;
|
|
X->alloc_ffta (_npar, _parsize);
|
|
}
|
|
|
|
for (Y = _out_list; Y && (Y->_out != out); Y = Y->_next)
|
|
;
|
|
if (!Y) {
|
|
if (!create)
|
|
return 0;
|
|
Y = new Outnode (out, _parsize);
|
|
Y->_next = _out_list;
|
|
_out_list = Y;
|
|
}
|
|
|
|
for (M = Y->_list; M && (M->_inpn != X); M = M->_next)
|
|
;
|
|
if (!M) {
|
|
if (!create)
|
|
return 0;
|
|
M = new Macnode (X);
|
|
M->_next = Y->_list;
|
|
Y->_list = M;
|
|
}
|
|
|
|
return M;
|
|
}
|
|
|
|
#ifdef ENABLE_VECTOR_MODE
|
|
|
|
void
|
|
Convlevel::fftswap (fftwf_complex* p)
|
|
{
|
|
uint32_t n = _parsize;
|
|
float a, b;
|
|
|
|
while (n) {
|
|
a = p[2][0];
|
|
b = p[3][0];
|
|
p[2][0] = p[0][1];
|
|
p[3][0] = p[1][1];
|
|
p[0][1] = a;
|
|
p[1][1] = b;
|
|
p += 4;
|
|
n -= 4;
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
Inpnode::Inpnode (uint16_t inp)
|
|
: _next (0)
|
|
, _ffta (0)
|
|
, _npar (0)
|
|
, _inp (inp)
|
|
{
|
|
}
|
|
|
|
Inpnode::~Inpnode (void)
|
|
{
|
|
free_ffta ();
|
|
}
|
|
|
|
void
|
|
Inpnode::alloc_ffta (uint16_t npar, int32_t size)
|
|
{
|
|
_npar = npar;
|
|
_ffta = new fftwf_complex*[_npar];
|
|
for (int i = 0; i < _npar; i++) {
|
|
_ffta[i] = calloc_complex (size + 1);
|
|
}
|
|
}
|
|
|
|
void
|
|
Inpnode::free_ffta (void)
|
|
{
|
|
if (!_ffta)
|
|
return;
|
|
for (uint16_t i = 0; i < _npar; i++) {
|
|
fftwf_free (_ffta[i]);
|
|
}
|
|
delete[] _ffta;
|
|
_ffta = 0;
|
|
_npar = 0;
|
|
}
|
|
|
|
Macnode::Macnode (Inpnode* inpn)
|
|
: _next (0)
|
|
, _inpn (inpn)
|
|
, _link (0)
|
|
, _fftb (0)
|
|
, _npar (0)
|
|
{
|
|
}
|
|
|
|
Macnode::~Macnode (void)
|
|
{
|
|
free_fftb ();
|
|
}
|
|
|
|
void
|
|
Macnode::alloc_fftb (uint16_t npar)
|
|
{
|
|
_npar = npar;
|
|
_fftb = new fftwf_complex*[_npar];
|
|
for (uint16_t i = 0; i < _npar; i++) {
|
|
_fftb[i] = 0;
|
|
}
|
|
}
|
|
|
|
void
|
|
Macnode::free_fftb (void)
|
|
{
|
|
if (!_fftb)
|
|
return;
|
|
for (uint16_t i = 0; i < _npar; i++) {
|
|
fftwf_free (_fftb[i]);
|
|
}
|
|
delete[] _fftb;
|
|
_fftb = 0;
|
|
_npar = 0;
|
|
}
|
|
|
|
Outnode::Outnode (uint16_t out, int32_t size)
|
|
: _next (0)
|
|
, _list (0)
|
|
, _out (out)
|
|
{
|
|
_buff[0] = calloc_real (size);
|
|
_buff[1] = calloc_real (size);
|
|
_buff[2] = calloc_real (size);
|
|
}
|
|
|
|
Outnode::~Outnode (void)
|
|
{
|
|
fftwf_free (_buff[0]);
|
|
fftwf_free (_buff[1]);
|
|
fftwf_free (_buff[2]);
|
|
}
|