libs/zita-resampler/resampler.cc

   1 // ----------------------------------------------------------------------------
   2 //
   3 //  Copyright (C) 2006-2012 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 <stdlib.h>
  21 #include <stdio.h>
  22 #include <string.h>
  23 #include <math.h>
  24
  25 #include "zita-resampler/resampler.h"
  26
  27 using namespace ArdourZita;
  28
  29 static unsigned int gcd (unsigned int a, unsigned int b)
  30 {
  31         if (a == 0) return b;
  32         if (b == 0) return a;
  33         while (1) {
  34                 if (a > b) {
  35                         a = a % b;
  36                         if (a == 0) return b;
  37                         if (a == 1) return 1;
  38                 } else {
  39                         b = b % a;
  40                         if (b == 0) return a;
  41                         if (b == 1) return 1;
  42                 }
  43         }
  44         return 1;
  45 }
  46
  47 Resampler::Resampler (void)
  48         : _table (0)
  49         , _nchan (0)
  50         , _buff  (0)
  51 {
  52         reset ();
  53 }
  54
  55 Resampler::~Resampler (void)
  56 {
  57         clear ();
  58 }
  59
  60 int
  61 Resampler::setup (unsigned int fs_inp,
  62                   unsigned int fs_out,
  63                   unsigned int nchan,
  64                   unsigned int hlen)
  65 {
  66         if ((hlen < 8) || (hlen > 96)) return 1;
  67         return setup (fs_inp, fs_out, nchan, hlen, 1.0 - 2.6 / hlen);
  68 }
  69
  70 int
  71 Resampler::setup (unsigned int fs_inp,
  72                   unsigned int fs_out,
  73                   unsigned int nchan,
  74                   unsigned int hlen,
  75                   double       frel)
  76 {
  77         unsigned int       g, h, k, n, s;
  78         double             r;
  79         float              *B = 0;
  80         Resampler_table    *T = 0;
  81
  82         k = s = 0;
  83         if (fs_inp && fs_out && nchan) {
  84                 r = (double) fs_out / (double) fs_inp;
  85                 g = gcd (fs_out, fs_inp);
  86                 n = fs_out / g;
  87                 s = fs_inp / g;
  88                 if ((16 * r >= 1) && (n <= 1000)) {
  89                         h = hlen;
  90                         k = 250;
  91                         if (r < 1) {
  92                                 frel *= r;
  93                                 h = (unsigned int)(ceil (h / r));
  94                                 k = (unsigned int)(ceil (k / r));
  95                         }
  96                         T = Resampler_table::create (frel, h, n);
  97                         B = new float [nchan * (2 * h - 1 + k)];
  98                 }
  99         }
 100         clear ();
 101         if (T) {
 102                 _table = T;
 103                 _buff  = B;
 104                 _nchan = nchan;
 105                 _inmax = k;
 106                 _pstep = s;
 107                 return reset ();
 108         }
 109         else return 1;
 110 }
 111
 112 void
 113 Resampler::clear (void)
 114 {
 115         Resampler_table::destroy (_table);
 116         delete[] _buff;
 117         _buff  = 0;
 118         _table = 0;
 119         _nchan = 0;
 120         _inmax = 0;
 121         _pstep = 0;
 122         reset ();
 123 }
 124
 125 double
 126 Resampler::inpdist (void) const
 127 {
 128         if (!_table) return 0;
 129         return (int)(_table->_hl + 1 - _nread) - (double)_phase / _table->_np;
 130 }
 131
 132 int
 133 Resampler::inpsize (void) const
 134 {
 135         if (!_table) return 0;
 136         return 2 * _table->_hl;
 137 }
 138
 139 int
 140 Resampler::reset (void)
 141 {
 142         if (!_table) return 1;
 143
 144         inp_count = 0;
 145         out_count = 0;
 146         inp_data = 0;
 147         out_data = 0;
 148         _index = 0;
 149         _nread = 0;
 150         _nzero = 0;
 151         _phase = 0;
 152         if (_table) {
 153                 _nread = 2 * _table->_hl;
 154                 return 0;
 155         }
 156         return 1;
 157 }
 158
 159 int
 160 Resampler::process (void)
 161 {
 162         unsigned int   hl, ph, np, dp, in, nr, nz, i, n, c;
 163         float          *p1, *p2;
 164
 165         if (!_table) return 1;
 166
 167         hl = _table->_hl;
 168         np = _table->_np;
 169         dp = _pstep;
 170         in = _index;
 171         nr = _nread;
 172         ph = _phase;
 173         nz = _nzero;
 174         n = (2 * hl - nr) * _nchan;
 175         p1 = _buff + in * _nchan;
 176         p2 = p1 + n;
 177
 178         while (out_count) {
 179                 if (nr) {
 180                         if (inp_count == 0) break;
 181                         if (inp_data) {
 182                                 for (c = 0; c < _nchan; c++) p2 [c] = inp_data [c];
 183                                 inp_data += _nchan;
 184                                 nz = 0;
 185                         } else {
 186                                 for (c = 0; c < _nchan; c++) p2 [c] = 0;
 187                                 if (nz < 2 * hl) nz++;
 188                         }
 189                         nr--;
 190                         p2 += _nchan;
 191                         inp_count--;
 192                 } else {
 193                         if (out_data) {
 194                                 if (nz < 2 * hl) {
 195                                         float *c1 = _table->_ctab + hl * ph;
 196                                         float *c2 = _table->_ctab + hl * (np - ph);
 197                                         for (c = 0; c < _nchan; c++) {
 198                                                 float *q1 = p1 + c;
 199                                                 float *q2 = p2 + c;
 200                                                 float s = 1e-20f;
 201                                                 for (i = 0; i < hl; i++) {
 202                                                         q2 -= _nchan;
 203                                                         s += *q1 * c1 [i] + *q2 * c2 [i];
 204                                                         q1 += _nchan;
 205                                                 }
 206                                                 *out_data++ = s - 1e-20f;
 207                                         }
 208                                 } else {
 209                                         for (c = 0; c < _nchan; c++) *out_data++ = 0;
 210                                 }
 211                         }
 212                         out_count--;
 213
 214                         ph += dp;
 215                         if (ph >= np) {
 216                                 nr = ph / np;
 217                                 ph -= nr * np;
 218                                 in += nr;
 219                                 p1 += nr * _nchan;;
 220                                 if (in >= _inmax) {
 221                                         n = (2 * hl - nr) * _nchan;
 222                                         memcpy (_buff, p1, n * sizeof (float));
 223                                         in = 0;
 224                                         p1 = _buff;
 225                                         p2 = p1 + n;
 226                                 }
 227                         }
 228                 }
 229         }
 230         _index = in;
 231         _nread = nr;
 232         _phase = ph;
 233         _nzero = nz;
 234
 235         return 0;
 236 }