shithub: sox

ref: c0ee26e6f06281f3a4834318babf0aa9ee17ece7
dir: /src/filter.c/

View raw version
/*
 * Windowed sinc lowpass/bandpass/highpass filter.
 */

/*
 * November 18, 1999
 * Copyright 1994 Julius O. Smith
 * Copyright 1991 (?) Lance Norskog (?)
 * Copyright 1999 Stan Brooks <stabro@megsinet.net>
 *
 * -------------------------------------------------------------------
 * This source code 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.
 * -------------------------------------------------------------------
 *
 * REMARKS: (Stan Brooks speaking)
 * This code is heavily based on the resample.c code which was
 * apparently itself a rewrite (by Lance Norskog?) of code originally
 * by Julius O. Smith, and distributed under the GPL license...
 *
 */

#include <math.h>
#include <string.h>
#include <stdlib.h>

#include "sox_i.h"

#define ISCALE 0x10000
#define BUFFSIZE 8192

/* Private data for Lerp via LCM file */
typedef struct filterstuff {
        sox_rate_t rate;
        sox_ssample_t freq0;/* low  corner freq */
        sox_ssample_t freq1;/* high corner freq */
        double beta;/* >2 is kaiser window beta, <=2 selects nuttall window */
        long Nwin;
        double *Fp;/* [Xh+1] Filter coefficients */
        long Xh;/* number of past/future samples needed by filter  */
        long Xt;/* target to enter new data into X */
        double *X, *Y;/* I/O buffers */
} *filter_t;

/* makeFilter() declared in resample.c */
extern int 
makeFilter(double Fp[], long Nwing, double Froll, double Beta, long Num, int Normalize);

static void FiltWin(filter_t f, long Nx);

/*
 * Process options
 */
static int sox_filter_getopts(sox_effect_t * effp, int n, char **argv)
{
        filter_t f = (filter_t) effp->priv;

        f->beta = 16;  /* Kaiser window, beta 16 */
        f->Nwin = 128;
        
        f->freq0 = f->freq1 = 0;
        if (n >= 1) {
                char *p;
                p = argv[0];
                if (*p != '-') {
                        f->freq1 = strtol(p, &p, 10);
                }
                if (*p == '-') {
                        f->freq0 = f->freq1;
                        f->freq1 = strtol(p+1, &p, 10);
                }
                if (*p) f->freq1 = f->freq0 = 0;
        }
        sox_debug("freq: %d-%d", f->freq0, f->freq1);
        if (f->freq0 == 0 && f->freq1 == 0)
        {
                sox_fail(effp->handler.usage);
                return (SOX_EOF);
        }

        if ((n >= 2) && !sscanf(argv[1], "%ld", &f->Nwin))
        {
                sox_fail(effp->handler.usage);
                return (SOX_EOF);
        }
        else if (f->Nwin < 4) {
                sox_fail("filter: window length (%ld) <4 is too short", f->Nwin);
                return (SOX_EOF);
        }

        if ((n >= 3) && !sscanf(argv[2], "%lf", &f->beta))
        {
                sox_fail(effp->handler.usage);
                return (SOX_EOF);
        }

        sox_debug("filter opts: %d-%d, window-len %d, beta %f", f->freq0, f->freq1, f->Nwin, f->beta);
        return (SOX_SUCCESS);
}

/*
 * Prepare processing.
 */
static int sox_filter_start(sox_effect_t * effp)
{
        filter_t f = (filter_t) effp->priv;
        double *Fp0, *Fp1;
        long Xh0, Xh1, Xh;
        int i;

        f->rate = effp->ininfo.rate;

        /* adjust upper frequency to Nyquist if necessary */
        if (f->freq1 > (sox_ssample_t)f->rate/2 || f->freq1 <= 0)
                f->freq1 = f->rate/2;

        if ((f->freq0 < 0) || (f->freq0 > f->freq1))
        {
                sox_fail("filter: low(%d),high(%d) parameters must satisfy 0 <= low <= high <= %d",
                                        f->freq0, f->freq1, f->rate/2);
                return (SOX_EOF);
        }
        
        Xh = f->Nwin/2;
        Fp0 = (double *) xmalloc(sizeof(double) * (Xh + 2)) + 1;
        if (f->freq0 > (sox_ssample_t)f->rate/200) {
                Xh0 = makeFilter(Fp0, Xh, 2.0*(double)f->freq0/f->rate, f->beta, 1, 0);
                if (Xh0 <= 1)
                {
                        sox_fail("filter: Unable to make low filter");
                        return (SOX_EOF);
                }
        } else {
                Xh0 = 0;
        }
        Fp1 = (double *) xmalloc(sizeof(double) * (Xh + 2)) + 1;
        /* need Fp[-1] and Fp[Xh] for makeFilter */
        if (f->freq1 < (sox_ssample_t)f->rate/2) {
                Xh1 = makeFilter(Fp1, Xh, 2.0*(double)f->freq1/f->rate, f->beta, 1, 0);
                if (Xh1 <= 1)
                {
                        sox_fail("filter: Unable to make high filter");
                        return (SOX_EOF);
                }
        } else {
                Fp1[0] = 1.0;
                Xh1 = 1;
        }
        /* now subtract Fp0[] from Fp1[] */
        Xh = (Xh0>Xh1)?  Xh0:Xh1; /* >=1, by above */
        for (i=0; i<Xh; i++) {
                double c0,c1;
                c0 = (i<Xh0)? Fp0[i]:0;
                c1 = (i<Xh1)? Fp1[i]:0;
                Fp1[i] = c1-c0;
        }

        free(Fp0 - 1); /* all done with Fp0 */
        f->Fp = Fp1;

        Xh -= 1;       /* Xh = 0 can only happen if filter was identity 0-Nyquist */
        if (Xh<=0)
                sox_warn("filter: adjusted freq %d-%d is identity", f->freq0, f->freq1);

        f->Nwin = 2*Xh + 1;  /* not really used afterwards */
        f->Xh = Xh;
        f->Xt = Xh;

        f->X = (double *) xmalloc(sizeof(double) * (2*BUFFSIZE + 2*Xh));
        f->Y = f->X + BUFFSIZE + 2*Xh;

        /* Need Xh zeros at beginning of X */
        for (i = 0; i < Xh; i++)
                f->X[i] = 0;
        return (SOX_SUCCESS);
}

