resample.c

基于VXWORKS H323通信技术源代码

/*
 * July 5, 1991
 * Copyright 1991 Lance Norskog And Sundry Contributors
 * This source code is freely redistributable and may be used for
 * any purpose.  This copyright notice must be maintained. 
 * Lance Norskog And Sundry Contributors are not responsible for 
 * the consequences of using this software.
 */

/*
 * Sound Tools rate change effect file.
 * Spiffy rate changer using Smith & Wesson Bandwidth-Limited Interpolation.
 * The algorithm is described in "Bandlimited Interpolation -
 * Introduction and Algorithm" by Julian O. Smith III.
 * Available on ccrma-ftp.stanford.edu as
 * pub/BandlimitedInterpolation.eps.Z or similar.
 *
 * The latest stand alone version of this algorithm can be found
 * at ftp://ccrma-ftp.stanford.edu/pub/NeXT/
 * under the name of resample-version.number.tar.Z
 *
 * NOTE: This source badly needs to be updated to reflect the latest
 * version of the above software!  Someone please perform this and
 * send patches to [e-mail].
 */
/* Fixed bug: roll off frequency was wrong, too high by 2 when upsampling,
 * too low by 2 when downsampling.
 * Andreas Wilde, 12. Feb. 1999, [e-mail]
*/
#include <math.h>
#include <stdlib.h>
#ifdef HAVE_MALLOC_H
#include <malloc.h>
#endif
#include "st.h"

/* resample includes */
#include "resdefs.h"
#include "resampl.h"

#define IBUFFSIZE 4096                         /* Input buffer size */
#define OBUFFSIZE (IBUFFSIZE*MAXFACTOR+2)      /* Calc'd out buffer size */

/* Private data for Lerp via LCM file */
typedef struct resamplestuff {
   double Factor;               /* Factor = Fout/Fin sample rates */
   double rolloff;              /* roll-off frequency */
   double beta;                 /* passband/stopband tuning magic */
   short InterpFilt;      	/* TRUE means interpolate filter coeffs */
   UHWORD Oskip;		/* number of bogus output samples at start */
   UHWORD LpScl, Nmult, Nwing;
   HWORD *Imp;         		/* impulse [MAXNWING] Filter coefficients */
   HWORD *ImpD;        		/* [MAXNWING] ImpD[n] = Imp[n+1]-Imp[n] */
   /* for resample main loop */
   UWORD Time;                  /* Current time/pos in input sample */
   UHWORD Xp, Xoff, Xread;
   HWORD *X, *Y; 		/* I/O buffers */
} *resample_t;

int makeFilter(P6(HWORD Imp[],
		  HWORD ImpD[],
		  UHWORD *LpScl,
		  UHWORD Nwing,
		  double Froll,
		  double Beta));
HWORD SrcUp(P10(HWORD X[],
		HWORD Y[],
		double Factor,
		UWORD *Time,
		UHWORD Nx,
		UHWORD Nwing,
		UHWORD LpScl,
		HWORD Imp[],
		HWORD ImpD[],
		BOOL Interp));
HWORD SrcUD(P10(HWORD X[],
		HWORD Y[],
		double Factor,
		UWORD *Time,
		UHWORD Nx,
		UHWORD Nwing,
		UHWORD LpScl,
		HWORD Imp[],
		HWORD ImpD[],
		BOOL Interp));
IWORD FilterUp(P7(HWORD Imp[],
		  HWORD ImpD[],
		  UHWORD Nwing,
		  BOOL Interp,
		  HWORD *Xp,
		  HWORD Ph,
		  HWORD Inc));
IWORD FilterUD(P8(HWORD Imp[],
		  HWORD ImpD[],
		  UHWORD Nwing,
		  BOOL Interp,
		  HWORD *Xp,
		  HWORD Ph,
		  HWORD Inc,
		  UHWORD dhb));

/*
 * Process options
 */
