diff options
Diffstat (limited to 'third_party/resample/src/resamplesubs.c')
| -rw-r--r-- | third_party/resample/src/resamplesubs.c | 520 |
1 files changed, 520 insertions, 0 deletions
diff --git a/third_party/resample/src/resamplesubs.c b/third_party/resample/src/resamplesubs.c new file mode 100644 index 00000000..920e56e6 --- /dev/null +++ b/third_party/resample/src/resamplesubs.c @@ -0,0 +1,520 @@ +/* resamplesubs.c - sampling rate conversion subroutines */ +// Altered version + +#include "resample.h" + +#include <stdlib.h> +#include <stdio.h> +#include <math.h> +#include <string.h> + +#define IBUFFSIZE 4096 /* Input buffer size */ + +#include "smallfilter.h" +#include "largefilter.h" + +#include "filterkit.h" +#include "sndlibextra.h" + +#ifdef WITH_PJ +#include "resamplesubs.h" + +unsigned resample_LARGE_FILTER_NMULT = LARGE_FILTER_NMULT; +unsigned resample_LARGE_FILTER_NWING = LARGE_FILTER_NWING; +unsigned resample_LARGE_FILTER_SCALE = LARGE_FILTER_SCALE; +short* resample_LARGE_FILTER_IMP = LARGE_FILTER_IMP; +short* resample_LARGE_FILTER_IMPD = LARGE_FILTER_IMPD; + +unsigned resample_SMALL_FILTER_NMULT = SMALL_FILTER_NMULT; +unsigned resample_SMALL_FILTER_NWING = SMALL_FILTER_NWING; +unsigned resample_SMALL_FILTER_SCALE = SMALL_FILTER_SCALE; +short* resample_SMALL_FILTER_IMP = SMALL_FILTER_IMP; +short* resample_SMALL_FILTER_IMPD = SMALL_FILTER_IMPD; +#endif + + +/* CAUTION: Assumes we call this for only one resample job per program run! */ +/* return: 0 - notDone */ +/* >0 - index of last sample */ +static int +readData(int infd, /* input file descriptor */ + int inCount, /* _total_ number of frames in input file */ + HWORD *outPtr1, /* array receiving left chan samps */ + HWORD *outPtr2, /* array receiving right chan samps */ + int dataArraySize, /* size of these arrays */ + int nChans, + int Xoff) /* read into input array starting at this index */ +{ + int i, Nsamps, nret; + static unsigned int framecount; /* frames previously read */ + static mus_sample_t **ibufs = NULL; + + if (ibufs == NULL) { /* first time called, so allocate it */ + ibufs = sndlib_allocate_buffers(nChans, dataArraySize); + if (ibufs == NULL) { + fprintf(stderr, "readData: Can't allocate input buffers!\n"); + exit(1); + } + framecount = 0; /* init this too */ + } + + Nsamps = dataArraySize - Xoff; /* Calculate number of samples to get */ + outPtr1 += Xoff; /* Start at designated sample number */ + outPtr2 += Xoff; + + nret = mus_file_read(infd, 0, Nsamps - 1, nChans, ibufs); + if (nret < 0) { + fprintf(stderr, "readData: Can't read data!\n"); + exit(1); + } + + /* NB: sndlib pads ibufs with zeros if it reads past EOF. */ + if (nChans == 1) { + for (i = 0; i < Nsamps; i++) + *outPtr1++ = MUS_SAMPLE_TYPE_TO_HWORD(ibufs[0][i]); + } + else { + for (i = 0; i < Nsamps; i++) { + *outPtr1++ = MUS_SAMPLE_TYPE_TO_HWORD(ibufs[0][i]); + *outPtr2++ = MUS_SAMPLE_TYPE_TO_HWORD(ibufs[1][i]); + } + } + + framecount += Nsamps; + + if (framecount >= (unsigned)inCount) /* return index of last samp */ + return (((Nsamps - (framecount - inCount)) - 1) + Xoff); + else + return 0; +} + + +#ifdef DEBUG +static int pof = 0; /* positive overflow count */ +static int nof = 0; /* negative overflow count */ +#endif + +static INLINE HWORD WordToHword(WORD v, int scl) +{ + HWORD out; + WORD llsb = (1<<(scl-1)); + v += llsb; /* round */ + v >>= scl; + if (v>MAX_HWORD) { +#ifdef DEBUG + if (pof == 0) + fprintf(stderr, "*** resample: sound sample overflow\n"); + else if ((pof % 10000) == 0) + fprintf(stderr, "*** resample: another ten thousand overflows\n"); + pof++; +#endif + v = MAX_HWORD; + } else if (v < MIN_HWORD) { +#ifdef DEBUG + if (nof == 0) + fprintf(stderr, "*** resample: sound sample (-) overflow\n"); + else if ((nof % 1000) == 0) + fprintf(stderr, "*** resample: another thousand (-) overflows\n"); + nof++; +#endif + v = MIN_HWORD; + } + out = (HWORD) v; + return out; +} + +/* Sampling rate conversion using linear interpolation for maximum speed. + */ +STATIC int + SrcLinear(HWORD X[], HWORD Y[], double factor, UWORD *Time, UHWORD Nx) +{ + HWORD iconst; + HWORD *Xp, *Ystart; + WORD v,x1,x2; + + 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 = dt*(1<<Np) + 0.5; /* Fixed-point representation */ + + Ystart = Y; + endTime = *Time + (1<<Np)*(WORD)Nx; + while (*Time < endTime) + { + iconst = (*Time) & Pmask; + Xp = &X[(*Time)>>Np]; /* Ptr to current input sample */ + x1 = *Xp++; + x2 = *Xp; + x1 *= ((1<<Np)-iconst); + x2 *= iconst; + v = x1 + x2; + *Y++ = WordToHword(v,Np); /* Deposit output */ + *Time += dtb; /* Move to next sample by time increment */ + } + return (Y - Ystart); /* Return number of output samples */ +} + +/* 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 = 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 = dt*(1<<Np) + 0.5; /* Fixed-point representation */ + + dh = MIN(Npc, factor*Npc); /* Filter sampling period */ + dhb = 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 */ +} + +#ifndef WITH_PJ +static int err_ret(char *s) +{ + fprintf(stderr,"resample: %s \n\n",s); /* Display error message */ + return -1; +} + +static int resampleFast( /* number of output samples returned */ + double factor, /* factor = Sndout/Sndin */ + 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) */ +{ + 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[2]; /* I/O buffers */ + HWORD X2[IBUFFSIZE], Y2[OBUFFSIZE]; /* I/O buffers */ + UHWORD Nout, Nx; + int i, Ycount, last; + + mus_sample_t **obufs = sndlib_allocate_buffers(nChans, OBUFFSIZE); + if (obufs == NULL) + return err_ret("Can't allocate output buffers"); + + Xoff = 10; + + 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; + Nout=SrcLinear(X1,Y1,factor,&Time,Nx); + if (nChans==2) + Nout=SrcLinear(X2,Y2,factor,&Time2,Nx); + + 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 */ +} + + +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_t **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 */ +} + + +int resample( /* number of output samples returned */ + double factor, /* factor = Sndout/Sndin */ + 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 */ + int fastMode, /* 0 = highest quality, slowest speed */ + BOOL largeFilter, /* TRUE means use 65-tap FIR filter */ + char *filterFile) /* NULL for internal filter, else filename */ +{ + UHWORD LpScl; /* Unity-gain scale factor */ + UHWORD Nwing; /* Filter table size */ + UHWORD Nmult; /* Filter length for up-conversions */ + HWORD *Imp=0; /* Filter coefficients */ + HWORD *ImpD=0; /* ImpD[n] = Imp[n+1]-Imp[n] */ + + if (fastMode) + return resampleFast(factor,infd,outfd,inCount,outCount,nChans); + +#ifdef DEBUG + /* Check for illegal constants */ + if (Np >= 16) + return err_ret("Error: Np>=16"); + if (Nb+Nhg+NLpScl >= 32) + return err_ret("Error: Nb+Nhg+NLpScl>=32"); + if (Nh+Nb > 32) + return err_ret("Error: Nh+Nb>32"); +#endif + + /* Set defaults */ + + if (filterFile != NULL && *filterFile != '\0') { + if (readFilter(filterFile, &Imp, &ImpD, &LpScl, &Nmult, &Nwing)) + return err_ret("could not find filter file, " + "or syntax error in contents of filter file"); + } else if (largeFilter) { + Nmult = LARGE_FILTER_NMULT; + Imp = LARGE_FILTER_IMP; /* Impulse response */ + ImpD = LARGE_FILTER_IMPD; /* Impulse response deltas */ + LpScl = LARGE_FILTER_SCALE; /* Unity-gain scale factor */ + Nwing = LARGE_FILTER_NWING; /* Filter table length */ + } else { + Nmult = SMALL_FILTER_NMULT; + Imp = SMALL_FILTER_IMP; /* Impulse response */ + ImpD = SMALL_FILTER_IMPD; /* Impulse response deltas */ + LpScl = SMALL_FILTER_SCALE; /* Unity-gain scale factor */ + Nwing = SMALL_FILTER_NWING; /* Filter table length */ + } +#if DEBUG + fprintf(stderr,"Attenuating resampler scale factor by 0.95 " + "to reduce probability of clipping\n"); +#endif + LpScl *= 0.95; + return resampleWithFilter(factor,infd,outfd,inCount,outCount,nChans, + interpFilt, Imp, ImpD, LpScl, Nmult, Nwing); +} +#endif /* WITH_PJ */ + |