resample.c

完整的RTP RTSP代码库

/*
 * 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: There is a newer version of the resample routine then what
 * this file was originally based on.  Those adventurous might be
 * interested in reviewing its improvesments and porting it to this
 * version.
 */

/* 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]
*/

/*
 * October 29, 1999
 * Various changes, bugfixes(?), increased precision, by Stan Brooks.
 *
 * 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.
 *
 */
/*
 * SJB: [11/25/99]
 * TODO: another idea for improvement...
 * note that upsampling usually doesn't require interpolation,
 * therefore is faster and more accurate than downsampling.
 * Downsampling by an integer factor is also simple, since
 * it just involves decimation if the input is already 
 * lowpass-filtered to the output Nyquist freqency.
 * Get the idea? :)
 */

#include "mpeg4ip.h"
#include <math.h>
#include <stdlib.h>
#include <string.h>
//#include "st_i.h"

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

/* this Float MUST match that in filter.c */
#define Float double/*float*/
#define ISCALE 0x10000

/* largest factor for which exact-coefficients upsampling will be used */
#define NQMAX 511

#define BUFFSIZE 8192 /*16384*/  /* Total I/O buffer size */

/* Private data for Lerp via LCM file */
struct resamplestuff {
   double Factor;     /* Factor = Fout/Fin sample rates */
   double rolloff;    /* roll-off frequency */
   double beta;       /* passband/stopband tuning magic */
   int quadr;         /* non-zero to use qprodUD quadratic interpolation */
   long Nmult;
   long Nwing;
   long Nq;
   Float *Imp;        /* impulse [Nwing+1] Filter coefficients */

   double Time;       /* Current time/pos in input sample */
   long dhb;

   long a,b;          /* gcd-reduced input,output rates   */
   long t;            /* Current time/pos for exact-coeff's method */

   long Xh;           /* number of past/future samples needed by filter  */
   long Xoff;         /* Xh plus some room for creep  */
   long Xread;        /* X[Xread] is start-position to enter new samples */
   long Xp;           /* X[Xp] is position to start filter application   */
   long Xsize,Ysize;  /* size (Floats) of X[],Y[]         */
   Float *X, *Y;      /* I/O buffers */
};

static void LpFilter(double c[],
		     long N,
		     double frq,
		     double Beta,
		     long Num);

/* makeFilter is used by filter.c */
int makeFilter(Float Imp[],
	       long Nwing,
	       double Froll,
	       double Beta,
	       long Num,
	       int Normalize);

static long SrcUD(resample_t r, long Nx);
static long SrcEX(resample_t r, long Nx);

static int32_t st_gcd(int32_t a, int32_t b)
{
        if (b == 0)
                return a;
        else
                return st_gcd(b, a % b);
}

/*
 * Process options
 */
#if 0
int st_resample_getopts(eff_t effp, int n, char **argv) 
{
	resample_t r = (resample_t) effp->priv;

	/* These defaults are conservative with respect to aliasing. */
	r->rolloff = 0.80;
	r->beta = 16; /* anything <=2 means Nutall window */
	r->quadr = 0;
	r->Nmult = 45;

	/* This used to fail, but with sox-12.15 it works. AW */
	if ((n >= 1)) {
		if (!strcmp(argv[0], "-qs")) {
			r->quadr = 1;
			n--; argv++;
		}
		else if (!strcmp(argv[0], "-q")) {
			r->rolloff = 0.875;
			r->quadr = 1;
			r->Nmult = 75;
			n--; argv++;
		}
		else if (!strcmp(argv[0], "-ql")) {
			r->rolloff = 0.94;
			r->quadr = 1;
			r->Nmult = 149;
			n--; argv++;
		}
	}

	if ((n >= 1) && (sscanf(argv[0], "%lf", &r->rolloff) != 1))
	{
	  //st_fail("Usage: resample [ rolloff [ beta ] ]");
	  return (ST_EOF);
	}
	else if ((r->rolloff <= 0.01) || (r->rolloff >= 1.0))
	{
	  st_fail("resample: rolloff factor (%f) no good, should be 0.01<x<1.0", r->rolloff);
	  return(ST_EOF);
	}

	if ((n >= 2) && !sscanf(argv[1], "%lf", &r->beta))
	{
	  st_fail("Usage: resample [ rolloff [ beta ] ]");
	  return (ST_EOF);
	}
	else if (r->beta <= 2.0) {
	  r->beta = 0;
	  //st_report("resample opts: Nuttall window, cutoff %f\n", r->rolloff);
	} else {
	  //st_report("resample opts: Kaiser window, cutoff %f, beta %f\n", r->rolloff, r->beta);
	}
	return (ST_SUCCESS);
}