/*
 * Processed signed long samples from ibuf to obuf.
 * Return number of samples processed.
 */
static int sox_filter_flow(sox_effect_t * effp, const sox_ssample_t *ibuf, sox_ssample_t *obuf, 
                   sox_size_t *isamp, sox_size_t *osamp)
{
        filter_t f = (filter_t) effp->priv;
        sox_size_t Nx;
        long i, Nproc;

        /* constrain amount we actually process */
        /* sox_debug("Xh %d, Xt %d, isamp %d, ",f->Xh, f->Xt, *isamp); */
        Nx = BUFFSIZE + 2*f->Xh - f->Xt;
        if (Nx > *isamp) Nx = *isamp;
        if (Nx > *osamp) Nx = *osamp;
        *isamp = Nx;

        {
                double *xp, *xtop;
                xp = f->X + f->Xt;
                xtop = xp + Nx;
                if (ibuf != NULL) {
                        while (xp < xtop)
                                *xp++ = (double)(*ibuf++) / ISCALE;
                } else {
                        while (xp < xtop)
                                *xp++ = 0;
                }
        }

        Nproc = f->Xt + Nx - 2*f->Xh;

        if (Nproc <= 0) {
                f->Xt += Nx;
                *osamp = 0;
                return (SOX_SUCCESS);
        }
        sox_debug("flow Nproc %d",Nproc);
        FiltWin(f, Nproc);

        /* Copy back portion of input signal that must be re-used */
        Nx += f->Xt;
        if (f->Xh)
                memmove(f->X, f->X + Nx - 2*f->Xh, sizeof(double)*2*f->Xh); 
        f->Xt = 2*f->Xh;

        for (i = 0; i < Nproc; i++)
                *obuf++ = f->Y[i] * ISCALE;

        *osamp = Nproc;
        return (SOX_SUCCESS);
}

/*
 * Process tail of input samples.
 */
static int sox_filter_drain(sox_effect_t * effp, sox_ssample_t *obuf, sox_size_t *osamp)
{
        filter_t f = (filter_t) effp->priv;
        long isamp_res, osamp_res;
        sox_ssample_t *Obuf;

        sox_debug("Xh %d, Xt %d  <--- DRAIN",f->Xh, f->Xt);

        /* stuff end with Xh zeros */
        isamp_res = f->Xh;
        osamp_res = *osamp;
        Obuf = obuf;
        while (isamp_res>0 && osamp_res>0) {
                sox_ssample_t Isamp, Osamp;
                Isamp = isamp_res;
                Osamp = osamp_res;
                sox_filter_flow(effp, NULL, Obuf, (sox_size_t *)&Isamp, (sox_size_t *)&Osamp);
          /* sox_debug("DRAIN isamp,osamp  (%d,%d) -> (%d,%d)",
                 * isamp_res,osamp_res,Isamp,Osamp); */
                Obuf += Osamp;
                osamp_res -= Osamp;
                isamp_res -= Isamp;
        };
        *osamp -= osamp_res;
        /* sox_debug("DRAIN osamp %d", *osamp); */
        if (isamp_res)
                sox_warn("drain overran obuf by %d", isamp_res);
        /* FIXME: This is very picky. osamp better be big enough to grab
         * all remaining samples or they will be discarded.
         */
        return (SOX_EOF);
}

/*
 * Do anything required when you stop reading samples.  
 * Don't close input file! 
 */
static int sox_filter_stop(sox_effect_t * effp)
{
        filter_t f = (filter_t) effp->priv;

        free(f->Fp - 1);
        free(f->X);
        return (SOX_SUCCESS);
}

static double jprod(const double *Fp, const double *Xp, long ct)
{
        const double *fp, *xp, *xq;
        double v = 0;
        
        fp = Fp + ct;   /* so sum starts with smaller coef's */
        xp = Xp - ct;
        xq = Xp + ct;
        while (fp > Fp) {   /* ct = 0 can happen */
                v += *fp * (*xp + *xq);
                xp++; xq--; fp--;
        }
        v += *fp * *xp;
        return v;
}

static void FiltWin(filter_t f, long Nx)
{
        double *Y;
        double *X, *Xend;

        Y = f->Y;
        X = f->X + f->Xh;                       /* Ptr to current input sample */
        Xend = X + Nx;
        while (X < Xend) {
                *Y++ = jprod(f->Fp, X, f->Xh);
                X++;
        }
}

static sox_effect_handler_t sox_filter_effect = {
  "filter",
  "Usage: filter low-high [ windowlength [ beta ] ]",
  0,
  sox_filter_getopts,
  sox_filter_start,
  sox_filter_flow,
  sox_filter_drain,
  sox_filter_stop,
  NULL
};

const sox_effect_handler_t *sox_filter_effect_fn(void)
{
    return &sox_filter_effect;
}