libs/zita-resampler/vmresampler.cc

   1 // ----------------------------------------------------------------------------
   2 //
   3 //  Copyright (C) 2006-2013 Fons Adriaensen <fons@linuxaudio.org>
   4 //  Copyright (C) 2017 Robin Gareus <robin@gareus.org>
   5 //
   6 //  This program is free software; you can redistribute it and/or modify
   7 //  it under the terms of the GNU General Public License as published by
   8 //  the Free Software Foundation; either version 3 of the License, or
   9 //  (at your option) any later version.
  10 //
  11 //  This program is distributed in the hope that it will be useful,
  12 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 //  GNU General Public License for more details.
  15 //
  16 //  You should have received a copy of the GNU General Public License
  17 //  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18 //
  19 // ----------------------------------------------------------------------------
  20
  21 #include <stdlib.h>
  22 #include <stdio.h>
  23 #include <string.h>
  24 #include <math.h>
  25 #include <algorithm>
  26
  27 #include "zita-resampler/vmresampler.h"
  28
  29 using namespace ArdourZita;
  30
  31 VMResampler::VMResampler (void)
  32         : _table (0)
  33   , _buff  (0)
  34   , _c1 (0)
  35   , _c2 (0)
  36 {
  37         reset ();
  38 }
  39
  40 VMResampler::~VMResampler (void)
  41 {
  42         clear ();
  43 }
  44
  45 int
  46 VMResampler::setup (unsigned int hlen)
  47 {
  48         if ((hlen < 8) || (hlen > 96)) return 1;
  49         return setup (hlen, 1.0 - 2.6 / hlen);
  50 }
  51
  52 int
  53 VMResampler::setup (unsigned int hlen, double frel)
  54 {
  55         unsigned int       h, k, n;
  56         double             s;
  57         Resampler_table    *T = 0;
  58
  59         n = NPHASE;
  60         s = n;
  61         h = hlen;
  62         k = 250;
  63         T = Resampler_table::create (frel, h, n);
  64         clear ();
  65         if (T) {
  66                 _table = T;
  67                 _buff  = new float [2 * h - 1 + k];
  68                 _c1 = new float [2 * h];
  69                 _c2 = new float [2 * h];
  70                 _inmax = k;
  71                 _pstep = s;
  72                 _qstep = s;
  73                 _wstep = 1;
  74                 return reset ();
  75         }
  76         else return 1;
  77 }
  78
  79 void
  80 VMResampler::clear (void)
  81 {
  82         Resampler_table::destroy (_table);
  83         delete[] _buff;
  84         delete[] _c1;
  85         delete[] _c2;
  86         _buff  = 0;
  87         _c1 = 0;
  88         _c2 = 0;
  89         _table = 0;
  90         _inmax = 0;
  91         _pstep = 0;
  92         _qstep = 0;
  93         _wstep = 1;
  94         reset ();
  95 }
  96
  97 void
  98 VMResampler::set_phase (double p)
  99 {
 100         if (!_table) return;
 101         _phase = (p - floor (p)) * _table->_np;
 102 }
 103
 104 void
 105 VMResampler::set_rrfilt (double t)
 106 {
 107         if (!_table) return;
 108         _wstep =  (t < 1) ? 1 : 1 - exp (-1 / t);
 109 }
 110
 111 double
 112 VMResampler::set_rratio (double r)
 113 {
 114         if (!_table) return 0;
 115         if (r > 16.0) r = 16.0;
 116         if (r < 0.02) r = 0.02;
 117
 118         _qstep = _table->_np / r;
 119
 120         if (_qstep < 4.) {
 121                 _qstep = 4.;
 122         }
 123         if (_qstep > 2. * _table->_np * _table->_hl) {
 124                 _qstep = 2. * _table->_np * _table->_hl;
 125         }
 126         return _table->_np / _qstep;
 127 }
 128
 129 double
 130 VMResampler::inpdist (void) const
 131 {
 132         if (!_table) return 0;
 133         return (int)(_table->_hl + 1 - _nread) - _phase / _table->_np;
 134 }
 135
 136 int
 137 VMResampler::inpsize (void) const
 138 {
 139         if (!_table) return 0;
 140         return 2 * _table->_hl;
 141 }
 142
 143 int
 144 VMResampler::reset (void)
 145 {
 146         if (!_table) return 1;
 147
 148         inp_count = 0;
 149         out_count = 0;
 150         inp_data = 0;
 151         out_data = 0;
 152         _index = 0;
 153         _phase = 0;
 154         _nread = 2 * _table->_hl;
 155
 156         memset (_buff, 0, sizeof(float) * (_nread + 249));
 157         _nread -= _table->_hl - 1;
 158         return 0;
 159 }
 160
 161 int
 162 VMResampler::process (void)
 163 {
 164         unsigned int   in, nr, n;
 165         double         ph, dp;
 166         float          a, *p1, *p2;
 167
 168         if (!_table) return 1;
 169
 170         const int hl = _table->_hl;
 171         const unsigned int np = _table->_np;
 172         in = _index;
 173         nr = _nread;
 174         ph = _phase;
 175         dp = _pstep;
 176         n = 2 * hl - nr;
 177
 178 #if 1
 179         /* optimized full-cycle no-resampling */
 180         if (dp == np && _qstep == np && nr == 1 && inp_count == out_count) {
 181
 182                 if (out_count >= n) {
 183                         const unsigned int h1 = hl - 1;
 184                         const unsigned int head = out_count - h1;
 185                         const unsigned int tail = out_count - n;
 186
 187                         memcpy (out_data, &_buff[in + hl], h1 * sizeof (float));
 188                         memcpy (&out_data[h1], inp_data, head * sizeof (float));
 189                         memcpy (_buff, &inp_data[tail], n * sizeof (float));
 190                         _index = 0;
 191                         inp_count = 0;
 192                         out_count = 0;
 193                         return 0;
 194                 }
 195
 196                 while (out_count) {
 197                         unsigned int to_proc = std::min (out_count, _inmax - in);
 198                         memcpy (&_buff[in + n], inp_data, to_proc * sizeof (float));
 199                         memcpy (out_data, &_buff[in + hl], to_proc * sizeof (float));
 200                         inp_data  += to_proc;
 201                         out_data  += to_proc;
 202                         out_count -= to_proc;
 203                         in        += to_proc;
 204                         if (in >= _inmax) {
 205                                 memcpy (_buff, _buff + in, (2 * hl - 1) * sizeof (float));
 206                                 in = 0;
 207                         }
 208                 }
 209                 inp_count = out_count;
 210                 _index = in;
 211                 return 0;
 212         }
 213 #endif
 214
 215         p1 = _buff + in;
 216         p2 = p1 + n;
 217
 218         while (out_count) {
 219                 if (nr) {
 220                         if (inp_count == 0) break;
 221                         *p2 = *inp_data;
 222                         inp_data++;
 223                         nr--;
 224                         p2++;
 225                         inp_count--;
 226                 } else {
 227                         if (dp == np) {
 228                                 const unsigned int k = (unsigned int) /*floor (ph / np) +*/ hl;
 229                                 *out_data++ = p1[k];
 230                         } else {
 231                                 const unsigned int k = (unsigned int) ph;
 232                                 const float bb = (float)(ph - k);
 233                                 const float aa = 1.0f - bb;
 234                                 float const* cq1 = _table->_ctab + hl * k;
 235                                 float const* cq2 = _table->_ctab + hl * (np - k);
 236                                 for (int i = 0; i < hl; i++) {
 237                                         _c1 [i] = aa * cq1 [i] + bb * cq1 [i + hl];
 238                                         _c2 [i] = aa * cq2 [i] + bb * cq2 [i - hl];
 239                                 }
 240
 241                                 a = 1e-25f;
 242                                 for (int i = 0; i < hl; i++) {
 243                                         a += p1[i] * _c1 [i] + p2[-i-1] * _c2 [i];
 244                                 }
 245                                 *out_data++ = a - 1e-25f;
 246                         }
 247                         out_count--;
 248
 249                         const double dd = _qstep - dp;
 250                         if (fabs (dd) < 1e-12) {
 251                                 dp = _qstep;
 252                         } else {
 253                                 dp += _wstep * dd;
 254                         }
 255                         ph += dp;
 256
 257                         if (ph >= np) {
 258                                 nr = (unsigned int) floor (ph / np);
 259                                 ph -= nr * np;
 260                                 in += nr;
 261                                 p1 += nr;
 262                                 if (in >= _inmax) {
 263                                         n = (2 * hl - nr);
 264                                         memcpy (_buff, p1, n * sizeof (float));
 265                                         in = 0;
 266                                         p1 = _buff;
 267                                         p2 = p1 + n;
 268                                 }
 269                         }
 270                 }
 271         }
 272         _index = in;
 273         _nread = nr;
 274         _phase = ph;
 275         _pstep = dp;
 276
 277         return 0;
 278 }