diff options
Diffstat (limited to 'sound/resample.cpp')
-rw-r--r-- | sound/resample.cpp | 1098 |
1 files changed, 625 insertions, 473 deletions
diff --git a/sound/resample.cpp b/sound/resample.cpp index 99deeaf3aa..5a772389fb 100644 --- a/sound/resample.cpp +++ b/sound/resample.cpp @@ -1,156 +1,578 @@ -/* -* 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. +#include "stdafx.h" +#include <math.h> +#include "sound/resample.h" +#include "sound/audiostream.h" + + +#pragma mark - + + + +/** + * Calculates the filter coeffs for a Kaiser-windowed low-pass filter with a + * given roll-off frequency. These coeffs are stored into a array of doubles. + * + * reference: "Digital Filters, 2nd edition" + * R.W. Hamming, pp. 178-179 + * + * LpFilter() computes the coeffs of a Kaiser-windowed low pass filter with + * the following characteristics: * - * 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 + * c[] = array in which to store computed coeffs + * frq = roll-off frequency of filter + * N = Half the window length in number of coeffs + * Beta = parameter of Kaiser window + * Num = number of coeffs before 1/frq * - * 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. + * Beta trades the rejection of the lowpass filter against the transition + * width from passband to stopband. Larger Beta means a slower + * transition and greater stopband rejection. See Rabiner and Gold + * (Theory and Application of DSP) under Kaiser windows for more about + * Beta. The following table from Rabiner and Gold gives some feel + * for the effect of Beta: + * + * All ripples in dB, width of transition band = D*N where N = window length + * + * BETA D PB RIP SB RIP + * 2.120 1.50 +-0.27 -30 + * 3.384 2.23 0.0864 -40 + * 4.538 2.93 0.0274 -50 + * 5.658 3.62 0.00868 -60 + * 6.764 4.32 0.00275 -70 + * 7.865 5.0 0.000868 -80 + * 8.960 5.7 0.000275 -90 + * 10.056 6.4 0.000087 -100 */ +static void LpFilter(double c[], int N, double frq, double Beta, int Num); + +/** + * Calls LpFilter() to create a filter, then scales the double coeffs into an + * array of half words. + * ERROR return codes: + * 0 - no error + * 1 - Nwing too large (Nwing is > MAXNWING) + * 2 - Froll is not in interval [0:1) + * 3 - Beta is < 1.0 + * 4 - LpScl will not fit in 16-bits + */ +static int makeFilter(HWORD Imp[], HWORD ImpD[], UHWORD *LpScl, UHWORD Nwing, + double Froll, double Beta); + +static WORD FilterUp(HWORD Imp[], HWORD ImpD[], UHWORD Nwing, bool Interp, + HWORD *Xp, HWORD Inc, HWORD Ph); + +static WORD FilterUD(HWORD Imp[], HWORD ImpD[], UHWORD Nwing, bool Interp, + HWORD *Xp, HWORD Ph, HWORD Inc, UHWORD dhb); + + + +#pragma mark - -/* Fixed bug: roll off frequency was wrong, too high by 2 when upsampling, - * too low by 2 when downsampling. - * Andreas Wilde, 12. Feb. 1999, andreas@eakaw2.et.tu-dresden.de -*/ /* - * 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. + * The configuration constants below govern + * the number of bits in the input sample and filter coefficients, the + * number of bits to the right of the binary-point for fixed-point math, etc. * - */ -/* - * 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 "stdafx.h" -#include <math.h> -#include "sound/resample.h" -#include "sound/audiostream.h" - - /* Conversion constants */ -#define Lc 7 -#define Nc (1<<Lc) -#define La 16 -#define Na (1<<La) -#define Lp (Lc+La) -#define Np (1<<Lp) -#define Amask (Na-1) -#define Pmask (Np-1) - -#define MAXNWING (80<<Lc) +#define Nhc 8 +#define Na 7 +#define Np (Nhc+Na) +#define Npc (1<<Nhc) +#define Amask ((1<<Na)-1) +#define Pmask ((1<<Np)-1) +#define Nh 16 +#define Nb 16 +#define Nhxn 14 +#define Nhg (Nh-Nhxn) +#define NLpScl 13 + /* Description of constants: * - * Nc - is the number of look-up values available for the lowpass filter + * Npc - is the number of look-up values available for the lowpass filter * between the beginning of its impulse response and the "cutoff time" * of the filter. The cutoff time is defined as the reciprocal of the * lowpass-filter cut off frequence in Hz. For example, if the - * lowpass filter were a sinc function, Nc would be the index of the + * lowpass filter were a sinc function, Npc would be the index of the * impulse-response lookup-table corresponding to the first zero- * crossing of the sinc function. (The inverse first zero-crossing * time of a sinc function equals its nominal cutoff frequency in Hz.) - * Nc must be a power of 2 due to the details of the current - * implementation. The default value of 128 is sufficiently high that + * Npc must be a power of 2 due to the details of the current + * implementation. The default value of 512 is sufficiently high that * using linear interpolation to fill in between the table entries - * gives approximately 16-bit precision, and quadratic interpolation - * gives about 23-bit (float) precision in filter coefficients. + * gives approximately 16-bit accuracy in filter coefficients. * - * Lc - is log base 2 of Nc. + * Nhc - is log base 2 of Npc. * - * La - is the number of bits devoted to linear interpolation of the + * Na - is the number of bits devoted to linear interpolation of the * filter coefficients. * - * Lp - is La + Lc, the number of bits to the right of the binary point + * Np - is Na + Nhc, the number of bits to the right of the binary point * in the integer "time" variable. To the left of the point, it indexes * the input array (X), and to the right, it is interpreted as a number - * between 0 and 1 sample of the input X. The default value of 23 is - * about right. There is a constraint that the filter window must be - * "addressable" in a int32_t, more precisely, if Nmult is the number - * of sinc zero-crossings in the right wing of the filter window, then - * (Nwing<<Lp) must be expressible in 31 bits. + * between 0 and 1 sample of the input X. Np must be less than 16 in + * this implementation. + * + * Nh - is the number of bits in the filter coefficients. The sum of Nh and + * the number of bits in the input data (typically 16) cannot exceed 32. + * Thus Nh should be 16. The largest filter coefficient should nearly + * fill 16 bits (32767). + * + * Nb - is the number of bits in the input data. The sum of Nb and Nh cannot + * exceed 32. + * + * Nhxn - is the number of bits to right shift after multiplying each input + * sample times a filter coefficient. It can be as great as Nh and as + * small as 0. Nhxn = Nh-2 gives 2 guard bits in the multiply-add + * accumulation. If Nhxn=0, the accumulation will soon overflow 32 bits. * + * Nhg - is the number of guard bits in mpy-add accumulation (equal to Nh-Nhxn) + * + * NLpScl - is the number of bits allocated to the unity-gain normalization + * factor. The output of the lowpass filter is multiplied by LpScl and + * then right-shifted NLpScl bits. To avoid overflow, we must have + * Nb+Nhg+NLpScl < 32. */ -/* this Float MUST match that in filter.c */ -#define Float double/*float*/ +#pragma mark - -/* largest factor for which exact-coefficients upsampling will be used */ -#define NQMAX 511 -#define BUFFSIZE 8192 /*16384*/ /* Total I/O buffer size */ +#define IBUFFSIZE 4096 /* Input buffer size */ + +static inline HWORD WordToHword(WORD v, int scl) +{ + HWORD out; + + v = (v + (1 << (NLpScl-1))) >> NLpScl; // Round & scale + + if (v>MAX_HWORD) { + v = MAX_HWORD; + } else if (v < MIN_HWORD) { + v = MIN_HWORD; + } + out = (HWORD) v; + return out; +} + +/* Sampling rate up-conversion only subroutine; + * Slightly faster than down-conversion; + */ +static int SrcUp(HWORD X[], HWORD Y[], double factor, UWORD *Time, + UHWORD Nx, UHWORD Nwing, UHWORD LpScl, + HWORD Imp[], HWORD ImpD[], bool Interp) +{ + HWORD *Xp, *Ystart; + WORD v; + + double dt; /* Step through input signal */ + UWORD dtb; /* Fixed-point version of Dt */ + UWORD endTime; /* When Time reaches EndTime, return to user */ + + dt = 1.0/factor; /* Output sampling period */ + dtb = (UWORD)(dt*(1<<Np) + 0.5); /* Fixed-point representation */ + + Ystart = Y; + endTime = *Time + (1<<Np)*(WORD)Nx; + while (*Time < endTime) + { + Xp = &X[*Time>>Np]; /* Ptr to current input sample */ + /* Perform left-wing inner product */ + v = FilterUp(Imp, ImpD, Nwing, Interp, Xp, (HWORD)(*Time&Pmask),-1); + /* Perform right-wing inner product */ + v += FilterUp(Imp, ImpD, Nwing, Interp, Xp+1, + /* previous (triggers warning): (HWORD)((-*Time)&Pmask),1); */ + (HWORD)((((*Time)^Pmask)+1)&Pmask),1); + v >>= Nhg; /* Make guard bits */ + v *= LpScl; /* Normalize for unity filter gain */ + *Y++ = WordToHword(v,NLpScl); /* strip guard bits, deposit output */ + *Time += dtb; /* Move to next sample by time increment */ + } + return (Y - Ystart); /* Return the number of output samples */ +} + + +/* Sampling rate conversion subroutine */ + +static int SrcUD(HWORD X[], HWORD Y[], double factor, UWORD *Time, + UHWORD Nx, UHWORD Nwing, UHWORD LpScl, + HWORD Imp[], HWORD ImpD[], bool Interp) +{ + HWORD *Xp, *Ystart; + WORD v; + + double dh; /* Step through filter impulse response */ + double dt; /* Step through input signal */ + UWORD endTime; /* When Time reaches EndTime, return to user */ + UWORD dhb, dtb; /* Fixed-point versions of Dh,Dt */ + + dt = 1.0/factor; /* Output sampling period */ + dtb = (UWORD)(dt*(1<<Np) + 0.5); /* Fixed-point representation */ + + dh = MIN((double)Npc, factor*Npc); /* Filter sampling period */ + dhb = (UWORD)(dh*(1<<Na) + 0.5); /* Fixed-point representation */ + + Ystart = Y; + endTime = *Time + (1<<Np)*(WORD)Nx; + while (*Time < endTime) + { + Xp = &X[*Time>>Np]; /* Ptr to current input sample */ + v = FilterUD(Imp, ImpD, Nwing, Interp, Xp, (HWORD)(*Time&Pmask), + -1, dhb); /* Perform left-wing inner product */ + v += FilterUD(Imp, ImpD, Nwing, Interp, Xp+1, + /* previous (triggers warning): (HWORD)((-*Time)&Pmask), */ + (HWORD)((((*Time)^Pmask)+1)&Pmask), + 1, dhb); /* Perform right-wing inner product */ + v >>= Nhg; /* Make guard bits */ + v *= LpScl; /* Normalize for unity filter gain */ + *Y++ = WordToHword(v,NLpScl); /* strip guard bits, deposit output */ + *Time += dtb; /* Move to next sample by time increment */ + } + return (Y - Ystart); /* Return the number of output samples */ +} + + +#pragma mark - + + +#define IzeroEPSILON 1E-21 /* Max error acceptable in Izero */ + +static double Izero(double x) +{ + double sum, u, halfx, temp; + int n; + + sum = u = n = 1; + halfx = x/2.0; + do { + temp = halfx/(double)n; + n += 1; + temp *= temp; + u *= temp; + sum += u; + } while (u >= IzeroEPSILON*sum); + return(sum); +} + + +void LpFilter(double c[], int N, double frq, double Beta, int Num) +{ + double IBeta, temp, inm1; + int i; + + /* Calculate ideal lowpass filter impulse response coefficients: */ + c[0] = 2.0*frq; + for (i=1; i<N; i++) { + temp = M_PI*(double)i/(double)Num; + c[i] = sin(2.0*temp*frq)/temp; /* Analog sinc function, cutoff = frq */ + } + + /* + * Calculate and Apply Kaiser window to ideal lowpass filter. + * Note: last window value is IBeta which is NOT zero. + * You're supposed to really truncate the window here, not ramp + * it to zero. This helps reduce the first sidelobe. + */ + IBeta = 1.0/Izero(Beta); + inm1 = 1.0/((double)(N-1)); + for (i=1; i<N; i++) { + temp = (double)i * inm1; + c[i] *= Izero(Beta*sqrt(1.0-temp*temp)) * IBeta; + } +} + +static double ImpR[MAXNWING]; + +int makeFilter(HWORD Imp[], HWORD ImpD[], UHWORD *LpScl, UHWORD Nwing, + double Froll, double Beta) +{ + double DCgain, Scl, Maxh; + HWORD Dh; + int i, temp; + + if (Nwing > MAXNWING) /* Check for valid parameters */ + return(1); + if ((Froll<=0) || (Froll>1)) + return(2); + if (Beta < 1) + return(3); + + /* + * Design Kaiser-windowed sinc-function low-pass filter + */ + LpFilter(ImpR, (int)Nwing, 0.5*Froll, Beta, Npc); + + /* Compute the DC gain of the lowpass filter, and its maximum coefficient + * magnitude. Scale the coefficients so that the maximum coeffiecient just + * fits in Nh-bit fixed-point, and compute LpScl as the NLpScl-bit (signed) + * scale factor which when multiplied by the output of the lowpass filter + * gives unity gain. */ + DCgain = 0; + Dh = Npc; /* Filter sampling period for factors>=1 */ + for (i=Dh; i<Nwing; i+=Dh) + DCgain += ImpR[i]; + DCgain = 2*DCgain + ImpR[0]; /* DC gain of real coefficients */ + + for (Maxh=i=0; i<Nwing; i++) + Maxh = MAX(Maxh, fabs(ImpR[i])); + + Scl = ((1<<(Nh-1))-1)/Maxh; /* Map largest coeff to 16-bit maximum */ + temp = (int)fabs((1<<(NLpScl+Nh))/(DCgain*Scl)); + if (temp >= 1<<16) + return(4); /* Filter scale factor overflows UHWORD */ + *LpScl = temp; + + /* Scale filter coefficients for Nh bits and convert to integer */ + if (ImpR[0] < 0) /* Need pos 1st value for LpScl storage */ + Scl = -Scl; + for (i=0; i<Nwing; i++) /* Scale them */ + ImpR[i] *= Scl; + for (i=0; i<Nwing; i++) /* Round them */ + Imp[i] = (HWORD)(ImpR[i] + 0.5); + + /* ImpD makes linear interpolation of the filter coefficients faster */ + for (i=0; i<Nwing-1; i++) + ImpD[i] = Imp[i+1] - Imp[i]; + ImpD[Nwing-1] = - Imp[Nwing-1]; /* Last coeff. not interpolated */ + + return(0); +} + + +#pragma mark - + + +WORD FilterUp(HWORD Imp[], HWORD ImpD[], + UHWORD Nwing, bool Interp, + HWORD *Xp, HWORD Ph, HWORD Inc) +{ + HWORD *Hp, *Hdp = NULL, *End; + HWORD a = 0; + WORD v, t; + + v=0; + Hp = &Imp[Ph>>Na]; + End = &Imp[Nwing]; + if (Interp) { + Hdp = &ImpD[Ph>>Na]; + a = Ph & Amask; + } + if (Inc == 1) /* If doing right wing... */ + { /* ...drop extra coeff, so when Ph is */ + End--; /* 0.5, we don't do too many mult's */ + if (Ph == 0) /* If the phase is zero... */ + { /* ...then we've already skipped the */ + Hp += Npc; /* first sample, so we must also */ + Hdp += Npc; /* skip ahead in Imp[] and ImpD[] */ + } + } + if (Interp) + while (Hp < End) { + t = *Hp; /* Get filter coeff */ + t += (((WORD)*Hdp)*a)>>Na; /* t is now interp'd filter coeff */ + Hdp += Npc; /* Filter coeff differences step */ + t *= *Xp; /* Mult coeff by input sample */ + if (t & (1<<(Nhxn-1))) /* Round, if needed */ + t += (1<<(Nhxn-1)); + t >>= Nhxn; /* Leave some guard bits, but come back some */ + v += t; /* The filter output */ + Hp += Npc; /* Filter coeff step */ + Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ + } + else + while (Hp < End) { + t = *Hp; /* Get filter coeff */ + t *= *Xp; /* Mult coeff by input sample */ + if (t & (1<<(Nhxn-1))) /* Round, if needed */ + t += (1<<(Nhxn-1)); + t >>= Nhxn; /* Leave some guard bits, but come back some */ + v += t; /* The filter output */ + Hp += Npc; /* Filter coeff step */ + Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ + } + return(v); +} + +WORD FilterUD( HWORD Imp[], HWORD ImpD[], + UHWORD Nwing, bool Interp, + HWORD *Xp, HWORD Ph, HWORD Inc, UHWORD dhb) +{ + HWORD a; + HWORD *Hp, *Hdp, *End; + WORD v, t; + UWORD Ho; + + v=0; + Ho = (Ph*(UWORD)dhb)>>Np; + End = &Imp[Nwing]; + if (Inc == 1) /* If doing right wing... */ + { /* ...drop extra coeff, so when Ph is */ + End--; /* 0.5, we don't do too many mult's */ + if (Ph == 0) /* If the phase is zero... */ + Ho += dhb; /* ...then we've already skipped the */ + } /* first sample, so we must also */ + /* skip ahead in Imp[] and ImpD[] */ + if (Interp) + while ((Hp = &Imp[Ho>>Na]) < End) { + t = *Hp; /* Get IR sample */ + Hdp = &ImpD[Ho>>Na]; /* get interp (lower Na) bits from diff table*/ + a = Ho & Amask; /* a is logically between 0 and 1 */ + t += (((WORD)*Hdp)*a)>>Na; /* t is now interp'd filter coeff */ + t *= *Xp; /* Mult coeff by input sample */ + if (t & 1<<(Nhxn-1)) /* Round, if needed */ + t += 1<<(Nhxn-1); + t >>= Nhxn; /* Leave some guard bits, but come back some */ + v += t; /* The filter output */ + Ho += dhb; /* IR step */ + Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ + } + else + while ((Hp = &Imp[Ho>>Na]) < End) { + t = *Hp; /* Get IR sample */ + t *= *Xp; /* Mult coeff by input sample */ + if (t & 1<<(Nhxn-1)) /* Round, if needed */ + t += 1<<(Nhxn-1); + t >>= Nhxn; /* Leave some guard bits, but come back some */ + v += t; /* The filter output */ + Ho += dhb; /* IR step */ + Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ + } + return(v); +} -/* 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 */ - 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[] */ - long Yposition; /* FIXME: offset into Y buffer */ - Float *X, *Y; /* I/O buffers */ -} *resample_t; - -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); +#pragma mark - + + +#if 0 +static int resampleWithFilter( /* number of output samples returned */ + double factor, /* factor = outSampleRate/inSampleRate */ + int infd, /* input and output file descriptors */ + int outfd, + int inCount, /* number of input samples to convert */ + int outCount, /* number of output samples to compute */ + int nChans, /* number of sound channels (1 or 2) */ + bool interpFilt, /* TRUE means interpolate filter coeffs */ + HWORD Imp[], HWORD ImpD[], + UHWORD LpScl, UHWORD Nmult, UHWORD Nwing) +{ + UWORD Time, Time2; /* Current time/pos in input sample */ + UHWORD Xp, Ncreep, Xoff, Xread; + int OBUFFSIZE = (int)(((double)IBUFFSIZE)*factor+2.0); + HWORD X1[IBUFFSIZE], Y1[OBUFFSIZE]; /* I/O buffers */ + HWORD X2[IBUFFSIZE], Y2[OBUFFSIZE]; /* I/O buffers */ + UHWORD Nout, Nx; + int i, Ycount, last; + + MUS_SAMPLE_TYPE **obufs = sndlib_allocate_buffers(nChans, OBUFFSIZE); + if (obufs == NULL) + return err_ret("Can't allocate output buffers"); + + /* Account for increased filter gain when using factors less than 1 */ + if (factor < 1) + LpScl = LpScl*factor + 0.5; + + /* Calc reach of LP filter wing & give some creeping room */ + Xoff = ((Nmult+1)/2.0) * MAX(1.0,1.0/factor) + 10; + + if (IBUFFSIZE < 2*Xoff) /* Check input buffer size */ + return err_ret("IBUFFSIZE (or factor) is too small"); + + Nx = IBUFFSIZE - 2*Xoff; /* # of samples to process each iteration */ + + last = 0; /* Have not read last input sample yet */ + Ycount = 0; /* Current sample and length of output file */ + Xp = Xoff; /* Current "now"-sample pointer for input */ + Xread = Xoff; /* Position in input array to read into */ + Time = (Xoff<<Np); /* Current-time pointer for converter */ + + for (i=0; i<Xoff; X1[i++]=0); /* Need Xoff zeros at begining of sample */ + for (i=0; i<Xoff; X2[i++]=0); /* Need Xoff zeros at begining of sample */ + + do { + if (!last) /* If haven't read last sample yet */ + { + last = readData(infd, inCount, X1, X2, IBUFFSIZE, + nChans, (int)Xread); + if (last && (last-Xoff<Nx)) { /* If last sample has been read... */ + Nx = last-Xoff; /* ...calc last sample affected by filter */ + if (Nx <= 0) + break; + } + } + /* Resample stuff in input buffer */ + Time2 = Time; + if (factor >= 1) { /* SrcUp() is faster if we can use it */ + Nout=SrcUp(X1,Y1,factor,&Time,Nx,Nwing,LpScl,Imp,ImpD,interpFilt); + if (nChans==2) + Nout=SrcUp(X2,Y2,factor,&Time2,Nx,Nwing,LpScl,Imp,ImpD, + interpFilt); + } + else { + Nout=SrcUD(X1,Y1,factor,&Time,Nx,Nwing,LpScl,Imp,ImpD,interpFilt); + if (nChans==2) + Nout=SrcUD(X2,Y2,factor,&Time2,Nx,Nwing,LpScl,Imp,ImpD, + interpFilt); + } + + Time -= (Nx<<Np); /* Move converter Nx samples back in time */ + Xp += Nx; /* Advance by number of samples processed */ + Ncreep = (Time>>Np) - Xoff; /* Calc time accumulation in Time */ + if (Ncreep) { + Time -= (Ncreep<<Np); /* Remove time accumulation */ + Xp += Ncreep; /* and add it to read pointer */ + } + for (i=0; i<IBUFFSIZE-Xp+Xoff; i++) { /* Copy part of input signal */ + X1[i] = X1[i+Xp-Xoff]; /* that must be re-used */ + if (nChans==2) + X2[i] = X2[i+Xp-Xoff]; /* that must be re-used */ + } + if (last) { /* If near end of sample... */ + last -= Xp; /* ...keep track were it ends */ + if (!last) /* Lengthen input by 1 sample if... */ + last++; /* ...needed to keep flag TRUE */ + } + Xread = i; /* Pos in input buff to read new data into */ + Xp = Xoff; + + Ycount += Nout; + if (Ycount>outCount) { + Nout -= (Ycount-outCount); + Ycount = outCount; + } + + if (Nout > OBUFFSIZE) /* Check to see if output buff overflowed */ + return err_ret("Output array overflow"); + + if (nChans==1) { + for (i = 0; i < Nout; i++) + obufs[0][i] = HWORD_TO_MUS_SAMPLE_TYPE(Y1[i]); + } else { + for (i = 0; i < Nout; i++) { + obufs[0][i] = HWORD_TO_MUS_SAMPLE_TYPE(Y1[i]); + obufs[1][i] = HWORD_TO_MUS_SAMPLE_TYPE(Y2[i]); + } + } + /* NB: errors reported within sndlib */ + mus_file_write(outfd, 0, Nout - 1, nChans, obufs); + + printf("."); fflush(stdout); + + } while (Ycount<outCount); /* Continue until done */ + + return(Ycount); /* Return # of samples in output file */ +} +#endif + + +#pragma mark - + + +#if 0 /* here for linear interp. might be useful for other things */ static st_rate_t st_gcd(st_rate_t a, st_rate_t b) { @@ -162,63 +584,9 @@ static st_rate_t st_gcd(st_rate_t a, st_rate_t b) /* - * Process options - */ -int st_resample_getopts(eff_t effp, int n, const 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); -} - -/* * Prepare processing. */ -int st_resample_start(eff_t effp, st_rate_t inrate, st_rate_t outrate) { - resample_t r = (resample_t) effp->priv; +int st_resample_start(resample_t r, st_rate_t inrate, st_rate_t outrate) { long Xoff, gcdrate; int i; @@ -256,7 +624,7 @@ int st_resample_start(eff_t effp, st_rate_t inrate, st_rate_t outrate) { 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); + i = makeFilter(r->Imp, r->Nwing, r->rolloff, r->beta, r->Nq); if (i <= 0) { st_fail("resample: Unable to make filter\n"); return (ST_EOF); @@ -313,8 +681,7 @@ int st_resample_start(eff_t effp, st_rate_t inrate, st_rate_t outrate) { * Processed signed long samples from ibuf to obuf. * Return number of samples processed. */ -int st_resample_flow(eff_t effp, AudioInputStream &input, st_sample_t *obuf, st_size_t *osamp, st_volume_t vol) { - resample_t r = (resample_t) effp->priv; +int st_resample_flow(resample_t r, AudioInputStream &input, st_sample_t *obuf, st_size_t *osamp, st_volume_t vol) { long i, k, last; long Nout = 0; // The number of bytes we effectively output long Nx; // The number of bytes we will read from input @@ -450,8 +817,7 @@ printf("osamp = %ld, Nout = %ld\n", obufSize, Nout); /* * Process tail of input samples. */ -int st_resample_drain(eff_t effp, st_sample_t *obuf, st_size_t *osamp, st_volume_t vol) { - resample_t r = (resample_t) effp->priv; +int st_resample_drain(resample_t r, st_sample_t *obuf, st_size_t *osamp, st_volume_t vol) { long osamp_res; st_sample_t *Obuf; int rc; @@ -465,7 +831,7 @@ int st_resample_drain(eff_t effp, st_sample_t *obuf, st_size_t *osamp, st_volume while (!zero.eos() && osamp_res > 0) { st_sample_t Osamp; Osamp = osamp_res; - rc = st_resample_flow(effp, zero, Obuf, (st_size_t *) & Osamp, vol); + rc = st_resample_flow(r, zero, Obuf, (st_size_t *) & Osamp, vol); if (rc) return rc; /*fprintf(stderr,"DRAIN isamp,osamp (%d,%d) -> (%d,%d)\n", @@ -485,322 +851,108 @@ int st_resample_drain(eff_t effp, st_sample_t *obuf, st_size_t *osamp, st_volume * Do anything required when you stop reading samples. * Don't close input file! */ -int st_resample_stop(eff_t effp) { - resample_t r = (resample_t) effp->priv; - +int st_resample_stop(resample_t r) { free(r->Imp - 1); free(r->X); /* free(r->Y); Y is in same block starting at X */ return (ST_SUCCESS); } -/* over 90% of CPU time spent in this iprodUD() function */ -/* quadratic interpolation */ -static double qprodUD(const Float Imp[], const Float *Xp, long Inc, double T0, - long dhb, long ct) { - const double f = 1.0 / (1 << La); - double v; - long Ho; - - Ho = (long)(T0 * dhb); - Ho += (ct - 1) * dhb; /* so Float sum starts with smallest coef's */ - Xp += (ct - 1) * Inc; - v = 0; - do { - Float coef; - long Hoh; - Hoh = Ho >> La; - coef = Imp[Hoh]; - { - Float dm, dp, t; - dm = coef - Imp[Hoh - 1]; - dp = Imp[Hoh + 1] - coef; - t = (Ho & Amask) * f; - coef += ((dp - dm) * t + (dp + dm)) * t * 0.5; - } - /* filter coef, lower La bits by quadratic interpolation */ - v += coef * *Xp; /* sum coeff * input sample */ - Xp -= Inc; /* Input signal step. NO CHECK ON ARRAY BOUNDS */ - Ho -= dhb; /* IR step */ - } while (--ct); - return v; -} - -/* linear interpolation */ -static double iprodUD(const Float Imp[], const Float *Xp, long Inc, - double T0, long dhb, long ct) { - const double f = 1.0 / (1 << La); - double v; - long Ho; - - Ho = (long)(T0 * dhb); - Ho += (ct - 1) * dhb; /* so Float sum starts with smallest coef's */ - Xp += (ct - 1) * Inc; - v = 0; - do { - Float coef; - long Hoh; - Hoh = Ho >> La; - /* if (Hoh >= End) break; */ - coef = Imp[Hoh] + (Imp[Hoh + 1] - Imp[Hoh]) * (Ho & Amask) * f; - /* filter coef, lower La bits by linear interpolation */ - v += coef * *Xp; /* sum coeff * input sample */ - Xp -= Inc; /* Input signal step. NO CHECK ON ARRAY BOUNDS */ - Ho -= dhb; /* IR step */ - } while (--ct); - return v; -} - -/* From resample:filters.c */ -/* Sampling rate conversion subroutine */ - -static long SrcUD(resample_t r, long Nx) { - Float *Ystart, *Y; - double Factor; - double dt; /* Step through input signal */ - double time; - double (*prodUD)(const Float Imp[], const Float *Xp, long Inc, double T0, long dhb, long ct); - int n; - - prodUD = (r->quadr) ? qprodUD : iprodUD; /* quadratic or linear interp */ - Factor = r->Factor; - time = r->Time; - dt = 1.0 / Factor; /* Output sampling period */ - //fprintf(stderr,"Factor %f, dt %f, ",Factor,dt); - //fprintf(stderr,"Time %f, ",r->Time); - /* (Xh * dhb)>>La is max index into Imp[] */ - /*fprintf(stderr,"ct=%d\n",ct);*/ - //fprintf(stderr,"ct=%.2f %d\n",(double)r->Nwing*Na/r->dhb, r->Xh); - //fprintf(stderr,"ct=%ld, T=%.6f, dhb=%6f, dt=%.6f\n", r->Xh, time-floor(time),(double)r->dhb/Na,dt); - Ystart = Y = r->Y + r->Yposition; - n = (int)ceil((double)Nx / dt); - while (n--) { - Float *Xp; - double v; - double T; - T = time - floor(time); /* fractional part of Time */ - Xp = r->X + (long)time; /* Ptr to current input sample */ - - /* Past inner product: */ - v = (*prodUD)(r->Imp, Xp, -1, T, r->dhb, r->Xh); /* needs Np*Nmult in 31 bits */ - /* Future inner product: */ - v += (*prodUD)(r->Imp, Xp + 1, 1, (1.0 - T), r->dhb, r->Xh); /* prefer even total */ - - if (Factor < 1) - v *= Factor; - *Y++ = v; /* Deposit output */ - time += dt; /* Move to next sample by time increment */ - } - r->Time = time; - fprintf(stderr,"Time %f\n",r->Time); - return (Y - Ystart); /* Return the number of output samples */ -} - -/* exact coeff's */ -static double prodEX(const Float Imp[], const Float *Xp, - long Inc, long T0, long dhb, long ct) { - double v; - const Float *Cp; - - Cp = Imp + (ct - 1) * dhb + T0; /* so Float sum starts with smallest coef's */ - Xp += (ct - 1) * Inc; - v = 0; - do { - v += *Cp * *Xp; /* sum coeff * input sample */ - Cp -= dhb; /* IR step */ - Xp -= Inc; /* Input signal step. */ - } while (--ct); - return v; -} - -static long SrcEX(resample_t r, long Nx) { - Float *Ystart, *Y; - double Factor; - long a, b; - long time; - int n; - - Factor = r->Factor; - time = r->t; - a = r->a; - b = r->b; - Ystart = Y = r->Y + r->Yposition; - n = (Nx * b + (a - 1)) / a; - while (n--) { - Float *Xp; - double v; - long T; - T = time % b; /* fractional part of Time */ - Xp = r->X + (time / b); /* Ptr to current input sample */ - - /* Past inner product: */ - v = prodEX(r->Imp, Xp, -1, T, b, r->Xh); - /* Future inner product: */ - v += prodEX(r->Imp, Xp + 1, 1, b - T, b, r->Xh); - - if (Factor < 1) - v *= Factor; - *Y++ = v; /* Deposit output */ - time += a; /* Move to next sample by time increment */ - } - r->t = time; - return (Y - Ystart); /* Return the number of output samples */ -} - -int makeFilter(Float Imp[], long Nwing, double Froll, double Beta, - long Num, int Normalize) { - double *ImpR; - long Mwing, i; - - if (Nwing > MAXNWING) /* Check for valid parameters */ - return ( -1); - if ((Froll <= 0) || (Froll > 1)) - return ( -2); - - /* it does help accuracy a bit to have the window stop at - * a zero-crossing of the sinc function */ - Mwing = (long)(floor((double)Nwing / (Num / Froll)) * (Num / Froll) + 0.5); - if (Mwing == 0) - return ( -4); - - ImpR = (double *) malloc(sizeof(double) * Mwing); - - /* Design a Nuttall or Kaiser windowed Sinc low-pass filter */ - LpFilter(ImpR, Mwing, Froll, Beta, Num); - - if (Normalize) { /* 'correct' the DC gain of the lowpass filter */ - long Dh; - double DCgain; - DCgain = 0; - Dh = Num; /* Filter sampling period for factors>=1 */ - for (i = Dh; i < Mwing; i += Dh) - DCgain += ImpR[i]; - DCgain = 2 * DCgain + ImpR[0]; /* DC gain of real coefficients */ - st_report("DCgain err=%.12f",DCgain-1.0); // FIXME - - DCgain = 1.0 / DCgain; - for (i = 0; i < Mwing; i++) - Imp[i] = ImpR[i] * DCgain; - - } else { - for (i = 0; i < Mwing; i++) - Imp[i] = ImpR[i]; - } - free(ImpR); - for (i = Mwing; i <= Nwing; i++) - Imp[i] = 0; - /* Imp[Mwing] and Imp[-1] needed for quadratic interpolation */ - Imp[ -1] = Imp[1]; - - return (Mwing); -} - -/* LpFilter() - * - * reference: "Digital Filters, 2nd edition" - * R.W. Hamming, pp. 178-179 - * - * Izero() computes the 0th order modified bessel function of the first kind. - * (Needed to compute Kaiser window). - * - * LpFilter() computes the coeffs of a Kaiser-windowed low pass filter with - * the following characteristics: - * - * c[] = array in which to store computed coeffs - * frq = roll-off frequency of filter - * N = Half the window length in number of coeffs - * Beta = parameter of Kaiser window - * Num = number of coeffs before 1/frq - * - * Beta trades the rejection of the lowpass filter against the transition - * width from passband to stopband. Larger Beta means a slower - * transition and greater stopband rejection. See Rabiner and Gold - * (Theory and Application of DSP) under Kaiser windows for more about - * Beta. The following table from Rabiner and Gold gives some feel - * for the effect of Beta: - * - * All ripples in dB, width of transition band = D*N where N = window length - * - * BETA D PB RIP SB RIP - * 2.120 1.50 +-0.27 -30 - * 3.384 2.23 0.0864 -40 - * 4.538 2.93 0.0274 -50 - * 5.658 3.62 0.00868 -60 - * 6.764 4.32 0.00275 -70 - * 7.865 5.0 0.000868 -80 - * 8.960 5.7 0.000275 -90 - * 10.056 6.4 0.000087 -100 - */ - - -#define IzeroEPSILON 1E-21 /* Max error acceptable in Izero */ +#endif -static double Izero(double x) { - double sum, u, halfx, temp; - long n; +#pragma mark - - sum = u = n = 1; - halfx = x / 2.0; - do { - temp = halfx / (double)n; - n += 1; - temp *= temp; - u *= temp; - sum += u; - } while (u >= IzeroEPSILON*sum); - return (sum); -} -static void LpFilter(double *c, long N, double frq, double Beta, long Num) { - long i; +ResampleRateConverter::ResampleRateConverter(st_rate_t inrate, st_rate_t outrate, int quality) { + // FIXME: quality is for now a nasty hack. Valid values are 0,1,2,3 - /* Calculate filter coeffs: */ - c[0] = frq; - for (i = 1; i < N; i++) { - double x = M_PI * (double)i / (double)(Num); - c[i] = sin(x * frq) / x; - } + double rolloff; /* roll-off frequency */ + double beta; /* passband/stopband tuning magic */ - if (Beta > 2) { /* Apply Kaiser window to filter coeffs: */ - double IBeta = 1.0 / Izero(Beta); - for (i = 1; i < N; i++) { - double x = (double)i / (double)(N); - c[i] *= Izero(Beta * sqrt(1.0 - x * x)) * IBeta; - } - } else { /* Apply Nuttall window: */ - for (i = 0; i < N; i++) { - double x = M_PI * i / N; - c[i] *= 0.36335819 + 0.4891775 * cos(x) + 0.1365995 * cos(2 * x) + 0.0106411 * cos(3 * x); - } + switch (quality) { + case 0: + /* These defaults are conservative with respect to aliasing. */ + rolloff = 0.80; + beta = 16; + quadr = 0; + Nmult = 45; + break; + case 1: + rolloff = 0.80; + beta = 16; + quadr = 1; + Nmult = 45; + break; + case 2: + rolloff = 0.875; + beta = 16; + quadr = 1; + Nmult = 75; + break; + case 3: + rolloff = 0.94; + beta = 16; + quadr = 1; + Nmult = 149; + break; + default: + error("Illegal quality level %d\n", quality); + break; } -} - - -#pragma mark - + makeFilter(Imp, ImpD, &LpScl, Nmult, rolloff, beta); + + int OBUFFSIZE = (IBUFFSIZE * outrate / inrate + 2); + X1 = (HWORD *)malloc(IBUFFSIZE); + X2 = (HWORD *)malloc(IBUFFSIZE); + Y1 = (HWORD *)malloc(OBUFFSIZE); + Y2 = (HWORD *)malloc(OBUFFSIZE); + + // HACK this is invalid code but "fixes" a compiler warning for now + double factor = outrate / (double)inrate; + UHWORD Xp, /*Ncreep,*/ Xoff, Xread; + UHWORD Nout, Nx; + int Ycount, last; + + /* Account for increased filter gain when using factors less than 1 */ + if (factor < 1) + LpScl = (UHWORD)(LpScl*factor + 0.5); + + /* Calc reach of LP filter wing & give some creeping room */ + Xoff = (UHWORD)(((Nmult+1)/2.0) * MAX(1.0,1.0/factor) + 10); + + if (IBUFFSIZE < 2*Xoff) /* Check input buffer size */ + error("IBUFFSIZE (or factor) is too small"); + + Nx = IBUFFSIZE - 2*Xoff; /* # of samples to process each iteration */ + + last = 0; /* Have not read last input sample yet */ + Ycount = 0; /* Current sample and length of output file */ + Xp = Xoff; /* Current "now"-sample pointer for input */ + Xread = Xoff; /* Position in input array to read into */ + Time = (Xoff<<Np); /* Current-time pointer for converter */ + + Nout = SrcUp(X1, Y1, factor, &Time, Nx, Nwing, LpScl, Imp, ImpD, quadr); + Nout = SrcUD(X1, Y1, factor, &Time, Nx, Nwing, LpScl, Imp, ImpD, quadr); -ResampleRateConverter::ResampleRateConverter(st_rate_t inrate, st_rate_t outrate, int quality) { - // FIXME: quality is for now a nasty hack. - // Valid values are 0,1,2,3 (everything else is treated like 0 for now) - const char *arg = 0; - switch (quality) { - case 1: arg = "-qs"; break; - case 2: arg = "-q"; break; - case 3: arg = "-ql"; break; - } - st_resample_getopts(&effp, arg ? 1 : 0, &arg); - st_resample_start(&effp, inrate, outrate); +// st_resample_start(&rstuff, inrate, outrate); } ResampleRateConverter::~ResampleRateConverter() { - st_resample_stop(&effp); +// st_resample_stop(&rstuff); + free(X1); + free(X2); + free(Y1); + free(Y2); } int ResampleRateConverter::flow(AudioInputStream &input, st_sample_t *obuf, st_size_t osamp, st_volume_t vol) { - return st_resample_flow(&effp, input, obuf, &osamp, vol); +// return st_resample_flow(&rstuff, input, obuf, &osamp, vol); + return 0; } int ResampleRateConverter::drain(st_sample_t *obuf, st_size_t osamp, st_volume_t vol) { - return st_resample_drain(&effp, obuf, &osamp, vol); +// return st_resample_drain(&rstuff, obuf, &osamp, vol); + return 0; } |