void resample_getopts(effp, n, argv) 
eff_t effp;
int n;
char **argv;
{
	resample_t resample = (resample_t) effp->priv;

	/* These defaults are conservative with respect to aliasing. */
	resample->rolloff = 0.8;
	resample->beta = 17.5;

	/* This used to fail, but with sox-12.15 it works. AW */
	if ((n >= 1) && !sscanf(argv[0], "%lf", &resample->rolloff))
		fail("Usage: resample [ rolloff [ beta ] ]");
	else if ((resample->rolloff < 0.01) || (resample->rolloff > 1.0))
	    fail("resample: rolloff factor (%f) no good, should be 0.01<x<1.0", 
			resample->rolloff);
	if ((n >= 2) && !sscanf(argv[1], "%lf", &resample->beta))
		fail("Usage: resample [ rolloff [ beta ] ]");
	else if (resample->beta < 1.0)
	        fail("resample: beta factor (%f) no good, should be >= 1.0", 
			resample->beta);
	report("resample opts: %f, %f\n", 
		resample->rolloff, resample->beta);
}

/*
 * Prepare processing.
 */
void resample_start(effp)
eff_t effp;
{
	resample_t resample = (resample_t) effp->priv;
	int i;
	
/*	debugger(); */

	resample->InterpFilt = 1;	/* interpolate filter: slower */
	resample->Factor = 
		(double)effp->outinfo.rate / (double)effp->ininfo.rate;
	
	/* Check for illegal constants */
	if (Np >= 16)
		fail("Error: Np>=16");
	if (Nb+Nhg+NLpScl >= 32)
		fail("Error: Nb+Nhg+NLpScl>=32");
	if (Nh+Nb > 32)
	      fail("Error: Nh+Nb>32");


	resample->Imp = (HWORD *) malloc(sizeof(HWORD) * MAXNWING);
	resample->ImpD = (HWORD *) malloc(sizeof(HWORD) * MAXNWING);
	resample->X = (HWORD *) malloc(sizeof(HWORD) * IBUFFSIZE);
	resample->Y = (HWORD *) malloc(sizeof(HWORD) * OBUFFSIZE);

	/* upsampling requires smaller Nmults */
	for(resample->Nmult = 37; resample->Nmult > 1; resample->Nmult -= 2) {
		/* # of filter coeffs in right wing */
		resample->Nwing = Npc*(resample->Nmult+1)/2;     
		/* This prevents just missing last coeff */
		/*   for integer conversion factors  */
		resample->Nwing += Npc/2 + 1;      

		/* returns error # or 0 for success */
		if (makeFilter(resample->Imp, resample->ImpD, 
				&resample->LpScl, resample->Nwing, 
				resample->rolloff, resample->beta))
				continue;
			else
				break;
			
	}

	if(resample->Nmult == 1)
		fail("resample: Unable to make filter\n");

	if (resample->Factor < 1)
		resample->LpScl = resample->LpScl*resample->Factor + 0.5;
	/* Calc reach of LP filter wing & give some creeping room */
	resample->Xoff = ((resample->Nmult+1)/2.0) * 
		MAX(1.0,1.0/resample->Factor) + 10;
	if (IBUFFSIZE < 2*resample->Xoff)      /* Check input buffer size */
		fail("IBUFFSIZE (or Factor) is too small");

	/* Current "now"-sample pointer for input */
	resample->Xp = resample->Xoff;             
	/* Position in input array to read into */
	resample->Xread = resample->Xoff;          
	/* Current-time pointer for converter */
	resample->Time = (resample->Xoff<<Np);     

	/* Set sample drop at beginning */
	resample->Oskip = resample->Xread * resample->Factor;

	/* Need Xoff zeros at begining of sample */
	for (i=0; i<resample->Xoff; i++)
		resample->X[i] = 0;
}

/*
 * Processed signed long samples from ibuf to obuf.
 * Return number of samples processed.
 */

