source/modules/zita-resampler/cresampler.cc

   1 // ----------------------------------------------------------------------------
   2 //
   3 //  Copyright (C) 2013 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 <stdlib.h>
  22 #include <stdio.h>
  23 #include <string.h>
  24 #include <math.h>
  25 #include "cresampler.h"
  26
  27
  28 CResampler::CResampler (void) :
  29     _nchan (0),
  30     _buff  (0)
  31 {
  32     reset ();
  33 }
  34
  35
  36 CResampler::~CResampler (void)
  37 {
  38     clear ();
  39 }
  40
  41
  42 int CResampler::setup (double       ratio,
  43                        unsigned int nchan)
  44 {
  45     if (! nchan) return 1;
  46     clear ();
  47     _inmax = 50;
  48     _buff = new float [nchan * (3 + _inmax)];
  49     _nchan = nchan;
  50     _pstep = 1 / ratio;
  51     return reset ();
  52 }
  53
  54
  55 void CResampler::clear (void)
  56 {
  57     delete[] _buff;
  58     _buff  = 0;
  59     _nchan = 0;
  60     _inmax = 0;
  61     _pstep = 0;
  62     reset ();
  63 }
  64
  65
  66 void CResampler::set_phase (double p)
  67 {
  68     _phase = p - floor (p);
  69 }
  70
  71
  72 void CResampler::set_ratio (double r)
  73 {
  74     _pstep = 1.0 / r;
  75 }
  76
  77
  78 double CResampler::inpdist (void) const
  79 {
  80     return (int)(3 - _nread) - _phase;
  81 }
  82
  83
  84 int CResampler::inpsize (void) const
  85 {
  86     return 4;
  87 }
  88
  89
  90 int CResampler::reset (void)
  91 {
  92     inp_count = 0;
  93     out_count = 0;
  94     inp_data = 0;
  95     out_data = 0;
  96     _index = 0;
  97     _phase = 0;
  98     _nread = 4;
  99     _nzero = 0;
 100     return 0;
 101 }
 102
 103
 104 int CResampler::process (void)
 105 {
 106     unsigned int   in, nr, n, c;
 107     int            nz;
 108     double         ph;
 109     float          *pb, a, b, d, m0, m1, m2, m3;
 110
 111     in = _index;
 112     nr = _nread;
 113     nz = _nzero;
 114     ph = _phase;
 115     pb = _buff + in * _nchan;
 116
 117     while (out_count)
 118     {
 119         if (nr)
 120         {
 121             if (inp_count == 0) break;
 122             n = (4 - nr) * _nchan;
 123             if (inp_data)
 124             {
 125                 for (c = 0; c < _nchan; c++) pb [n + c] = inp_data [c];
 126                 inp_data += _nchan;
 127                 nz = 0;
 128             }
 129             else
 130             {
 131                 for (c = 0; c < _nchan; c++) pb [n + c] = 0;
 132                 if (nz < 4) nz++;
 133             }
 134             nr--;
 135             inp_count--;
 136         }
 137         else
 138         {
 139             n = _nchan;
 140             if (out_data)
 141             {
 142                 if (nz < 4)
 143                 {
 144                     a = ph;
 145                     b = 1 - a;
 146                     d = a * b / 2;
 147                     m0 = -d * b;
 148                     m1 = b + (3 * b - 1) * d;
 149                     m2 = a + (3 * a - 1) * d;
 150                     m3 = -d * a;
 151                     for (c = 0; c < n; c++)
 152                     {
 153                         *out_data++ = m0 * pb [0]
 154                                     + m1 * pb [n]
 155                                     + m2 * pb [2 * n]
 156                                     + m3 * pb [3 * n];
 157                         pb++;
 158                     }
 159                     pb -= n;
 160                 }
 161                 else
 162                 {
 163                     for (c = 0; c < n; c++) *out_data++ = 0;
 164                 }
 165             }
 166             out_count--;
 167
 168             ph += _pstep;
 169             if (ph >= 1.0)
 170             {
 171                 nr = (unsigned int) floor (ph);
 172                 ph -= nr;
 173                 in += nr;
 174                 pb += nr * _nchan;
 175                 if (in >= _inmax)
 176                 {
 177                     memcpy (_buff, pb, (4 - nr) * _nchan * sizeof (float));
 178                     in = 0;
 179                     pb = _buff;
 180                 }
 181             }
 182         }
 183     }
 184
 185     _index = in;
 186     _nread = nr;
 187     _nzero = nz;
 188     _phase = ph;
 189
 190     return 0;
 191 }
 192