diff options
Diffstat (limited to 'third_party/resample/src/filterkit.c')
| -rw-r--r-- | third_party/resample/src/filterkit.c | 631 |
1 files changed, 0 insertions, 631 deletions
diff --git a/third_party/resample/src/filterkit.c b/third_party/resample/src/filterkit.c deleted file mode 100644 index 5171b5ea..00000000 --- a/third_party/resample/src/filterkit.c +++ /dev/null @@ -1,631 +0,0 @@ -/* - * 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 <stdlib.h> -#include <string.h> -#include <stdio.h> -#include <math.h> - -/* - * 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<N; i++) { - temp = 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; - } -} - - -/* Write a filter to a file - * Filter file format: - * file name: "F" Nmult "T" Nhc ".filter" - * 1st line: the string "ScaleFactor" followed by its value. - * 2nd line: the string "Length" followed by Nwing's value. - * 3rd line: the string "Nmult" followed by Nmult's value. - * 4th line: the string "Coeffs:" on a separate line. - * following lines: Nwing number of 16-bit impulse response values - * in the right wing of the impulse response (the Imp[] array). - * (Nwing is equal to Npc*(Nmult+1)/2+1, where Npc is defined in the - * file "resample.h".) Each coefficient is on a separate line. - * next line: the string "Differences:" on a separate line. - * following lines: Nwing number of 16-bit impulse-response - * successive differences: ImpD[i] = Imp[i+1] - Imp[i]. - * ERROR codes: - * 0 - no error - * 1 - could not open file - */ - -int writeFilter(HWORD Imp[], HWORD ImpD[], UHWORD LpScl, UHWORD Nmult, - UHWORD Nwing) -{ - char fname[30]; - FILE *fp; - int i; - - sprintf(fname, "F%dT%d.filter", Nmult, Nhc); - fp = fopen(fname, "w"); - if (!fp) - return(1); - fprintf(fp, "ScaleFactor %d\n", LpScl); - fprintf(fp, "Length %d\n", Nwing); - fprintf(fp, "Nmult %d\n", Nmult); - fprintf(fp, "Coeffs:\n"); - for (i=0; i<Nwing; i++) /* Put array of 16-bit filter coefficients */ - fprintf(fp, "%d\n", Imp[i]); - fprintf(fp, "Differences:\n"); - for (i=0; i<Nwing; i++) /* Put array of 16-bit filter coeff differences */ - fprintf(fp, "%d\n", ImpD[i]); - fclose(fp); - printf("Wrote filter file '%s' in current directory.\n",fname); - return(0); -} - - -/* 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 - * - * 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<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 = 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] = 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); -} - - -/* Read-in a filter - * Filter file format: - * file name: "F" Nmult "T" Nhc ".filter" - * 1st line: the string "ScaleFactor" followed by its value. - * 2nd line: the string "Length" followed by Nwing's value. - * 3rd line: the string "Coeffs:" on separate line. - * Nwing number of 16-bit impulse response values in the right - * wing of the impulse response. (Length=Npc*(Nmult+1)/2+1, - * where originally Npc=2^9, and Nmult=13.) Each on separate line. - * The string "Differences:" on separate line. - * Nwing number of 16-bit impulse-response successive differences: - * ImpDiff[i] = Imp[i+1] - Imp[i]. - * - * ERROR return codes: - * 0 - no error - * 1 - file not found - * 2 - invalid ScaleFactor in file - * 3 - invalid Length in file - * 4 - invalid Nmult in file - */ -int readFilter(char *filterFile, HWORD **ImpP, HWORD **ImpDP, UHWORD *LpScl, - UHWORD *Nmult, UHWORD *Nwing) -{ - char *fname; - FILE *fp; - int i, temp; - HWORD *Imp,*ImpD; - - if (!filterFile || !(*filterFile)) { - fname = (char *) malloc(32); - if ((*Nmult)>0 && ((*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<<Na) + 0.5; - - for (i=0; i<MAXITER; i++) - { - x = (hi+lo)/2.0; - v = FilterUD(Imp,ImpD,Nwing,TRUE,Data, (HWORD)(x*Pmask), -1,dhb); - v += FilterUD(Imp,ImpD,Nwing,TRUE,Data+1,(HWORD)((1-x)*Pmask), 1,dhb); - v >>= Nhg; - v *= LpScl; - out = (double)v / (double)(1<<NLpScl); - if (out < 0.0) - lo = x; - else - hi = x; - if (ABS(out) <= ZeroxEPSILON) - return(x); - } - printf("|ZeroX Error| x:%g, \t Data[x]:%d, \t Data[x+1]:%d\n", - x, *Data, *(Data+1)); - printf("|\tABS(out):%g \t EPSILON:%g\n", ABS(out),ZeroxEPSILON); - if (ABS(out) <= ZeroxMAXERROR) - return(x); - return(-100.0); -} - - -BOOL Query(char *prompt, BOOL deflt, char *help) -{ - char s[80]; - - while (TRUE) - { - sprintf(s,"\n%s%s", prompt, (*help) ? " (Type ? for help)" : ""); - getstr(s,(deflt)?"yes":"no",s); - if (*s=='?' && *help) - printf(help); - if (*s=='Y' || *s=='y') - return(TRUE); - if (*s=='N' || *s=='n') - return(FALSE); - } -} - - -char *GetString(char *prompt, char *deflt, char *help) -{ - static char s[200]; - - while (TRUE) - { - sprintf(s,"\n%s%s",prompt, (*help) ? " (Type ? for Help)" : ""); - getstr(s,deflt,s); - if (*s=='?' && *help) - printf(help); - else - return(s); - } -} - - -double GetDouble(char *title, double deflt, char *help) -{ - char s[80],sdeflt[80]; - double newval; - - while (TRUE) - { - sprintf(s,"\n%s:%s",title, (*help) ? " (Type ? for Help)" : ""); - sprintf(sdeflt,"%g",deflt); - getstr(s,sdeflt,s); - if (*s=='?' && *help) - printf(help); - else - { - if (!sscanf(s,"%lf",&newval)) - return(deflt); - return(newval); - } - } -} - - -unsigned short GetUShort(char *title, unsigned short deflt, char *help) -{ - char s[80],sdeflt[80]; - int newval; - - while (TRUE) - { - sprintf(s,"\n%s:%s",title, (*help) ? " (Type ? for Help)" : ""); - sprintf(sdeflt,"%d",deflt); - getstr(s,sdeflt,s); - if (*s=='?' && *help) - printf(help); - else - { - if (!sscanf(s,"%d",&newval)) - printf("unchanged (%d)\n",(newval=deflt)); - if (newval < 0) - printf("Error: value must be >= zero\n"); - else - return(newval); - } - } -} |