void resample_flow(effp, ibuf, obuf, isamp, osamp)
eff_t effp;
LONG *ibuf, *obuf;
LONG *isamp, *osamp;
{
	resample_t resample = (resample_t) effp->priv;
	LONG i, last, creep, Nout, Nx;
	UHWORD Nproc;

	/* constrain amount we actually process */
	Nproc = IBUFFSIZE - resample->Xp;
	if (Nproc * resample->Factor >= OBUFFSIZE)
		Nproc = OBUFFSIZE / resample->Factor;
	if (Nproc * resample->Factor >= *osamp)
		Nproc = *osamp / resample->Factor;
	
	Nx = Nproc - resample->Xread;
	if (Nx <= 0)
		fail("Nx negative: %d", Nx);
	if (Nx > *isamp) {
		Nx = *isamp;
	}
	for(i = resample->Xread; i < Nx + resample->Xread  ; i++) 
		resample->X[i] = RIGHT(*ibuf++ + 0x8000, 16);
	last = i;
	Nproc = last - (resample->Xoff * 2);
	for(; i < last + resample->Xoff  ; i++) 
		resample->X[i] = 0;

	/* If we're draining out a buffer tail, 
	 * just do it next time or in drain.
	 */
	if ((Nx == *isamp) && (Nx <= resample->Xoff)) {
		/* fill in starting here next time */
		resample->Xread = last;
		/* leave *isamp alone, we consumed it */
		*osamp = 0;
		return;
	}


        /* SrcUp() is faster if we can use it */
	if (resample->Factor > 1)       /* Resample stuff in input buffer */
	    Nout = SrcUp(resample->X, resample->Y,
		resample->Factor, &resample->Time, Nproc,
		resample->Nwing, resample->LpScl,
		resample->Imp, resample->ImpD, 
		resample->InterpFilt);      
	else
            Nout = SrcUD(resample->X, resample->Y,
		resample->Factor, &resample->Time, Nproc,
		resample->Nwing, resample->LpScl,
		resample->Imp, resample->ImpD,
		resample->InterpFilt);

	/* Move converter Nproc samples back in time */
	resample->Time -= (Nproc<<Np); 
        /* Advance by number of samples processed */
	resample->Xp += Nproc;
	/* Calc time accumulation in Time */
	creep = (resample->Time>>Np) - resample->Xoff; 
	if (creep)
	{
		resample->Time -= (creep<<Np);   /* Remove time accumulation */
		resample->Xp += creep;     /* and add it to read pointer */
	}

	/* Copy back portion of input signal that must be re-used */
	for (i=0; i<last - resample->Xp + resample->Xoff; i++) 
	    resample->X[i] = resample->X[i + resample->Xp - resample->Xoff];

	/* Pos in input buff to read new data into */
	resample->Xread = i;                 
	resample->Xp = resample->Xoff;

	/* copy to output buffer, zero-filling beginning */
	/* zero-fill to preserve length and loop points */
	for(i = 0; i < resample->Oskip; i++) {
		*obuf++ = 0;
	}
	for(i = resample->Oskip; i < Nout + resample->Oskip; i++) {
		*obuf++ = LEFT(resample->Y[i], 16);
	}

	*isamp = Nx;
	*osamp = Nout;

	resample->Oskip = 0;
}

/*
 * Process tail of input samples.
 */
void resample_drain(effp, obuf, osamp)
eff_t effp;
ULONG *obuf;
ULONG *osamp;
{
	resample_t resample = (resample_t) effp->priv;
	LONG i, Nout;
	UHWORD Nx;
	
	Nx = resample->Xread - resample->Xoff;
	if (Nx <= resample->Xoff * 2) {
		/* zero-fill end */
		for(i = 0; i < resample->Xoff; i++)
			*obuf++ = 0;
		*osamp = resample->Xoff;
		return;
	}

	if (Nx * resample->Factor >= *osamp)
		fail("resample_drain: Overran output buffer!\n");

	/* fill out end with zeros */
	for(i = 0; i < resample->Xoff; i++)
		resample->X[i + resample->Xread] = 0;
        /* SrcUp() is faster if we can use it */
	if (resample->Factor >= 1)       /* Resample stuff in input buffer */
	    Nout = SrcUp(resample->X, resample->Y,
		resample->Factor, &resample->Time, Nx,
		resample->Nwing, resample->LpScl,
		resample->Imp, resample->ImpD, 
		resample->InterpFilt);      
	else
            Nout = SrcUD(resample->X, resample->Y,
		resample->Factor, &resample->Time, Nx,
		resample->Nwing, resample->LpScl,
		resample->Imp, resample->ImpD,
		resample->InterpFilt);
	
	for(i = resample->Oskip; i < Nout; i++) {