libs/zita-convolver/zita-convolver.cc

   1 // ----------------------------------------------------------------------------
   2 //
   3 //  Copyright (C) 2006-2018 Fons Adriaensen <fons@linuxaudio.org>
   4 //
   5 //  This program is free software; you can redistribute it and/or modify
   6 //  it under the terms of the GNU General Public License as published by
   7 //  the Free Software Foundation; either version 3 of the License, or
   8 //  (at your option) any later version.
   9 //
  10 //  This program is distributed in the hope that it will be useful,
  11 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 //  GNU General Public License for more details.
  14 //
  15 //  You should have received a copy of the GNU General Public License
  16 //  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17 //
  18 // ----------------------------------------------------------------------------
  19
  20 #include <stdio.h>
  21 #include <stdlib.h>
  22 #include <string.h>
  23 #include <unistd.h>
  24
  25 #include "zita-convolver/zita-convolver.h"
  26
  27 using namespace ArdourZita;
  28
  29 float Convproc::_mac_cost = 1.0f;
  30 float Convproc::_fft_cost = 5.0f;
  31
  32 static float*
  33 calloc_real (uint32_t k)
  34 {
  35         float* p = fftwf_alloc_real (k);
  36         if (!p)
  37                 throw (Converror (Converror::MEM_ALLOC));
  38         memset (p, 0, k * sizeof (float));
  39         return p;
  40 }
  41
  42 static fftwf_complex*
  43 calloc_complex (uint32_t k)
  44 {
  45         fftwf_complex* p = fftwf_alloc_complex (k);
  46         if (!p)
  47                 throw (Converror (Converror::MEM_ALLOC));
  48         memset (p, 0, k * sizeof (fftwf_complex));
  49         return p;
  50 }
  51
  52 Convproc::Convproc (void)
  53         : _state (ST_IDLE)
  54         , _options (0)
  55         , _skipcnt (0)
  56         , _ninp (0)
  57         , _nout (0)
  58         , _quantum (0)
  59         , _minpart (0)
  60         , _maxpart (0)
  61         , _nlevels (0)
  62         , _latecnt (0)
  63 {
  64         memset (_inpbuff, 0, MAXINP * sizeof (float*));
  65         memset (_outbuff, 0, MAXOUT * sizeof (float*));
  66         memset (_convlev, 0, MAXLEV * sizeof (Convlevel*));
  67 }
  68
  69 Convproc::~Convproc (void)
  70 {
  71         stop_process ();
  72         cleanup ();
  73 }
  74
  75 void
  76 Convproc::set_options (uint32_t options)
  77 {
  78         _options = options;
  79 }
  80
  81 void
  82 Convproc::set_skipcnt (uint32_t skipcnt)
  83 {
  84         if ((_quantum == _minpart) && (_quantum == _maxpart))
  85                 _skipcnt = skipcnt;
  86 }
  87
  88 int
  89 Convproc::configure (uint32_t ninp,
  90                      uint32_t nout,
  91                      uint32_t maxsize,
  92                      uint32_t quantum,
  93                      uint32_t minpart,
  94                      uint32_t maxpart,
  95                      float    density)
  96 {
  97         uint32_t offs, npar, size, pind, nmin, i;
  98         int      prio, step, d, r, s;
  99         float    cfft, cmac;
 100
 101         if (_state != ST_IDLE)
 102                 return Converror::BAD_STATE;
 103         if (   (ninp < 1) || (ninp > MAXINP)
 104             || (nout < 1) || (nout > MAXOUT)
 105             || (quantum & (quantum - 1))
 106             || (quantum < MINQUANT)
 107             || (quantum > MAXQUANT)
 108             || (minpart & (minpart - 1))
 109             || (minpart < MINPART)
 110             || (minpart < quantum)
 111             || (minpart > MAXDIVIS * quantum)
 112             || (maxpart & (maxpart - 1))
 113             || (maxpart > MAXPART)
 114             || (maxpart < minpart))
 115                 return Converror::BAD_PARAM;
 116
 117         nmin = (ninp < nout) ? ninp : nout;
 118         if (density <= 0.0f)
 119                 density = 1.0f / nmin;
 120         if (density > 1.0f)
 121                 density = 1.0f;
 122         cfft            = _fft_cost * (ninp + nout);
 123         cmac            = _mac_cost * ninp * nout * density;
 124         step            = (cfft < 4 * cmac) ? 1 : 2;
 125         if (step == 2) {
 126                 r = maxpart / minpart;
 127                 s = (r & 0xAAAA) ? 1 : 2;
 128         } else
 129                 s = 1;
 130         nmin      = (s == 1) ? 2 : 6;
 131         if (minpart == quantum)
 132                 nmin++;
 133         prio = 0;
 134         size = quantum;
 135         while (size < minpart) {
 136                 prio -= 1;
 137                 size <<= 1;
 138         }
 139
 140         try {
 141                 for (offs = pind = 0; offs < maxsize; pind++) {
 142                         npar = (maxsize - offs + size - 1) / size;
 143                         if ((size < maxpart) && (npar > nmin)) {
 144                                 r = 1 << s;
 145                                 d = npar - nmin;
 146                                 d = d - (d + r - 1) / r;
 147                                 if (cfft < d * cmac)
 148                                         npar = nmin;
 149                         }
 150                         _convlev[pind] = new Convlevel ();
 151                         _convlev[pind]->configure (prio, offs, npar, size, _options);
 152                         offs += size * npar;
 153                         if (offs < maxsize) {
 154                                 prio -= s;
 155                                 size <<= s;
 156                                 s    = step;
 157                                 nmin = (s == 1) ? 2 : 6;
 158                         }
 159                 }
 160
 161                 _ninp    = ninp;
 162                 _nout    = nout;
 163                 _quantum = quantum;
 164                 _minpart = minpart;
 165                 _maxpart = size;
 166                 _nlevels = pind;
 167                 _latecnt = 0;
 168                 _inpsize = 2 * size;
 169
 170                 for (i              = 0; i < ninp; i++)
 171                         _inpbuff[i] = new float[_inpsize];
 172                 for (i              = 0; i < nout; i++)
 173                         _outbuff[i] = new float[_minpart];
 174         } catch (...) {
 175                 cleanup ();
 176                 return Converror::MEM_ALLOC;
 177         }
 178
 179         _state = ST_STOP;
 180         return 0;
 181 }
 182
 183 int
 184 Convproc::impdata_create (uint32_t inp,
 185                           uint32_t out,
 186                           int32_t  step,
 187                           float*   data,
 188                           int32_t  ind0,
 189                           int32_t  ind1)
 190 {
 191         uint32_t j;
 192
 193         if (_state != ST_STOP)
 194                 return Converror::BAD_STATE;
 195         if ((inp >= _ninp) || (out >= _nout))
 196                 return Converror::BAD_PARAM;
 197         try {
 198                 for (j = 0; j < _nlevels; j++) {
 199                         _convlev[j]->impdata_write (inp, out, step, data, ind0, ind1, true);
 200                 }
 201         } catch (...) {
 202                 cleanup ();
 203                 return Converror::MEM_ALLOC;
 204         }
 205         return 0;
 206 }
 207
 208 int
 209 Convproc::impdata_clear (uint32_t inp, uint32_t out)
 210 {
 211         uint32_t k;
 212
 213         if (_state < ST_STOP)
 214                 return Converror::BAD_STATE;
 215         for (k = 0; k < _nlevels; k++)
 216                 _convlev[k]->impdata_clear (inp, out);
 217         return 0;
 218 }
 219
 220 int
 221 Convproc::impdata_update (uint32_t inp,
 222                           uint32_t out,
 223                           int32_t  step,
 224                           float*   data,
 225                           int32_t  ind0,
 226                           int32_t  ind1)
 227 {
 228         uint32_t j;
 229
 230         if (_state < ST_STOP)
 231                 return Converror::BAD_STATE;
 232         if ((inp >= _ninp) || (out >= _nout))
 233                 return Converror::BAD_PARAM;
 234         for (j = 0; j < _nlevels; j++) {
 235                 _convlev[j]->impdata_write (inp, out, step, data, ind0, ind1, false);
 236         }
 237         return 0;
 238 }
 239
 240 int
 241 Convproc::impdata_link (uint32_t inp1,
 242                         uint32_t out1,
 243                         uint32_t inp2,
 244                         uint32_t out2)
 245 {
 246         uint32_t j;
 247
 248         if ((inp1 >= _ninp) || (out1 >= _nout))
 249                 return Converror::BAD_PARAM;
 250         if ((inp2 >= _ninp) || (out2 >= _nout))
 251                 return Converror::BAD_PARAM;
 252         if ((inp1 == inp2) && (out1 == out2))
 253                 return Converror::BAD_PARAM;
 254         if (_state != ST_STOP)
 255                 return Converror::BAD_STATE;
 256         try {
 257                 for (j = 0; j < _nlevels; j++) {
 258                         _convlev[j]->impdata_link (inp1, out1, inp2, out2);
 259                 }
 260         } catch (...) {
 261                 cleanup ();
 262                 return Converror::MEM_ALLOC;
 263         }
 264         return 0;
 265 }
 266
 267 int
 268 Convproc::reset (void)
 269 {
 270         uint32_t k;
 271
 272         if (_state == ST_IDLE)
 273                 return Converror::BAD_STATE;
 274         for (k = 0; k < _ninp; k++)
 275                 memset (_inpbuff[k], 0, _inpsize * sizeof (float));
 276         for (k = 0; k < _nout; k++)
 277                 memset (_outbuff[k], 0, _minpart * sizeof (float));
 278         for (k = 0; k < _nlevels; k++)
 279                 _convlev[k]->reset (_inpsize, _minpart, _inpbuff, _outbuff);
 280         return 0;
 281 }
 282
 283 int
 284 Convproc::start_process (int abspri, int policy)
 285 {
 286         uint32_t k;
 287
 288         if (_state != ST_STOP)
 289                 return Converror::BAD_STATE;
 290         _latecnt = 0;
 291         _inpoffs = 0;
 292         _outoffs = 0;
 293         reset ();
 294
 295         for (k = (_minpart == _quantum) ? 1 : 0; k < _nlevels; k++) {
 296                 _convlev[k]->start (abspri, policy);
 297         }
 298         _state = ST_PROC;
 299         return 0;
 300 }
 301
 302 int
 303 Convproc::process (bool sync)
 304 {
 305         uint32_t k;
 306         int      f = 0;
 307
 308         if (_state != ST_PROC)
 309                 return 0;
 310         _inpoffs += _quantum;
 311         if (_inpoffs == _inpsize)
 312                 _inpoffs = 0;
 313         _outoffs += _quantum;
 314         if (_outoffs == _minpart) {
 315                 _outoffs = 0;
 316                 for (k = 0; k < _nout; k++)
 317                         memset (_outbuff[k], 0, _minpart * sizeof (float));
 318                 for (k = 0; k < _nlevels; k++)
 319                         f |= _convlev[k]->readout (sync, _skipcnt);
 320                 if (_skipcnt < _minpart)
 321                         _skipcnt = 0;
 322                 else
 323                         _skipcnt -= _minpart;
 324                 if (f) {
 325                         if (++_latecnt >= 5) {
 326                                 if (~_options & OPT_LATE_CONTIN)
 327                                         stop_process ();
 328                                 f |= FL_LOAD;
 329                         }
 330                 } else
 331                         _latecnt = 0;
 332         }
 333         return f;
 334 }
 335
 336 int
 337 Convproc::stop_process (void)
 338 {
 339         uint32_t k;
 340
 341         if (_state != ST_PROC)
 342                 return Converror::BAD_STATE;
 343         for (k = 0; k < _nlevels; k++)
 344                 _convlev[k]->stop ();
 345         _state = ST_WAIT;
 346         return 0;
 347 }
 348
 349 int
 350 Convproc::cleanup (void)
 351 {
 352         uint32_t k;
 353
 354         while (!check_stop ()) {
 355                 usleep (100000);
 356         }
 357         for (k = 0; k < _ninp; k++) {
 358                 delete[] _inpbuff[k];
 359                 _inpbuff[k] = 0;
 360         }
 361         for (k = 0; k < _nout; k++) {
 362                 delete[] _outbuff[k];
 363                 _outbuff[k] = 0;
 364         }
 365         for (k = 0; k < _nlevels; k++) {
 366                 delete _convlev[k];
 367                 _convlev[k] = 0;
 368         }
 369
 370         _state   = ST_IDLE;
 371         _options = 0;
 372         _skipcnt = 0;
 373         _ninp    = 0;
 374         _nout    = 0;
 375         _quantum = 0;
 376         _minpart = 0;
 377         _maxpart = 0;
 378         _nlevels = 0;
 379         _latecnt = 0;
 380         return 0;
 381 }
 382
 383 bool
 384 Convproc::check_stop (void)
 385 {
 386         uint32_t k;
 387
 388         for (k = 0; (k < _nlevels) && (_convlev[k]->_stat == Convlevel::ST_IDLE); k++)
 389                 ;
 390         if (k == _nlevels) {
 391                 _state = ST_STOP;
 392                 return true;
 393         }
 394         return false;
 395 }
 396
 397 void
 398 Convproc::print (FILE* F)
 399 {
 400         uint32_t k;
 401
 402         for (k = 0; k < _nlevels; k++)
 403                 _convlev[k]->print (F);
 404 }
 405
 406 typedef float FV4 __attribute__ ((vector_size (16)));
 407
 408 Convlevel::Convlevel (void)
 409         : _stat (ST_IDLE)
 410         , _npar (0)
 411         , _parsize (0)
 412         , _options (0)
 413 #ifndef PTW32_VERSION
 414         , _pthr (0)
 415 #endif
 416         , _inp_list (0)
 417         , _out_list (0)
 418         , _plan_r2c (0)
 419         , _plan_c2r (0)
 420         , _time_data (0)
 421         , _prep_data (0)
 422         , _freq_data (0)
 423 {
 424 }
 425
 426 Convlevel::~Convlevel (void)
 427 {
 428         cleanup ();
 429 }
 430
 431 void
 432 Convlevel::configure (int      prio,
 433                       uint32_t offs,
 434                       uint32_t npar,
 435                       uint32_t parsize,
 436                       uint32_t options)
 437 {
 438         int fftwopt = (options & OPT_FFTW_MEASURE) ? FFTW_MEASURE : FFTW_ESTIMATE;
 439
 440         _prio    = prio;
 441         _offs    = offs;
 442         _npar    = npar;
 443         _parsize = parsize;
 444         _options = options;
 445
 446         _time_data = calloc_real (2 * _parsize);
 447         _prep_data = calloc_real (2 * _parsize);
 448         _freq_data = calloc_complex (_parsize + 1);
 449         _plan_r2c  = fftwf_plan_dft_r2c_1d (2 * _parsize, _time_data, _freq_data, fftwopt);
 450         _plan_c2r  = fftwf_plan_dft_c2r_1d (2 * _parsize, _freq_data, _time_data, fftwopt);
 451         if (_plan_r2c && _plan_c2r)
 452                 return;
 453         throw (Converror (Converror::MEM_ALLOC));
 454 }
 455
 456 void
 457 Convlevel::impdata_write (uint32_t inp,
 458                           uint32_t out,
 459                           int32_t  step,
 460                           float*   data,
 461                           int32_t  i0,
 462                           int32_t  i1,
 463                           bool     create)
 464 {
 465         uint32_t       k;
 466         int32_t        j, j0, j1, n;
 467         float          norm;
 468         fftwf_complex* fftb;
 469         Macnode*       M;
 470
 471         n  = i1 - i0;
 472         i0 = _offs - i0;
 473         i1 = i0 + _npar * _parsize;
 474         if ((i0 >= n) || (i1 <= 0))
 475                 return;
 476
 477         if (create) {
 478                 M = findmacnode (inp, out, true);
 479                 if (M == 0 || M->_link)
 480                         return;
 481                 if (M->_fftb == 0)
 482                         M->alloc_fftb (_npar);
 483         } else {
 484                 M = findmacnode (inp, out, false);
 485                 if (M == 0 || M->_link || M->_fftb == 0)
 486                         return;
 487         }
 488
 489         norm = 0.5f / _parsize;
 490         for (k = 0; k < _npar; k++) {
 491                 i1 = i0 + _parsize;
 492                 if ((i0 < n) && (i1 > 0)) {
 493                         fftb = M->_fftb[k];
 494                         if (fftb == 0 && create) {
 495                                 M->_fftb[k] = fftb = calloc_complex (_parsize + 1);
 496                         }
 497                         if (fftb && data) {
 498                                 memset (_prep_data, 0, 2 * _parsize * sizeof (float));
 499                                 j0 = (i0 < 0) ? 0 : i0;
 500                                 j1 = (i1 > n) ? n : i1;
 501                                 for (j                     = j0; j < j1; j++)
 502                                         _prep_data[j - i0] = norm * data[j * step];
 503                                 fftwf_execute_dft_r2c (_plan_r2c, _prep_data, _freq_data);
 504 #ifdef ENABLE_VECTOR_MODE
 505                                 if (_options & OPT_VECTOR_MODE)
 506                                         fftswap (_freq_data);
 507 #endif
 508                                 for (j = 0; j <= (int)_parsize; j++) {
 509                                         fftb[j][0] += _freq_data[j][0];
 510                                         fftb[j][1] += _freq_data[j][1];
 511                                 }
 512                         }
 513                 }
 514                 i0 = i1;
 515         }
 516 }
 517
 518 void
 519 Convlevel::impdata_clear (uint32_t inp, uint32_t out)
 520 {
 521         uint32_t i;
 522         Macnode* M;
 523
 524         M = findmacnode (inp, out, false);
 525         if (M == 0 || M->_link || M->_fftb == 0)
 526                 return;
 527         for (i = 0; i < _npar; i++) {
 528                 if (M->_fftb[i]) {
 529                         memset (M->_fftb[i], 0, (_parsize + 1) * sizeof (fftwf_complex));
 530                 }
 531         }
 532 }
 533
 534 void
 535 Convlevel::impdata_link (uint32_t inp1,
 536                          uint32_t out1,
 537                          uint32_t inp2,
 538                          uint32_t out2)
 539 {
 540         Macnode* M1;
 541         Macnode* M2;
 542
 543         M1 = findmacnode (inp1, out1, false);
 544         if (!M1)
 545                 return;
 546         M2 = findmacnode (inp2, out2, true);
 547         M2->free_fftb ();
 548         M2->_link = M1;
 549 }
 550
 551 void
 552 Convlevel::reset (uint32_t inpsize,
 553                   uint32_t outsize,
 554                   float**  inpbuff,
 555                   float**  outbuff)
 556 {
 557         uint32_t i;
 558         Inpnode* X;
 559         Outnode* Y;
 560
 561         _inpsize = inpsize;
 562         _outsize = outsize;
 563         _inpbuff = inpbuff;
 564         _outbuff = outbuff;
 565         for (X = _inp_list; X; X = X->_next) {
 566                 for (i = 0; i < _npar; i++) {
 567                         memset (X->_ffta[i], 0, (_parsize + 1) * sizeof (fftwf_complex));
 568                 }
 569         }
 570         for (Y = _out_list; Y; Y = Y->_next) {
 571                 for (i = 0; i < 3; i++) {
 572                         memset (Y->_buff[i], 0, _parsize * sizeof (float));
 573                 }
 574         }
 575         if (_parsize == _outsize) {
 576                 _outoffs = 0;
 577                 _inpoffs = 0;
 578         } else {
 579                 _outoffs = _parsize / 2;
 580                 _inpoffs = _inpsize - _outoffs;
 581         }
 582         _bits  = _parsize / _outsize;
 583         _wait  = 0;
 584         _ptind = 0;
 585         _opind = 0;
 586         _trig.init (0, 0);
 587         _done.init (0, 0);
 588 }
 589
 590 void
 591 Convlevel::start (int abspri, int policy)
 592 {
 593         int                min, max;
 594         pthread_attr_t     attr;
 595         struct sched_param parm;
 596
 597 #ifndef PTW32_VERSION
 598         _pthr = 0;
 599 #endif
 600         min   = sched_get_priority_min (policy);
 601         max   = sched_get_priority_max (policy);
 602         abspri += _prio;
 603         if (abspri > max)
 604                 abspri = max;
 605         if (abspri < min)
 606                 abspri      = min;
 607         parm.sched_priority = abspri;
 608         pthread_attr_init (&attr);
 609         pthread_attr_setdetachstate (&attr, PTHREAD_CREATE_DETACHED);
 610         pthread_attr_setschedpolicy (&attr, policy);
 611         pthread_attr_setschedparam (&attr, &parm);
 612         pthread_attr_setscope (&attr, PTHREAD_SCOPE_SYSTEM);
 613         pthread_attr_setinheritsched (&attr, PTHREAD_EXPLICIT_SCHED);
 614         pthread_attr_setstacksize (&attr, 0x10000);
 615         pthread_create (&_pthr, &attr, static_main, this);
 616         pthread_attr_destroy (&attr);
 617 }
 618
 619 void
 620 Convlevel::stop (void)
 621 {
 622         if (_stat != ST_IDLE) {
 623                 _stat = ST_TERM;
 624                 _trig.post ();
 625         }
 626 }
 627
 628 void
 629 Convlevel::cleanup (void)
 630 {
 631         Inpnode *X, *X1;
 632         Outnode *Y, *Y1;
 633         Macnode *M, *M1;
 634
 635         X = _inp_list;
 636         while (X) {
 637                 X1 = X->_next;
 638                 delete X;
 639                 X = X1;
 640         }
 641         _inp_list = 0;
 642
 643         Y = _out_list;
 644         while (Y) {
 645                 M = Y->_list;
 646                 while (M) {
 647                         M1 = M->_next;
 648                         delete M;
 649                         M = M1;
 650                 }
 651                 Y1 = Y->_next;
 652                 delete Y;
 653                 Y = Y1;
 654         }
 655         _out_list = 0;
 656
 657         fftwf_destroy_plan (_plan_r2c);
 658         fftwf_destroy_plan (_plan_c2r);
 659         fftwf_free (_time_data);
 660         fftwf_free (_prep_data);
 661         fftwf_free (_freq_data);
 662         _plan_r2c  = 0;
 663         _plan_c2r  = 0;
 664         _time_data = 0;
 665         _prep_data = 0;
 666         _freq_data = 0;
 667 }
 668
 669 void*
 670 Convlevel::static_main (void* arg)
 671 {
 672         ((Convlevel*)arg)->main ();
 673         return 0;
 674 }
 675
 676 void
 677 Convlevel::main (void)
 678 {
 679         _stat = ST_PROC;
 680         while (true) {
 681                 _trig.wait ();
 682                 if (_stat == ST_TERM) {
 683                         _stat = ST_IDLE;
 684 #ifndef PTW32_VERSION
 685                         _pthr = 0;
 686 #endif
 687                         return;
 688                 }
 689                 process (false);
 690                 _done.post ();
 691         }
 692 }
 693
 694 void
 695 Convlevel::process (bool skip)
 696 {
 697         uint32_t       i, i1, j, k, n1, n2, opi1, opi2;
 698         Inpnode*       X;
 699         Macnode*       M;
 700         Outnode*       Y;
 701         fftwf_complex* ffta;
 702         fftwf_complex* fftb;
 703         float*         inpd;
 704         float*         outd;
 705
 706         i1       = _inpoffs;
 707         n1       = _parsize;
 708         n2       = 0;
 709         _inpoffs = i1 + n1;
 710         if (_inpoffs >= _inpsize) {
 711                 _inpoffs -= _inpsize;
 712                 n2 = _inpoffs;
 713                 n1 -= n2;
 714         }
 715
 716         opi1 = (_opind + 1) % 3;
 717         opi2 = (_opind + 2) % 3;
 718
 719         for (X = _inp_list; X; X = X->_next) {
 720                 inpd = _inpbuff[X->_inp];
 721                 if (n1)
 722                         memcpy (_time_data, inpd + i1, n1 * sizeof (float));
 723                 if (n2)
 724                         memcpy (_time_data + n1, inpd, n2 * sizeof (float));
 725                 memset (_time_data + _parsize, 0, _parsize * sizeof (float));
 726                 fftwf_execute_dft_r2c (_plan_r2c, _time_data, X->_ffta[_ptind]);
 727 #ifdef ENABLE_VECTOR_MODE
 728                 if (_options & OPT_VECTOR_MODE)
 729                         fftswap (X->_ffta[_ptind]);
 730 #endif
 731         }
 732
 733         if (skip) {
 734                 for (Y = _out_list; Y; Y = Y->_next) {
 735                         outd = Y->_buff[opi2];
 736                         memset (outd, 0, _parsize * sizeof (float));
 737                 }
 738         } else {
 739                 for (Y = _out_list; Y; Y = Y->_next) {
 740                         memset (_freq_data, 0, (_parsize + 1) * sizeof (fftwf_complex));
 741                         for (M = Y->_list; M; M = M->_next) {
 742                                 X = M->_inpn;
 743                                 i = _ptind;
 744                                 for (j = 0; j < _npar; j++) {
 745                                         ffta = X->_ffta[i];
 746                                         fftb = M->_link ? M->_link->_fftb[j] : M->_fftb[j];
 747                                         if (fftb) {
 748 #ifdef ENABLE_VECTOR_MODE
 749                                                 if (_options & OPT_VECTOR_MODE) {
 750                                                         FV4* A = (FV4*)ffta;
 751                                                         FV4* B = (FV4*)fftb;
 752                                                         FV4* D = (FV4*)_freq_data;
 753                                                         for (k = 0; k < _parsize; k += 4) {
 754                                                                 D[0] += A[0] * B[0] - A[1] * B[1];
 755                                                                 D[1] += A[0] * B[1] + A[1] * B[0];
 756                                                                 A += 2;
 757                                                                 B += 2;
 758                                                                 D += 2;
 759                                                         }
 760                                                         _freq_data[_parsize][0] += ffta[_parsize][0] * fftb[_parsize][0];
 761                                                         _freq_data[_parsize][1] = 0;
 762                                                 } else
 763 #endif
 764                                                 {
 765                                                         for (k = 0; k <= _parsize; k++) {
 766                                                                 _freq_data[k][0] += ffta[k][0] * fftb[k][0] - ffta[k][1] * fftb[k][1];
 767                                                                 _freq_data[k][1] += ffta[k][0] * fftb[k][1] + ffta[k][1] * fftb[k][0];
 768                                                         }
 769                                                 }
 770                                         }
 771                                         if (i == 0)
 772                                                 i = _npar;
 773                                         i--;
 774                                 }
 775                         }
 776
 777 #ifdef ENABLE_VECTOR_MODE
 778                         if (_options & OPT_VECTOR_MODE)
 779                                 fftswap (_freq_data);
 780 #endif
 781                         fftwf_execute_dft_c2r (_plan_c2r, _freq_data, _time_data);
 782                         outd = Y->_buff[opi1];
 783                         for (k = 0; k < _parsize; k++)
 784                                 outd[k] += _time_data[k];
 785                         outd = Y->_buff[opi2];
 786                         memcpy (outd, _time_data + _parsize, _parsize * sizeof (float));
 787                 }
 788         }
 789
 790         _ptind++;
 791         if (_ptind == _npar)
 792                 _ptind = 0;
 793 }
 794
 795 int
 796 Convlevel::readout (bool sync, uint32_t skipcnt)
 797 {
 798         uint32_t i;
 799         float *  p, *q;
 800         Outnode* Y;
 801
 802         _outoffs += _outsize;
 803         if (_outoffs == _parsize) {
 804                 _outoffs = 0;
 805                 if (_stat == ST_PROC) {
 806                         while (_wait) {
 807                                 if (sync)
 808                                         _done.wait ();
 809                                 else if (_done.trywait ())
 810                                         break;
 811                                 _wait--;
 812                         }
 813                         if (++_opind == 3)
 814                                 _opind = 0;
 815                         _trig.post ();
 816                         _wait++;
 817                 } else {
 818                         process (skipcnt >= 2 * _parsize);
 819                         if (++_opind == 3)
 820                                 _opind = 0;
 821                 }
 822         }
 823
 824         for (Y = _out_list; Y; Y = Y->_next) {
 825                 p = Y->_buff[_opind] + _outoffs;
 826                 q = _outbuff[Y->_out];
 827                 for (i = 0; i < _outsize; i++)
 828                         q[i] += p[i];
 829         }
 830
 831         return (_wait > 1) ? _bits : 0;
 832 }
 833
 834 void
 835 Convlevel::print (FILE* F)
 836 {
 837         fprintf (F, "prio = %4d, offs = %6d,  parsize = %5d,  npar = %3d\n", _prio, _offs, _parsize, _npar);
 838 }
 839
 840 Macnode*
 841 Convlevel::findmacnode (uint32_t inp, uint32_t out, bool create)
 842 {
 843         Inpnode* X;
 844         Outnode* Y;
 845         Macnode* M;
 846
 847         for (X = _inp_list; X && (X->_inp != inp); X = X->_next)
 848                 ;
 849         if (!X) {
 850                 if (!create)
 851                         return 0;
 852                 X         = new Inpnode (inp);
 853                 X->_next  = _inp_list;
 854                 _inp_list = X;
 855                 X->alloc_ffta (_npar, _parsize);
 856         }
 857
 858         for (Y = _out_list; Y && (Y->_out != out); Y = Y->_next)
 859                 ;
 860         if (!Y) {
 861                 if (!create)
 862                         return 0;
 863                 Y         = new Outnode (out, _parsize);
 864                 Y->_next  = _out_list;
 865                 _out_list = Y;
 866         }
 867
 868         for (M = Y->_list; M && (M->_inpn != X); M = M->_next)
 869                 ;
 870         if (!M) {
 871                 if (!create)
 872                         return 0;
 873                 M        = new Macnode (X);
 874                 M->_next = Y->_list;
 875                 Y->_list = M;
 876         }
 877
 878         return M;
 879 }
 880
 881 #ifdef ENABLE_VECTOR_MODE
 882
 883 void
 884 Convlevel::fftswap (fftwf_complex* p)
 885 {
 886         uint32_t n = _parsize;
 887         float    a, b;
 888
 889         while (n) {
 890                 a       = p[2][0];
 891                 b       = p[3][0];
 892                 p[2][0] = p[0][1];
 893                 p[3][0] = p[1][1];
 894                 p[0][1] = a;
 895                 p[1][1] = b;
 896                 p += 4;
 897                 n -= 4;
 898         }
 899 }
 900
 901 #endif
 902
 903 Inpnode::Inpnode (uint16_t inp)
 904         : _next (0)
 905         , _ffta (0)
 906         , _npar (0)
 907         , _inp (inp)
 908 {
 909 }
 910
 911 Inpnode::~Inpnode (void)
 912 {
 913         free_ffta ();
 914 }
 915
 916 void
 917 Inpnode::alloc_ffta (uint16_t npar, int32_t size)
 918 {
 919         _npar = npar;
 920         _ffta = new fftwf_complex*[_npar];
 921         for (int i = 0; i < _npar; i++) {
 922                 _ffta[i] = calloc_complex (size + 1);
 923         }
 924 }
 925
 926 void
 927 Inpnode::free_ffta (void)
 928 {
 929         if (!_ffta)
 930                 return;
 931         for (uint16_t i = 0; i < _npar; i++) {
 932                 fftwf_free (_ffta[i]);
 933         }
 934         delete[] _ffta;
 935         _ffta = 0;
 936         _npar = 0;
 937 }
 938
 939 Macnode::Macnode (Inpnode* inpn)
 940         : _next (0)
 941         , _inpn (inpn)
 942         , _link (0)
 943         , _fftb (0)
 944         , _npar (0)
 945 {
 946 }
 947
 948 Macnode::~Macnode (void)
 949 {
 950         free_fftb ();
 951 }
 952
 953 void
 954 Macnode::alloc_fftb (uint16_t npar)
 955 {
 956         _npar = npar;
 957         _fftb = new fftwf_complex*[_npar];
 958         for (uint16_t i = 0; i < _npar; i++) {
 959                 _fftb[i] = 0;
 960         }
 961 }
 962
 963 void
 964 Macnode::free_fftb (void)
 965 {
 966         if (!_fftb)
 967                 return;
 968         for (uint16_t i = 0; i < _npar; i++) {
 969                 fftwf_free (_fftb[i]);
 970         }
 971         delete[] _fftb;
 972         _fftb = 0;
 973         _npar = 0;
 974 }
 975
 976 Outnode::Outnode (uint16_t out, int32_t size)
 977         : _next (0)
 978         , _list (0)
 979         , _out (out)
 980 {
 981         _buff[0] = calloc_real (size);
 982         _buff[1] = calloc_real (size);
 983         _buff[2] = calloc_real (size);
 984 }
 985
 986 Outnode::~Outnode (void)
 987 {
 988         fftwf_free (_buff[0]);
 989         fftwf_free (_buff[1]);
 990         fftwf_free (_buff[2]);
 991 }