ref: c6b7f738caa13c26ed423d21f3246a60fd35075a
dir: /src/filter.c/
/*
* 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 "st_i.h"
#ifndef HAVE_MEMMOVE
#define memmove(dest,src,len) bcopy((src),(dest),(len))
#endif
/* this Float MUST match that in resample.h */
#define Float double/*float*/
#define ISCALE 0x10000
#define BUFFSIZE 8192
/* Private data for Lerp via LCM file */
typedef struct filterstuff {
st_rate_t rate;
st_sample_t freq0;/* low corner freq */
st_sample_t freq1;/* high corner freq */
double beta;/* >2 is kaiser window beta, <=2 selects nuttall window */
long Nwin;
Float *Fp;/* [Xh+1] Filter coefficients */
long Xh;/* number of past/future samples needed by filter */
long Xt;/* target to enter new data into X */
Float *X, *Y;/* I/O buffers */
} *filter_t;
/* makeFilter() declared in resample.c */
extern int
makeFilter(Float Fp[], long Nwing, double Froll, double Beta, long Num, int Normalize);
static void FiltWin(filter_t f, long Nx);
/*
* Process options
*/
int st_filter_getopts(eff_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;
}
/* fprintf(stderr,"freq: %d-%d\n", f->freq0, f->freq1);fflush(stderr); */
if (f->freq0 == 0 && f->freq1 == 0)
{
st_fail("Usage: filter low-high [ windowlength [ beta ] ]");
return (ST_EOF);
}
if ((n >= 2) && !sscanf(argv[1], "%ld", &f->Nwin))
{
st_fail("Usage: filter low-high [ windowlength ]");
return (ST_EOF);
}
else if (f->Nwin < 4) {
st_fail("filter: window length (%ld) <4 is too short", f->Nwin);
return (ST_EOF);
}
if ((n >= 3) && !sscanf(argv[2], "%lf", &f->beta))
{
st_fail("Usage: filter low-high [ windowlength [ beta ] ]");
return (ST_EOF);
}
st_report("filter opts: %d-%d, window-len %d, beta %f\n", f->freq0, f->freq1, f->Nwin, f->beta);
return (ST_SUCCESS);
}
/*
* Prepare processing.
*/
int st_filter_start(eff_t effp)
{
filter_t f = (filter_t) effp->priv;
Float *Fp0, *Fp1;
long Xh0, Xh1, Xh;
int i;
f->rate = effp->ininfo.rate;
/* adjust upper frequency to Nyquist if necessary */
if (f->freq1 > f->rate/2 || f->freq1 <= 0)
f->freq1 = f->rate/2;
if ((f->freq0 < 0) || (f->freq0 > f->freq1))
{
st_fail("filter: low(%d),high(%d) parameters must satisfy 0 <= low <= high <= %d",
f->freq0, f->freq1, f->rate/2);
return (ST_EOF);
}
Xh = f->Nwin/2;
Fp0 = (Float *) malloc(sizeof(Float) * (Xh + 2)) + 1;
if (f->freq0 > f->rate/200) {
Xh0 = makeFilter(Fp0, Xh, 2.0*(double)f->freq0/f->rate, f->beta, 1, 0);
if (Xh0 <= 1)
{
st_fail("filter: Unable to make low filter\n");
return (ST_EOF);
}
} else {
Xh0 = 0;
}
Fp1 = (Float *) malloc(sizeof(Float) * (Xh + 2)) + 1;
/* need Fp[-1] and Fp[Xh] for makeFilter */
if (f->freq1 < f->rate/2) {
Xh1 = makeFilter(Fp1, Xh, 2.0*(double)f->freq1/f->rate, f->beta, 1, 0);
if (Xh1 <= 1)
{
st_fail("filter: Unable to make high filter\n");
return (ST_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++) {
Float 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)
st_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 = (Float *) malloc(sizeof(Float) * (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 (ST_SUCCESS);
}
/*
* Processed signed long samples from ibuf to obuf.
* Return number of samples processed.
*/
int st_filter_flow(eff_t effp, st_sample_t *ibuf, st_sample_t *obuf,
st_size_t *isamp, st_size_t *osamp)
{
filter_t f = (filter_t) effp->priv;
long i, Nx, Nproc;
/* constrain amount we actually process */
/* fprintf(stderr,"Xh %d, Xt %d, isamp %d, ",f->Xh, f->Xt, *isamp);fflush(stderr); */
Nx = BUFFSIZE + 2*f->Xh - f->Xt;
if (Nx > *isamp) Nx = *isamp;
if (Nx > *osamp) Nx = *osamp;
*isamp = Nx;
{
Float *xp, *xtop;
xp = f->X + f->Xt;
xtop = xp + Nx;
if (ibuf != NULL) {
while (xp < xtop)
*xp++ = (Float)(*ibuf++) / ISCALE;
} else {
while (xp < xtop)
*xp++ = 0;
}
}
Nproc = f->Xt + Nx - 2*f->Xh;
if (Nproc <= 0) {
f->Xt += Nx;
*osamp = 0;
return (ST_SUCCESS);
}
/* fprintf(stderr,"flow Nproc %d\n",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(Float)*2*f->Xh);
f->Xt = 2*f->Xh;
for (i = 0; i < Nproc; i++)
*obuf++ = f->Y[i] * ISCALE;
*osamp = Nproc;
return (ST_SUCCESS);
}
/*
* Process tail of input samples.
*/
int st_filter_drain(eff_t effp, st_sample_t *obuf, st_size_t *osamp)
{
filter_t f = (filter_t) effp->priv;
long isamp_res, osamp_res;
st_sample_t *Obuf;
/* fprintf(stderr,"Xh %d, Xt %d <--- DRAIN\n",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) {
st_sample_t Isamp, Osamp;
Isamp = isamp_res;
Osamp = osamp_res;
st_filter_flow(effp, NULL, Obuf, (st_size_t *)&Isamp, (st_size_t *)&Osamp);
/* fprintf(stderr,"DRAIN isamp,osamp (%d,%d) -> (%d,%d)\n",
* isamp_res,osamp_res,Isamp,Osamp); */
Obuf += Osamp;
osamp_res -= Osamp;
isamp_res -= Isamp;
};
*osamp -= osamp_res;
/* fprintf(stderr,"DRAIN osamp %d\n", *osamp); */
if (isamp_res)
st_warn("drain overran obuf by %d\n", isamp_res); fflush(stderr);
return (ST_SUCCESS);
}
/*
* Do anything required when you stop reading samples.
* Don't close input file!
*/
int st_filter_stop(eff_t effp)
{
filter_t f = (filter_t) effp->priv;
free(f->Fp - 1);
free(f->X);
return (ST_SUCCESS);
}
static double jprod(const Float *Fp, const Float *Xp, long ct)
{
const Float *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)
{
Float *Y;
Float *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++;
}
}