Carla/source/modules/zita-resampler/resampler.cc

// ----------------------------------------------------------------------------
//
//  Copyright (C) 2006-2012 Fons Adriaensen <fons@linuxaudio.org>
//    
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 3 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
// ----------------------------------------------------------------------------


#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "resampler.h"


static unsigned int gcd (unsigned int a, unsigned int b)
{
    if (a == 0) return b;
    if (b == 0) return a;
    while (1)
    {
	if (a > b)
	{
	    a = a % b;
	    if (a == 0) return b;
	    if (a == 1) return 1;
	}
	else
	{
	    b = b % a;
	    if (b == 0) return a;
	    if (b == 1) return 1;
	}
    }    
    return 1; 
}


Resampler::Resampler (void) noexcept :
    _table (0),
    _nchan (0),
    _buff  (0)
{
    reset ();
}


Resampler::~Resampler (void)
{
    clear ();
}


int Resampler::setup (unsigned int fs_inp,
                      unsigned int fs_out,
                      unsigned int nchan,
                      unsigned int hlen)
{
    if ((hlen < 8) || (hlen > 96)) return 1;
    return setup (fs_inp, fs_out, nchan, hlen, 1.0 - 2.6 / hlen);
}


int Resampler::setup (unsigned int fs_inp,
                      unsigned int fs_out,
                      unsigned int nchan,
                      unsigned int hlen,
                      double       frel)
{
    unsigned int       g, h, k, n, s;
    double             r;
    float              *B = 0;
    Resampler_table    *T = 0;

    k = s = 0;
    if (fs_inp && fs_out && nchan)
    {
        r = (double) fs_out / (double) fs_inp;
        g = gcd (fs_out, fs_inp);
        n = fs_out / g;
        s = fs_inp / g;
        if ((16 * r >= 1) && (n <= 1000))
        {
            h = hlen;
            k = 250;
            if (r < 1)
            {
                frel *= r;
                h = (unsigned int)(ceil (h / r));
                k = (unsigned int)(ceil (k / r));
            }
            T = Resampler_table::create (frel, h, n);
            B = new float [nchan * (2 * h - 1 + k)];
        }
    }
    clear ();
    if (T)
    {
        _table = T;
        _buff  = B;
        _nchan = nchan;
        _inmax = k;
        _pstep = s;
        return reset ();
    }
    return 1;
}


void Resampler::clear (void)
{
    Resampler_table::destroy (_table);
    delete[] _buff;
    _buff  = 0;
    _table = 0;
    _nchan = 0;
    _inmax = 0;
    _pstep = 0;
    reset ();
}


double Resampler::inpdist (void) const noexcept
{
    if (!_table) return 0;
    return (int)(_table->_hl + 1 - _nread) - (double)_phase / _table->_np;
}


unsigned int Resampler::inpsize (void) const noexcept
{
    if (!_table) return 0;
    return 2 * _table->_hl;
}


bool Resampler::reset (void) noexcept
{
    if (!_table) return false;

    inp_count = 0;
    out_count = 0;
    inp_data = nullptr;
    out_data = nullptr;
    _index = 0;
    _nread = 0;
    _nzero = 0;
    _phase = 0;
    _nread = 2 * _table->_hl;
    return true;
}


bool Resampler::process (void)
{
    unsigned int   hl, ph, np, dp, in, nr, nz, i, n, c;
    float          *p1, *p2;

    if (!_table) return false;

    hl = _table->_hl;
    np = _table->_np;
    dp = _pstep;
    in = _index;
    nr = _nread;
    ph = _phase;
    nz = _nzero;
    n = (2 * hl - nr) * _nchan;
    p1 = _buff + in * _nchan;
    p2 = p1 + n;

    while (out_count)
    {
        if (nr)
        {
            if (inp_count == 0) break;
            if (inp_data)
            {
                for (c = 0; c < _nchan; c++) p2 [c] = inp_data [c];
                inp_data += _nchan;
              nz = 0;
            }
            else
            {
                for (c = 0; c < _nchan; c++) p2 [c] = 0;
                if (nz < 2 * hl) nz++;
            }
            nr--;
            p2 += _nchan;
            inp_count--;
        }
        else
        {
            if (out_data)
            {
                if (nz < 2 * hl)
                {
                    float *c1 = _table->_ctab + hl * ph;
                    float *c2 = _table->_ctab + hl * (np - ph);
                    for (c = 0; c < _nchan; c++)
                    {
                        float *q1 = p1 + c;
                        float *q2 = p2 + c;
                        float s = 1e-20f;
                        for (i = 0; i < hl; i++)
                        {
                            q2 -= _nchan;
                            s += *q1 * c1 [i] + *q2 * c2 [i];
                            q1 += _nchan;
                        }
                        *out_data++ = s - 1e-20f;
                    }
                }
                else
                {
                    for (c = 0; c < _nchan; c++) *out_data++ = 0;
                }
            }
            out_count--;

            ph += dp;
            if (ph >= np)
            {
                nr = ph / np;
                ph -= nr * np;
                in += nr;
                p1 += nr * _nchan;;
                if (in >= _inmax)
                {
                    n = (2 * hl - nr) * _nchan;
                    memcpy (_buff, p1, n * sizeof (float));
                    in = 0;
                    p1 = _buff;
                    p2 = p1 + n;
                }
            }
        }
    }
    _index = in;
    _nread = nr;
    _phase = ph;
    _nzero = nz;

    return true;
}