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