#endif
/*
 * Prepare processing.
 */
resample_t st_resample_start(uint32_t inrate, uint32_t outrate)
{
	resample_t r;
	long Xoff, gcdrate;
	int i;
 	if (inrate == outrate) 
	{
	  //st_fail("Input and Output rates must be different to use resample effect");
	    return(NULL);
	}

	r = (resample_t)malloc(sizeof(*r));
	memset(r, 0, sizeof(*r));

	r->rolloff = 0.80;
	r->beta = 16; /* anything <=2 means Nutall window */
	r->quadr = 0;
	r->Nmult = 45;

		
	r->Factor = (double)outrate / (double)inrate;

	gcdrate = st_gcd((long)inrate, (long)outrate);
	r->a = inrate / gcdrate;
	r->b = outrate / gcdrate;

	if (r->a <= r->b && r->b <= NQMAX) {
		r->quadr = -1; /* exact coeff's   */
		r->Nq = r->b;  /* MAX(r->a,r->b);	*/
	} else {
		r->Nq = Nc; /* for now */
	}

	/* Check for illegal constants */
# if 0
	if (Lp >= 16) st_fail("Error: Lp>=16");
	if (Nb+Nhg+NLpScl >= 32) st_fail("Error: Nb+Nhg+NLpScl>=32");
	if (Nh+Nb > 32) st_fail("Error: Nh+Nb>32");
# endif

	/* Nwing: # of filter coeffs in right wing */
	r->Nwing = r->Nq * (r->Nmult/2+1) + 1;

	r->Imp = (Float *)malloc(sizeof(Float) * (r->Nwing+2)) + 1;
	/* need Imp[-1] and Imp[Nwing] for quadratic interpolation */
	/* returns error # <=0, or adjusted wing-len > 0 */
	i = makeFilter(r->Imp, r->Nwing, r->rolloff, r->beta, r->Nq, 1);
	if (i <= 0)
	{
	  free(r);
	  //st_fail("resample: Unable to make filter\n");
	  return (NULL);
	}

	/*st_report("Nmult: %ld, Nwing: %ld, Nq: %ld\n",r->Nmult,r->Nwing,r->Nq);*/

	if (r->quadr < 0) { /* exact coeff's method */
		r->Xh = r->Nwing/r->b;
		//st_report("resample: rate ratio %ld:%ld, coeff interpolation not needed\n", r->a, r->b);
	} else {
	  r->dhb = Np;  /* Fixed-point Filter sampling-time-increment */
	  if (r->Factor<1.0) r->dhb = r->Factor*Np + 0.5;
	  r->Xh = (r->Nwing<<La)/r->dhb;
	  /* (Xh * dhb)>>La is max index into Imp[] */
	}

	/* reach of LP filter wings + some creeping room */
	Xoff = r->Xh + 10;
	r->Xoff = Xoff;

	/* Current "now"-sample pointer for input to filter */
	r->Xp = Xoff;
	/* Position in input array to read into */
	r->Xread = Xoff;
	/* Current-time pointer for converter */
	r->Time = Xoff;
	if (r->quadr < 0) { /* exact coeff's method */
		r->t = Xoff*r->Nq;
	}
	i = BUFFSIZE - 2*Xoff;
	if (i < r->Factor + 1.0/r->Factor)      /* Check input buffer size */
	{
	  //st_fail("Factor is too small or large for BUFFSIZE");
	  free(r);
		return (NULL);
	}
	
	r->Xsize = 2*Xoff + i/(1.0+r->Factor);
	r->Ysize = BUFFSIZE - r->Xsize;
	/* st_report("Xsize %d, Ysize %d, Xoff %d",r->Xsize,r->Ysize,r->Xoff); */

	r->X = (Float *) malloc(sizeof(Float) * (BUFFSIZE));
	r->Y = r->X + r->Xsize;

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

/*
 * Processed signed long samples from ibuf to obuf.
 * Return number of samples processed.
 */
int st_resample_flow (resample_t r, 
		      const int16_t *ibuf, 
		      int16_t *obuf, 
		      uint32_t *isamp, 
		      uint32_t *osamp,
		      uint8_t chans)
{
	long i, last, Nout, Nx, Nproc;

	/* constrain amount we actually process */
	/*fprintf(stderr,"Xp %d, Xread %d, isamp %d, ",r->Xp, r->Xread,*isamp);*/

	Nproc = r->Xsize - r->Xp;

#if 0
	debug_message("xsize %u ysize %u xp %u osamp %u, rfactor %u",
		      r->Xsize, r->Ysize, r->Xp, *osamp, r->Factor);
#endif
	i = (r->Ysize < *osamp)? r->Ysize : *osamp;
	if (Nproc * r->Factor >= i)
	  Nproc = i / r->Factor;

	Nx = Nproc - r->Xread; /* space for right-wing future-data */
	if (Nx <= 0)
	{
	  error_message("resample: Can not handle this sample rate change. Nx not positive: %d %d %d", Nx, Nproc, r->Xread);
	debug_message("xsize %u ysize %u xp %u osamp %u, rfactor %u",
		      r->Xsize, r->Ysize, r->Xp, *osamp, r->Factor);
		return (-1);
	}
	if (Nx > *isamp)
		Nx = *isamp;
	/*fprintf(stderr,"Nx %d\n",Nx);*/

	if (ibuf == NULL) {
		for(i = r->Xread; i < Nx + r->Xread  ; i++) 
			r->X[i] = 0;
	} else {
	  for(i = r->Xread; i < Nx + r->Xread  ; i++) {
	    r->X[i] = (Float)(*ibuf)/ISCALE;
	    ibuf += chans;
	  }
	}
	last = i;
	Nproc = last - r->Xoff - r->Xp;

	if (Nproc <= 0) {
		/* fill in starting here next time */
		r->Xread = last;
		/* leave *isamp alone, we consumed it */
		*osamp = 0;
		return (0);
	}
	if (r->quadr < 0) { /* exact coeff's method */
		long creep; 
		Nout = SrcEX(r, Nproc);
		/*fprintf(stderr,"Nproc %d --> %d\n",Nproc,Nout);*/
		/* Move converter Nproc samples back in time */
		r->t -= Nproc * r->b;
		/* Advance by number of samples processed */
		r->Xp += Nproc;
		/* Calc time accumulation in Time */
		creep = r->t/r->b - r->Xoff; 
		if (creep)
		{
		  r->t -= creep * r->b;  /* Remove time accumulation   */
		  r->Xp += creep;        /* and add it to read pointer */
		  /*fprintf(stderr,"Nproc %ld, creep %ld\n",Nproc,creep);*/
		}
	} else { /* approx coeff's method */
		long creep; 
		Nout = SrcUD(r, Nproc);
		/*fprintf(stderr,"Nproc %d --> %d\n",Nproc,Nout);*/
		/* Move converter Nproc samples back in time */
		r->Time -= Nproc;
		/* Advance by number of samples processed */
		r->Xp += Nproc;
		/* Calc time accumulation in Time */
		creep = r->Time - r->Xoff; 
		if (creep)
		{
		  r->Time -= creep;   /* Remove time accumulation   */
		  r->Xp += creep;     /* and add it to read pointer */