From ca07a9833149d270d1e5b44cfa381ac6ac03e39c Mon Sep 17 00:00:00 2001 From: Benny Prijono Date: Sat, 7 Apr 2007 14:53:15 +0000 Subject: Moved resample to third_party directory git-svn-id: http://svn.pjsip.org/repos/pjproject/branches/split-3rd-party@1170 74dad513-b988-da41-8d7b-12977e46ad98 --- third_party/resample/src/filterkit.c | 631 +++++++++++++++++++++++++++++++++++ 1 file changed, 631 insertions(+) create mode 100644 third_party/resample/src/filterkit.c (limited to 'third_party/resample/src/filterkit.c') diff --git a/third_party/resample/src/filterkit.c b/third_party/resample/src/filterkit.c new file mode 100644 index 00000000..5171b5ea --- /dev/null +++ b/third_party/resample/src/filterkit.c @@ -0,0 +1,631 @@ +/* + * filterkit.c (library "filterkit.a"): Kaiser-windowed low-pass filter support. + */ + +/* filterkit.c + * + * LpFilter() - 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. + * writeFilter() - Writes a filter to a file. + * makeFilter() - Calls LpFilter() to create a filter, then scales the double + * coeffs into an array of half words. + * readFilter() - Reads a filter from a file. + * FilterUp() - Applies a filter to a given sample when up-converting. + * FilterUD() - Applies a filter to a given sample when up- or down- + * converting. + * initZerox() - Initialization routine for the zerox() function. Must + * be called before zerox() is called. This routine loads + * the correct filter so zerox() can use it. + * zerox() - Given a pointer into a sample, finds a zero-crossing on the + * interval [pointer-1:pointer+2] by iteration. + * Query() - Ask the user for a yes/no question with prompt, default, + * and optional help. + * GetUShort() - Ask the user for a unsigned short with prompt, default, + * and optional help. + * GetDouble() - Ask the user for a double with prompt, default, and + * optional help. + * GetString() - Ask the user for a string with prompt, default, and + * optional help. + */ + +#include "resample.h" +#include "filterkit.h" + +#include +#include +#include +#include + +/* + * Getstr() will print the passed "prompt" as a message to the user, and + * wait for the user to type an input string. The string is + * then copied into "answer". If the user types just a carriage + * return, then the string "defaultAnswer" is copied into "answer". + * ??? + * "Answer" and "defaultAnswer" may be the same string, in which case, + * the defaultAnswer value will be the original contents of "answer". + * ??? + */ +static void getstr(char *prompt, char *defaultAnswer, char *answer) +{ + char *p,s[200]; + + printf("%s (defaultAnswer = %s):",prompt,defaultAnswer); + strcpy(s, prompt); + if (!(*s)) + strcpy(s, "input:"); + p = s; + while (*p && *p != '(') + p++; + p--; + while (*p == ' ') + p--; + *(++p) = '\0'; + + /* gets(answer); */ + fgets(answer,sizeof(answer),stdin); + answer[strlen(answer)-1] = '\0'; + + if (answer[0] == '\0') { + strcpy(answer, defaultAnswer); + printf("\t%s = %s\n",s,answer); + } else { + printf("\t%s set to %s\n",s,answer); + } +} + + +/* 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 */ + +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 MAXNWING) + * 2 - Froll is not in interval [0:1) + * 3 - Beta is < 1.0 + * 4 - LpScl will not fit in 16-bits + * + * Made following global to avoid stack problems in Sun3 compilation: */ + +#define MAXNWING 8192 +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= 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; i0 && ((*Nmult)&1)) + sprintf(fname, "F%dT%d.filter", *Nmult, Nhc); + else + sprintf(fname, "F65dT%d.filter", Nhc); + } else + fname = filterFile; + + fp = fopen(fname, "r"); + if (fp == NULL) + return(1); + + fscanf(fp, "ScaleFactor "); + if (1 != fscanf(fp,"%d",&temp)) + return(2); + *LpScl = temp; + + fscanf(fp, "\nLength "); + if (1 != fscanf(fp,"%d",&temp)) + return(3); + *Nwing = temp; + + fscanf(fp, "\nNmult "); + if (1 != fscanf(fp,"%d",&temp)) + return(4); + *Nmult = temp; + + Imp = (HWORD *) malloc(*Nwing * sizeof(HWORD)); + + fscanf(fp, "\nCoeffs:\n"); + for (i=0; i<*Nwing; i++) { /* Get array of 16-bit filter coefficients */ + fscanf(fp, "%d\n", &temp); + Imp[i] = temp; + } + + ImpD = (HWORD *) malloc(*Nwing * sizeof(HWORD)); + + fscanf(fp, "\nDifferences:\n"); + for (i=0; i<*Nwing; i++) { /* Get array of 16bit filter coeff differences */ + fscanf(fp, "%d\n", &temp); + ImpD[i] = temp; + } + + fclose(fp); + if (!filterFile || !(*filterFile)) + free(fname); + *ImpP = Imp; + *ImpDP = ImpD; + return(0); +} + + +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); +} + +/* + * double zerox(Data, Factor) + * HWORD *Data; + * double Factor; + * Given a pointer into a sound sample, this function uses a low-pass + * filter to estimate the x coordinate of the zero-crossing which must ocurr + * between Data[0] and Data[1]. This value is returned as the value of the + * function. A return value of -100 indicates there was no zero-crossing in + * the x interval [-1,2]. Factor is the resampling factor: Rate(out) / + * Rate(in). Nmult (which determines which filter is used) is passed the + * zerox's initialization routine: initZerox(Nmult) + * UHWORD Nmult; + */ + +static UHWORD LpScl, Nmult, Nwing; +static HWORD *Imp; +static HWORD *ImpD; + +/* ERROR return values: + * 0 - no error + * 1 - Nmult is even (should be odd) + * 2 - filter file not found + * 3 - invalid ScaleFactor in input file + * 4 - invalid Length in file + * 5 - invalid Nmult in file + */ +int initZerox(UHWORD tempNmult) +{ + int err; + + /* Check for illegal input values */ + if (!(tempNmult % 2)) + return(1); + err = readFilter(NULL, (HWORD **)&Imp, (HWORD **)&ImpD, + &LpScl, &tempNmult, &Nwing); + if (err) + return(1+err); + + Nmult = tempNmult; + return(0); +} + +#define MAXITER 64 +#define ZeroxEPSILON (1E-4) +#define ZeroxMAXERROR (5.0) + +double zerox(HWORD *Data, double Factor) +{ + double x, out; + double lo, hi; + double dh; + UWORD dhb; + WORD v; + int i; + + if (!Data[0]) + return (0.0); + if (!Data[1]) + return (1.0); + + if (Data[0] < Data[1]) + { + lo = -1.0; + hi = 2.0; + } + else + { + lo = 2.0; + hi = -1.0; + } + dh = (Factor<1) ? (Factor*Npc) : (Npc); + dhb = dh * (1<>= Nhg; + v *= LpScl; + out = (double)v / (double)(1<= zero\n"); + else + return(newval); + } + } +} -- cgit v1.2.3