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