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