1 //
2 //	Fidlib digital filter designer code
3 //	-----------------------------------
4 //
5 //        Copyright (c) 2002-2004 Jim Peters <http://uazu.net/>.  This
6 //        file is released under the GNU Lesser General Public License
7 //        (LGPL) version 2.1 as published by the Free Software
8 //        Foundation.  See the file COPYING_LIB for details, or visit
9 //        <http://www.fsf.org/licenses/licenses.html>.
10 //
11 //	The code in this file was written to go with the Fiview app
12 //	(http://uazu.net/fiview/), but it may be used as a library for
13 //	other applications.  The idea behind this library is to allow
14 //	filters to be designed at run-time, which gives much greater
15 //	flexibility to filtering applications.
16 //
17 //	This file depends on the fidmkf.h file which provides the
18 //	filter types from Tony Fisher's 'mkfilter' package.  See that
19 //	file for references and links used there.
20 //
21 //
22 //	Here are some of the sources I used whilst writing this code:
23 //
24 //	Robert Bristow-Johnson's EQ cookbook formulae:
25 //	  http://www.harmony-central.com/Computer/Programming/Audio-EQ-Cookbook.txt
26 //
27 
28 #define FIDLIB_VERSION "0.9.9"
29 
30 //
31 //	Filter specification string
32 //	---------------------------
33 //
34 //	The filter specification string can be used to completely
35 //	specify the filter, or it can be used with the frequency or
36 //	frequency range missing, in which case default values are
37 //	picked up from values passed directly to the routine.
38 //
39 //	The spec consists of a series of letters usually followed by
40 //	the order of the filter and then by any other parameters
41 //	required, preceded by slashes.  For example:
42 //
43 //	  LpBu4/20.4	Lowpass butterworth, 4th order, -3.01dB at 20.4Hz
44 //	  BpBu2/3-4	Bandpass butterworth, 2nd order, from 3 to 4Hz
45 //	  BpBu2/=3-4	Same filter, but adjusted exactly to the range given
46 //	  BsRe/1000/10	Bandstop resonator, Q=1000, frequency 10Hz
47 //
48 //	The routines fid_design() or fid_parse() are used to convert
49 //	this spec-string into filter coefficients and a description
50 //	(if required).
51 //
52 //
53 //	Typical usage:
54 //	-------------
55 //
56 //	FidFilter *filt, *filt2;
57 //	char *desc;
58 //	FidRun *run;
59 //	FidFunc *funcp;
60 //	void *fbuf1, *fbuf2;
61 //	int delay;
62 //	void my_error_func(char *err);
63 //
64 //	// Design a filter, and optionally get its long description
65 //	filt= fid_design(spec, rate, freq0, freq1, adj, &desc);
66 //
67 //	// List all the possible filter types
68 //	fid_list_filters(stdout);
69 //	okay= fid_list_filters_buf(buf, buf+sizeof(buf));
70 //
71 //	// Calculate the response of the filter at a given frequency
72 //	// (frequency is given as a proportion of the sampling rate, in
73 //	// the range 0 to 0.5).  If phase is returned, then this is
74 //	// given in the range 0 to 1 (for 0 to 2*pi).
75 //	resp= fid_response(filt, freq);
76 //	resp= fid_response_pha(filt, freq, &phase);
77 //
78 //	// Estimate the signal delay caused by a particular filter, in samples
79 //	delay= fid_calc_delay(filt);
80 //
81 //	// Run a given filter (this will do JIT filter compilation if this is
82 //	// implemented for this processor / OS)
83 //	run= fid_run_new(filt, &funcp);
84 //	fbuf1= fid_run_newbuf(run);
85 //	fbuf2= fid_run_newbuf(run);
86 //	while (...) {
87 //	   out_1= funcp(fbuf1, in_1);
88 //	   out_2= funcp(fbuf2, in_2);
89 //	   if (restart_required) fid_run_zapbuf(fbuf1);
90 //	   ...
91 //	}
92 //	fid_run_freebuf(fbuf2);
93 //	fid_run_freebuf(fbuf1);
94 //	fid_run_free(run);
95 //
96 //	// If you need to allocate your own buffers separately for some
97 //	// reason, then do it this way:
98 //	run= fid_run_new(filt, &funcp);
99 //	len= fid_run_bufsize(run);
100 //	fbuf1= Alloc(len); fid_run_initbuf(run, fbuf1);
101 //	fbuf2= Alloc(len); fid_run_initbuf(run, fbuf2);
102 //	while (...) {
103 //	   out_1= funcp(fbuf1, in_1);
104 //	   out_2= funcp(fbuf2, in_2);
105 //	   if (restart_required) fid_run_zapbuf(fbuf1);
106 //	   ...
107 //	}
108 //	free(fbuf2);
109 //	free(fbuf1);
110 //	fid_run_free(run);
111 //
112 //	// Convert an arbitrary filter into a new filter which is a single
113 //	// IIR/FIR pair.  This is done by convolving the coefficients.  This
114 //	// flattened filter will give the same result, in theory.  However,
115 //	// in practice this will be less accurate, especially in cases where
116 //	// the limits of the floating point format are being reached (e.g.
117 //	// subtracting numbers with small highly significant differences).
118 //	// The routine also ensures that the IIR first coefficient is 1.0.
119 //	filt2= fid_flatten(filt);
120 //	free(filt);
121 //
122 //	// Parse an entire filter-spec string possibly containing several FIR,
123 //	// IIR and predefined filters and return it as a FidFilter at the given
124 //	// location.  Stops at the first ,; or unmatched )]} character, or the end
125 //	// of the string.  Returns a strdup'd error string on error, or else 0.
126 //	err= fid_parse(double rate, char **pp, FidFilter **ffp);
127 //
128 //	// Set up your own fatal-error handler (default is to dump a message
129 //	// to STDERR and exit on fatal conditions)
130 //	fid_set_error_handler(&my_error_func);
131 //
132 //	// Get the version number of the library as a string (e.g. "1.0.0")
133 //	txt= fid_version();
134 //
135 //	// Design a filter and reduce it to a list of all the non-const
136 //	// coefficients, which is returned in the given double[].  The number
137 //	// of coefficients expected must be provided (as a check).
138 //	#define N_COEF <whatever>
139 //	double coef[N_COEF], gain;
140 //	gain= fid_design_coef(coef, N_COEF, spec, rate, freq0, freq1, adj);
141 //
142 //	// Rewrite a filter spec in a full and/or separated-out form
143 //	char *full, *min;
144 //	double minf0, minf1;
145 //	int minadj;
146 //	fid_rewrite_spec(spec, freq0, freq1, adj, &full, &min, &minf0, &minf1, &minadj);
147 //	...
148 //	free(full); free(min);
149 //
150 //	// Create a FidFilter based on coefficients provided in the
151 //	// given double array.
152 //	static double array[]= { 'I', 3, 1.0, 0.55, 0.77, 'F', 3, 1, -2, 1, 0 };
153 //	filt= fid_cv_array(array);
154 //
155 //	// Join a number of filters into a single filter (and free them too,
156 //	// if the first argument is 1)
157 //	filt= fid_cat(0, filt1, filt2, filt3, filt4, 0);
158 //
159 //
160 
161 //
162 //	Format of returned filter
163 //	-------------------------
164 //
165 //	The filter returned is a single chunk of allocated memory in
166 //	which is stored a number of FidFilter instances.  Each
167 //	instance has variable length according to the coefficients
168 //	contained in it.  It is probably easier to think of this as a
169 //	stream of items in memory.  Each sub-filter starts with its
170 //	type as a short -- either 'I' for IIR filters, or 'F' for FIR
171 //	filters.  (Other types may be added later, e.g. AM modulation
172 //	elements, or whatever).  This is followed by a short bitmap
173 //	which indicates which of the coefficients are constants,
174 //	aiding code-generation.  Next comes the count of the following
175 //	coefficients, as an int.  (These header fields normally takes 8
176 //	bytes, the same as a double, but this might depend on the
177 //	platform).  Then follow the coefficients, as doubles.  The next
178 //	sub-filter follows on straight after that.  The end of the list
179 //	is marked by 8 zero bytes, meaning typ==0, cbm==0 and len==0.
180 //
181 //	The filter can be read with the aid of the FidFilter structure
182 //	(giving typ, cbm, len and val[] elements) and the FFNEXT()
183 //	macro: using ff= FFNEXT(ff) steps to the next FidFilter
184 //	structure along the chain.
185 //
186 //	Note that within the sub-filters, coefficients are listed in
187 //	the order that they apply to data, from current-sample
188 //	backwards in time, i.e. most recent first (so an FIR val[] of
189 //	0, 0, 1 represents a two-sample delay FIR filter).  IIR
190 //	filters are *not* necessarily adjusted so that their first
191 //	coefficient is 1.
192 //
193 //	Most filters have their gain pre-adjusted so that some
194 //	suitable part of the response is at gain==1.0.  However, this
195 //	depends on the filter type.
196 //
197 
198 //
199 //	Check that a target macro has been set.  This macro selects
200 //	various fixes required on various platforms:
201 //
202 //	  T_LINUX  Linux, or probably any UNIX-like platform with GCC
203 //	  T_MINGW  MinGW -- either building on Win32 or cross-compiling
204 //	  T_MSVC   Microsoft Visual C
205 //
206 //	(On MSVC, add "T_MSVC" to the preprocessor definitions in the
207 //	project settings, or add /D "T_MSVC" to the compiler
208 //	command-line.)
209 //
210 
211 #ifndef FIDLIB_LINUX
212 #ifndef FIDLIB_MINGW
213 #ifndef FIDLIB_MSVC
214 #error Please define one of the T_* target macros (e.g. -DT_LINUX); see fidlib.c
215 #endif
216 #endif
217 #endif
218 
219 
220 //
221 //	Select which method of filter execution is preferred.
222 //	RF_CMDLIST is recommended (and is the default).
223 //
224 //	  RF_COMBINED -- easy to understand code, lower accuracy
225 //	  RF_CMDLIST  -- faster pre-compiled code
226 //	  RF_JIT      -- fastest JIT run-time generated code (no longer supported)
227 //
228 
229 #ifndef RF_COMBINED
230 #ifndef RF_CMDLIST
231 #ifndef RF_JIT
232 
233 #define RF_CMDLIST
234 
235 #endif
236 #endif
237 #endif
238 
239 //
240 //	Includes
241 //
242 
243 #include <stdlib.h>
244 #include <stdarg.h>
245 #include <stdio.h>
246 #include <string.h>
247 #include <ctype.h>
248 #include <math.h>
249 #include "fidlib.h"
250 
251 #ifndef M_PI
252 #define M_PI           3.14159265358979323846
253 #endif
254 
255 extern FidFilter *mkfilter(char *, ...);
256 
257 //
258 //	Target-specific fixes
259 //
260 
261 // Macro for local inline routines that shouldn't be visible externally
262 #ifdef FIDLIB_MSVC
263  #define STATIC_INLINE static __inline
264 #else
265  #define STATIC_INLINE static inline
266 #endif
267 
268 // MinGW and MSVC fixes
269 #if defined(FIDLIB_MINGW) || defined(FIDLIB_MSVC)
270  #ifndef vsnprintf
271   #define vsnprintf _vsnprintf
272  #endif
273  #ifndef snprintf
274   #define snprintf _snprintf
275  #endif
276  STATIC_INLINE double
asinh(double val)277  asinh(double val) {
278     return log(val + sqrt(val*val + 1.0));
279  }
280 #endif
281 
282 
283 //
284 //	Support code
285 //
286 
287 static void (*error_handler)(char *err)= 0;
288 
289 static void
error(char * fmt,...)290 error(char *fmt, ...) {
291    char buf[1024];
292    va_list ap;
293    va_start(ap, fmt);
294 
295    vsnprintf(buf, sizeof(buf), fmt, ap);	// Ignore overflow
296    buf[sizeof(buf)-1]= 0;
297    if (error_handler) error_handler(buf);
298 
299    // If error handler routine returns, we dump to STDERR and exit anyway
300    fprintf(stderr, "fidlib error: %s\n", buf);
301    exit(1);
302 }
303 
304 static char *
strdupf(char * fmt,...)305 strdupf(char *fmt, ...) {
306    va_list ap;
307    char buf[1024], *rv;
308    int len;
309    va_start(ap, fmt);
310    len= vsnprintf(buf, sizeof(buf), fmt, ap);
311    if (len < 0 || len >= sizeof(buf)-1)
312       error("strdupf exceeded buffer");
313    rv= strdup(buf);
314    if (!rv) error("Out of memory");
315    return rv;
316 }
317 
318 static void *
Alloc(int size)319 Alloc(int size) {
320    void *vp= calloc(1, size);
321    if (!vp) error("Out of memory");
322    return vp;
323 }
324 
325 #define ALLOC(type) ((type*)Alloc(sizeof(type)))
326 #define ALLOC_ARR(cnt, type) ((type*)Alloc((cnt) * sizeof(type)))
327 
328 
329 //
330 //      Complex multiply: aa *= bb;
331 //
332 
333 STATIC_INLINE void
cmul(double * aa,double * bb)334 cmul(double *aa, double *bb) {
335    double rr= aa[0] * bb[0] - aa[1] * bb[1];
336    double ii= aa[0] * bb[1] + aa[1] * bb[0];
337    aa[0]= rr;
338    aa[1]= ii;
339 }
340 
341 //
342 //      Complex square: aa *= aa;
343 //
344 
345 STATIC_INLINE void
csqu(double * aa)346 csqu(double *aa) {
347    double rr= aa[0] * aa[0] - aa[1] * aa[1];
348    double ii= 2 * aa[0] * aa[1];
349    aa[0]= rr;
350    aa[1]= ii;
351 }
352 
353 //
354 //      Complex multiply by real: aa *= bb;
355 //
356 
357 STATIC_INLINE void
cmulr(double * aa,double fact)358 cmulr(double *aa, double fact) {
359    aa[0] *= fact;
360    aa[1] *= fact;
361 }
362 
363 //
364 //	Complex conjugate: aa= aa*
365 //
366 
367 STATIC_INLINE void
cconj(double * aa)368 cconj(double *aa) {
369    aa[1]= -aa[1];
370 }
371 
372 //
373 //      Complex divide: aa /= bb;
374 //
375 
376 STATIC_INLINE void
cdiv(double * aa,double * bb)377 cdiv(double *aa, double *bb) {
378    double rr= aa[0] * bb[0] + aa[1] * bb[1];
379    double ii= -aa[0] * bb[1] + aa[1] * bb[0];
380    double fact= 1.0 / (bb[0] * bb[0] + bb[1] * bb[1]);
381    aa[0]= rr * fact;
382    aa[1]= ii * fact;
383 }
384 
385 //
386 //	Complex reciprocal: aa= 1/aa
387 //
388 
389 STATIC_INLINE void
crecip(double * aa)390 crecip(double *aa) {
391    double fact= 1.0 / (aa[0] * aa[0] + aa[1] * aa[1]);
392    aa[0] *= fact;
393    aa[1] *= -fact;
394 }
395 
396 //
397 //	Complex assign: aa= bb
398 //
399 
400 STATIC_INLINE void
cass(double * aa,double * bb)401 cass(double *aa, double *bb) {
402    memcpy(aa, bb, 2*sizeof(double));  // Assigning doubles is really slow
403 }
404 
405 //
406 //	Complex assign: aa= (rr + ii*j)
407 //
408 
409 STATIC_INLINE void
cassz(double * aa,double rr,double ii)410 cassz(double *aa, double rr, double ii) {
411    aa[0]= rr;
412    aa[1]= ii;
413 }
414 
415 //
416 //	Complex add: aa += bb
417 //
418 
419 STATIC_INLINE void
cadd(double * aa,double * bb)420 cadd(double *aa, double *bb) {
421    aa[0] += bb[0];
422    aa[1] += bb[1];
423 }
424 
425 //
426 //	Complex add: aa += (rr + ii*j)
427 //
428 
429 STATIC_INLINE void
caddz(double * aa,double rr,double ii)430 caddz(double *aa, double rr, double ii) {
431    aa[0] += rr;
432    aa[1] += ii;
433 }
434 
435 //
436 //	Complex subtract: aa -= bb
437 //
438 
439 STATIC_INLINE void
csub(double * aa,double * bb)440 csub(double *aa, double *bb) {
441    aa[0] -= bb[0];
442    aa[1] -= bb[1];
443 }
444 
445 //
446 //	Complex subtract: aa -= (rr + ii*j)
447 //
448 
449 STATIC_INLINE void
csubz(double * aa,double rr,double ii)450 csubz(double *aa, double rr, double ii) {
451    aa[0] -= rr;
452    aa[1] -= ii;
453 }
454 
455 //
456 //	Complex negate: aa= -aa
457 //
458 
459 STATIC_INLINE void
cneg(double * aa)460 cneg(double *aa) {
461    aa[0]= -aa[0];
462    aa[1]= -aa[1];
463 }
464 
465 //
466 //      Evaluate a complex polynomial given the coefficients.
467 //      rv[0]+i*rv[1] is the result, in[0]+i*in[1] is the input value.
468 //      Coefficients are real values.
469 //
470 
471 STATIC_INLINE void
evaluate(double * rv,double * coef,int n_coef,double * in)472 evaluate(double *rv, double *coef, int n_coef, double *in) {
473    double pz[2];        // Powers of Z
474 
475    // Handle first iteration by hand
476    rv[0]= *coef++;
477    rv[1]= 0;
478 
479    if (--n_coef > 0) {
480       // Handle second iteration by hand
481       pz[0]= in[0];
482       pz[1]= in[1];
483       rv[0] += *coef * pz[0];
484       rv[1] += *coef * pz[1];
485       coef++; n_coef--;
486 
487       // Loop for remainder
488       while (n_coef > 0) {
489          cmul(pz, in);
490          rv[0] += *coef * pz[0];
491          rv[1] += *coef * pz[1];
492          coef++;
493          n_coef--;
494       }
495    }
496 }
497 
498 
499 //
500 //	Housekeeping
501 //
502 
503 void
fid_set_error_handler(void (* rout)(char *))504 fid_set_error_handler(void (*rout)(char*)) {
505    error_handler= rout;
506 }
507 
508 char *
fid_version()509 fid_version() {
510    return FIDLIB_VERSION;
511 }
512 
513 
514 //
515 //	Get the response and phase of a filter at the given frequency
516 //	(expressed as a proportion of the sampling rate, 0->0.5).
517 //	Phase is returned as a number from 0 to 1, representing a
518 //	phase between 0 and two-pi.
519 //
520 
521 double
fid_response_pha(FidFilter * filt,double freq,double * phase)522 fid_response_pha(FidFilter *filt, double freq, double *phase) {
523    double top[2], bot[2];
524    double theta= freq * 2 * M_PI;
525    double zz[2];
526 
527    top[0]= 1;
528    top[1]= 0;
529    bot[0]= 1;
530    bot[1]= 0;
531    zz[0]= cos(theta);
532    zz[1]= sin(theta);
533 
534    while (filt->len) {
535       double resp[2];
536       int cnt= filt->len;
537       evaluate(resp, filt->val, cnt, zz);
538       if (filt->typ == 'I')
539 	 cmul(bot, resp);
540       else if (filt->typ == 'F')
541 	 cmul(top, resp);
542       else
543 	 error("Unknown filter type %d in fid_response_pha()", filt->typ);
544       filt= FFNEXT(filt);
545    }
546 
547    cdiv(top, bot);
548 
549    if (phase) {
550       double pha= atan2(top[1], top[0]) / (2 * M_PI);
551       if (pha < 0) pha += 1.0;
552       *phase= pha;
553    }
554 
555    return hypot(top[1], top[0]);
556 }
557 
558 //
559 //	Get the response of a filter at the given frequency (expressed
560 //	as a proportion of the sampling rate, 0->0.5).
561 //
562 //	Code duplicate, as I didn't want the overhead of a function
563 //	call to fid_response_pha.  Almost every call in this routine
564 //	can be inlined.
565 //
566 
567 double
fid_response(FidFilter * filt,double freq)568 fid_response(FidFilter *filt, double freq) {
569    double top[2], bot[2];
570    double theta= freq * 2 * M_PI;
571    double zz[2];
572 
573    top[0]= 1;
574    top[1]= 0;
575    bot[0]= 1;
576    bot[1]= 0;
577    zz[0]= cos(theta);
578    zz[1]= sin(theta);
579 
580    while (filt->len) {
581       double resp[2];
582       int cnt= filt->len;
583       evaluate(resp, filt->val, cnt, zz);
584       if (filt->typ == 'I')
585 	 cmul(bot, resp);
586       else if (filt->typ == 'F')
587 	 cmul(top, resp);
588       else
589 	 error("Unknown filter type %d in fid_response()", filt->typ);
590       filt= FFNEXT(filt);
591    }
592 
593    cdiv(top, bot);
594 
595    return hypot(top[1], top[0]);
596 }
597 
598 
599 //
600 //	Estimate the delay that a filter causes to the signal by
601 //	looking for the point at which 50% of the filter calculations
602 //	are complete.  This involves running test impulses through the
603 //	filter several times.  The estimated delay in samples is
604 //	returned.
605 //
606 //	Delays longer than 8,000,000 samples are not handled well, as
607 //	the code drops out at this point rather than get stuck in an
608 //	endless loop.
609 //
610 #ifdef MOO
611 int
fid_calc_delay(FidFilter * filt)612 fid_calc_delay(FidFilter *filt) {
613    FidRun *run;
614    FidFunc *dostep;
615    void *f1, *f2;
616    double tot, tot100, tot50;
617    int cnt;
618 
619    run= fid_run_new(filt, &dostep);
620 
621    // Run through to find at least the 99.9% point of filter; the r2
622    // (tot100) filter runs at 4x the speed of the other one to act as
623    // a reference point much further ahead in the impulse response.
624    f1= fid_run_newbuf(run);
625    f2= fid_run_newbuf(run);
626 
627    tot= fabs(dostep(f1, 1.0));
628    tot100= fabs(dostep(f2, 1.0));
629    tot100 += fabs(dostep(f2, 0.0));
630    tot100 += fabs(dostep(f2, 0.0));
631    tot100 += fabs(dostep(f2, 0.0));
632 
633    for (cnt= 1; cnt < 0x1000000; cnt++) {
634       tot += fabs(dostep(f1, 0.0));
635       tot100 += fabs(dostep(f2, 0.0));
636       tot100 += fabs(dostep(f2, 0.0));
637       tot100 += fabs(dostep(f2, 0.0));
638       tot100 += fabs(dostep(f2, 0.0));
639 
640       if (tot/tot100 >= 0.999) break;
641    }
642    fid_run_freebuf(f1);
643    fid_run_freebuf(f2);
644 
645    // Now find the 50% point
646    tot50= tot100/2;
647    f1= fid_run_newbuf(run);
648    tot= fabs(dostep(f1, 1.0));
649    for (cnt= 0; tot < tot50; cnt++)
650       tot += fabs(dostep(f1, 0.0));
651    fid_run_freebuf(f1);
652 
653    // Clean up, return
654    fid_run_free(run);
655    return cnt;
656 }
657 #endif
658 
659 //
660 //	'mkfilter'-derived code
661 //
662 
663 #include "fidmkf.h"
664 
665 
666 //
667 //	Stack a number of identical filters, generating the required
668 //	FidFilter* return value
669 //
670 
671 static FidFilter*
stack_filter(int order,int n_head,int n_val,...)672 stack_filter(int order, int n_head, int n_val, ...) {
673    FidFilter *rv= FFALLOC(n_head * order, n_val * order);
674    FidFilter *p, *q;
675    va_list ap;
676    int a, b, len;
677 
678    if (order == 0) return rv;
679 
680    // Copy from ap
681    va_start(ap, n_val);
682    p= q= rv;
683    for (a= 0; a<n_head; a++) {
684       p->typ= va_arg(ap, int);
685       p->cbm= va_arg(ap, int);
686       p->len= va_arg(ap, int);
687       for (b= 0; b<p->len; b++)
688 	 p->val[b]= va_arg(ap, double);
689       p= FFNEXT(p);
690    }
691    order--;
692 
693    // Check length
694    len= ((char*)p)-((char*)q);
695    if (len != FFCSIZE(n_head-1, n_val))
696       error("Internal error; bad call to stack_filter(); length mismatch (%d,%d)",
697 	    len, FFCSIZE(n_head-1, n_val));
698 
699    // Make as many additional copies as necessary
700    while (order-- > 0) {
701       memcpy(p, q, len);
702       p= (void*)(len + (char*)p);
703    }
704 
705    // List is already terminated due to zeroed allocation
706    return rv;
707 }
708 
709 //
710 //	Search for a peak between two given frequencies.  It is
711 //	assumed that the gradient goes upwards from 'f0' to the peak,
712 //	and then down again to 'f3'.  If there are any other curves,
713 //	this routine will get confused and will come up with some
714 //	frequency, although probably not the right one.
715 //
716 //	Returns the frequency of the peak.
717 //
718 
719 static double
search_peak(FidFilter * ff,double f0,double f3)720 search_peak(FidFilter *ff, double f0, double f3) {
721    double f1, f2;
722    double r1, r2;
723    int a;
724 
725    // Binary search, modified, taking two intermediate points.  Do 20
726    // subdivisions, which should give 1/2^20 == 1e-6 accuracy compared
727    // to original range.
728    for (a= 0; a<20; a++) {
729       f1= 0.51 * f0 + 0.49 * f3;
730       f2= 0.49 * f0 + 0.51 * f3;
731       if (f1 == f2) break;		// We're hitting FP limit
732       r1= fid_response(ff, f1);
733       r2= fid_response(ff, f2);
734       if (r1 > r2)	// Peak is either to the left, or between f1/f2
735 	 f3= f2;
736       else	 	// Peak is either to the right, or between f1/f2
737 	 f0= f1;
738    }
739    return (f0+f3)*0.5;
740 }
741 
742 //
743 //	Handle the different 'back-ends' for Bessel, Butterworth and
744 //	Chebyshev filters.  First argument selects between bilinear
745 //	(0) and matched-Z (non-0).  The BL and MZ macros makes this a
746 //	bit more obvious in the code.
747 //
748 //	Overall filter gain is adjusted to give the peak at 1.0.  This
749 //	is easy for all types except for band-pass, where a search is
750 //	required to find the precise peak.  This is much slower than
751 //	the other types.
752 //
753 
754 #define BL 0
755 #define MZ 1
756 
757 static FidFilter*
do_lowpass(int mz,double freq)758 do_lowpass(int mz, double freq) {
759    FidFilter *rv;
760    lowpass(prewarp(freq));
761    if (mz) s2z_matchedZ(); else s2z_bilinear();
762    rv= z2fidfilter(1.0, ~0);	// FIR is constant
763    rv->val[0]= 1.0 / fid_response(rv, 0.0);
764    return rv;
765 }
766 
767 static FidFilter*
do_highpass(int mz,double freq)768 do_highpass(int mz, double freq) {
769    FidFilter *rv;
770    highpass(prewarp(freq));
771    if (mz) s2z_matchedZ(); else s2z_bilinear();
772    rv= z2fidfilter(1.0, ~0);	// FIR is constant
773    rv->val[0]= 1.0 / fid_response(rv, 0.5);
774    return rv;
775 }
776 
777 static FidFilter*
do_bandpass(int mz,double f0,double f1)778 do_bandpass(int mz, double f0, double f1) {
779    FidFilter *rv;
780    bandpass(prewarp(f0), prewarp(f1));
781    if (mz) s2z_matchedZ(); else s2z_bilinear();
782    rv= z2fidfilter(1.0, ~0);	// FIR is constant
783    rv->val[0]= 1.0 / fid_response(rv, search_peak(rv, f0, f1));
784    return rv;
785 }
786 
787 static FidFilter*
do_bandstop(int mz,double f0,double f1)788 do_bandstop(int mz, double f0, double f1) {
789    FidFilter *rv;
790    bandstop(prewarp(f0), prewarp(f1));
791    if (mz) s2z_matchedZ(); else s2z_bilinear();
792    rv= z2fidfilter(1.0, 5);	// FIR second coefficient is *non-const* for bandstop
793    rv->val[0]= 1.0 / fid_response(rv, 0.0);	// Use 0Hz response as reference
794    return rv;
795 }
796 
797 
798 //
799 //	Information passed to individual filter design routines:
800 //
801 //	  double* rout(double rate, double f0, double f1,
802 //		       int order, int n_arg, double *arg);
803 //
804 //	'rate' is the sampling rate, or 1 if not set
805 //	'f0' and 'f1' give the frequency or frequency range as a
806 //	 	proportion of the sampling rate
807 //	'order' is the order of the filter (the integer passed immediately
808 //		after the name)
809 //	'n_arg' is the number of additional arguments for the filter
810 //	'arg' gives the additional argument values: arg[n]
811 //
812 //	Note that #O #o #F and #R are mapped to the f0/f1/order
813 //	arguments, and are not included in the arg[] array.
814 //
815 //	See the previous description for the required meaning of the
816 //	return value FidFilter list.
817 //
818 
819 //
820 //	Filter design routines and supporting code
821 //
822 
823 static FidFilter*
des_bpre(double rate,double f0,double f1,int order,int n_arg,double * arg)824 des_bpre(double rate, double f0, double f1, int order, int n_arg, double *arg) {
825    bandpass_res(f0, arg[0]);
826    return z2fidfilter(1.0, ~0);	// FIR constant
827 }
828 
829 static FidFilter*
des_bsre(double rate,double f0,double f1,int order,int n_arg,double * arg)830 des_bsre(double rate, double f0, double f1, int order, int n_arg, double *arg) {
831    bandstop_res(f0, arg[0]);
832    return z2fidfilter(1.0, 0);	// FIR not constant, depends on freq
833 }
834 
835 static FidFilter*
des_apre(double rate,double f0,double f1,int order,int n_arg,double * arg)836 des_apre(double rate, double f0, double f1, int order, int n_arg, double *arg) {
837    allpass_res(f0, arg[0]);
838    return z2fidfilter(1.0, 0);	// FIR not constant, depends on freq
839 }
840 
841 static FidFilter*
des_pi(double rate,double f0,double f1,int order,int n_arg,double * arg)842 des_pi(double rate, double f0, double f1, int order, int n_arg, double *arg) {
843    prop_integral(prewarp(f0));
844    s2z_bilinear();
845    return z2fidfilter(1.0, 0);	// FIR not constant, depends on freq
846 }
847 
848 static FidFilter*
des_piz(double rate,double f0,double f1,int order,int n_arg,double * arg)849 des_piz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
850    prop_integral(prewarp(f0));
851    s2z_matchedZ();
852    return z2fidfilter(1.0, 0);	// FIR not constant, depends on freq
853 }
854 
855 static FidFilter*
des_lpbe(double rate,double f0,double f1,int order,int n_arg,double * arg)856 des_lpbe(double rate, double f0, double f1, int order, int n_arg, double *arg) {
857    bessel(order);
858    return do_lowpass(BL, f0);
859 }
860 
861 static FidFilter*
des_hpbe(double rate,double f0,double f1,int order,int n_arg,double * arg)862 des_hpbe(double rate, double f0, double f1, int order, int n_arg, double *arg) {
863    bessel(order);
864    return do_highpass(BL, f0);
865 }
866 
867 static FidFilter*
des_bpbe(double rate,double f0,double f1,int order,int n_arg,double * arg)868 des_bpbe(double rate, double f0, double f1, int order, int n_arg, double *arg) {
869    bessel(order);
870    return do_bandpass(BL, f0, f1);
871 }
872 
873 static FidFilter*
des_bsbe(double rate,double f0,double f1,int order,int n_arg,double * arg)874 des_bsbe(double rate, double f0, double f1, int order, int n_arg, double *arg) {
875    bessel(order);
876    return do_bandstop(BL, f0, f1);
877 }
878 
879 static FidFilter*
des_lpbez(double rate,double f0,double f1,int order,int n_arg,double * arg)880 des_lpbez(double rate, double f0, double f1, int order, int n_arg, double *arg) {
881    bessel(order);
882    return do_lowpass(MZ, f0);
883 }
884 
885 static FidFilter*
des_hpbez(double rate,double f0,double f1,int order,int n_arg,double * arg)886 des_hpbez(double rate, double f0, double f1, int order, int n_arg, double *arg) {
887    bessel(order);
888    return do_highpass(MZ, f0);
889 }
890 
891 static FidFilter*
des_bpbez(double rate,double f0,double f1,int order,int n_arg,double * arg)892 des_bpbez(double rate, double f0, double f1, int order, int n_arg, double *arg) {
893    bessel(order);
894    return do_bandpass(MZ, f0, f1);
895 }
896 
897 static FidFilter*
des_bsbez(double rate,double f0,double f1,int order,int n_arg,double * arg)898 des_bsbez(double rate, double f0, double f1, int order, int n_arg, double *arg) {
899    bessel(order);
900    return do_bandstop(MZ, f0, f1);
901 }
902 
903 static FidFilter*	// Butterworth-Bessel cross
des_lpbube(double rate,double f0,double f1,int order,int n_arg,double * arg)904 des_lpbube(double rate, double f0, double f1, int order, int n_arg, double *arg) {
905    double tmp[MAXPZ];
906    int a;
907    bessel(order); memcpy(tmp, pol, order * sizeof(double));
908    butterworth(order);
909    for (a= 0; a<order; a++) pol[a] += (tmp[a]-pol[a]) * 0.01 * arg[0];
910    //for (a= 1; a<order; a+=2) pol[a] += arg[1] * 0.01;
911    return do_lowpass(BL, f0);
912 }
913 
914 static FidFilter*
des_lpbu(double rate,double f0,double f1,int order,int n_arg,double * arg)915 des_lpbu(double rate, double f0, double f1, int order, int n_arg, double *arg) {
916    butterworth(order);
917    return do_lowpass(BL, f0);
918 }
919 
920 static FidFilter*
des_hpbu(double rate,double f0,double f1,int order,int n_arg,double * arg)921 des_hpbu(double rate, double f0, double f1, int order, int n_arg, double *arg) {
922    butterworth(order);
923    return do_highpass(BL, f0);
924 }
925 
926 static FidFilter*
des_bpbu(double rate,double f0,double f1,int order,int n_arg,double * arg)927 des_bpbu(double rate, double f0, double f1, int order, int n_arg, double *arg) {
928    butterworth(order);
929    return do_bandpass(BL, f0, f1);
930 }
931 
932 static FidFilter*
des_bsbu(double rate,double f0,double f1,int order,int n_arg,double * arg)933 des_bsbu(double rate, double f0, double f1, int order, int n_arg, double *arg) {
934    butterworth(order);
935    return do_bandstop(BL, f0, f1);
936 }
937 
938 static FidFilter*
des_lpbuz(double rate,double f0,double f1,int order,int n_arg,double * arg)939 des_lpbuz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
940    butterworth(order);
941    return do_lowpass(MZ, f0);
942 }
943 
944 static FidFilter*
des_hpbuz(double rate,double f0,double f1,int order,int n_arg,double * arg)945 des_hpbuz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
946    butterworth(order);
947    return do_highpass(MZ, f0);
948 }
949 
950 static FidFilter*
des_bpbuz(double rate,double f0,double f1,int order,int n_arg,double * arg)951 des_bpbuz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
952    butterworth(order);
953    return do_bandpass(MZ, f0, f1);
954 }
955 
956 static FidFilter*
des_bsbuz(double rate,double f0,double f1,int order,int n_arg,double * arg)957 des_bsbuz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
958    butterworth(order);
959    return do_bandstop(MZ, f0, f1);
960 }
961 
962 static FidFilter*
des_lpch(double rate,double f0,double f1,int order,int n_arg,double * arg)963 des_lpch(double rate, double f0, double f1, int order, int n_arg, double *arg) {
964    chebyshev(order, arg[0]);
965    return do_lowpass(BL, f0);
966 }
967 
968 static FidFilter*
des_hpch(double rate,double f0,double f1,int order,int n_arg,double * arg)969 des_hpch(double rate, double f0, double f1, int order, int n_arg, double *arg) {
970    chebyshev(order, arg[0]);
971    return do_highpass(BL, f0);
972 }
973 
974 static FidFilter*
des_bpch(double rate,double f0,double f1,int order,int n_arg,double * arg)975 des_bpch(double rate, double f0, double f1, int order, int n_arg, double *arg) {
976    chebyshev(order, arg[0]);
977    return do_bandpass(BL, f0, f1);
978 }
979 
980 static FidFilter*
des_bsch(double rate,double f0,double f1,int order,int n_arg,double * arg)981 des_bsch(double rate, double f0, double f1, int order, int n_arg, double *arg) {
982    chebyshev(order, arg[0]);
983    return do_bandstop(BL, f0, f1);
984 }
985 
986 static FidFilter*
des_lpchz(double rate,double f0,double f1,int order,int n_arg,double * arg)987 des_lpchz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
988    chebyshev(order, arg[0]);
989    return do_lowpass(MZ, f0);
990 }
991 
992 static FidFilter*
des_hpchz(double rate,double f0,double f1,int order,int n_arg,double * arg)993 des_hpchz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
994    chebyshev(order, arg[0]);
995    return do_highpass(MZ, f0);
996 }
997 
998 static FidFilter*
des_bpchz(double rate,double f0,double f1,int order,int n_arg,double * arg)999 des_bpchz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1000    chebyshev(order, arg[0]);
1001    return do_bandpass(MZ, f0, f1);
1002 }
1003 
1004 static FidFilter*
des_bschz(double rate,double f0,double f1,int order,int n_arg,double * arg)1005 des_bschz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1006    chebyshev(order, arg[0]);
1007    return do_bandstop(MZ, f0, f1);
1008 }
1009 
1010 static FidFilter*
des_lpbq(double rate,double f0,double f1,int order,int n_arg,double * arg)1011 des_lpbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1012    double omega= 2 * M_PI * f0;
1013    double cosv= cos(omega);
1014    double alpha= sin(omega) / 2 / arg[0];
1015    return stack_filter(order, 3, 7,
1016 		       'I', 0x0, 3, 1 + alpha, -2 * cosv, 1 - alpha,
1017 		       'F', 0x7, 3, 1.0, 2.0, 1.0,
1018 		       'F', 0x0, 1, (1-cosv) * 0.5);
1019 }
1020 
1021 static FidFilter*
des_hpbq(double rate,double f0,double f1,int order,int n_arg,double * arg)1022 des_hpbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1023    double omega= 2 * M_PI * f0;
1024    double cosv= cos(omega);
1025    double alpha= sin(omega) / 2 / arg[0];
1026    return stack_filter(order, 3, 7,
1027 		       'I', 0x0, 3, 1 + alpha, -2 * cosv, 1 - alpha,
1028 		       'F', 0x7, 3, 1.0, -2.0, 1.0,
1029 		       'F', 0x0, 1, (1+cosv) * 0.5);
1030 }
1031 
1032 static FidFilter*
des_bpbq(double rate,double f0,double f1,int order,int n_arg,double * arg)1033 des_bpbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1034    double omega= 2 * M_PI * f0;
1035    double cosv= cos(omega);
1036    double alpha= sin(omega) / 2 / arg[0];
1037    return stack_filter(order, 3, 7,
1038 		       'I', 0x0, 3, 1 + alpha, -2 * cosv, 1 - alpha,
1039 		       'F', 0x7, 3, 1.0, 0.0, -1.0,
1040 		       'F', 0x0, 1, alpha);
1041 }
1042 
1043 static FidFilter*
des_bsbq(double rate,double f0,double f1,int order,int n_arg,double * arg)1044 des_bsbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1045    double omega= 2 * M_PI * f0;
1046    double cosv= cos(omega);
1047    double alpha= sin(omega) / 2 / arg[0];
1048    return stack_filter(order, 2, 6,
1049 		       'I', 0x0, 3, 1 + alpha, -2 * cosv, 1 - alpha,
1050 		       'F', 0x5, 3, 1.0, -2 * cosv, 1.0);
1051 }
1052 
1053 static FidFilter*
des_apbq(double rate,double f0,double f1,int order,int n_arg,double * arg)1054 des_apbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1055    double omega= 2 * M_PI * f0;
1056    double cosv= cos(omega);
1057    double alpha= sin(omega) / 2 / arg[0];
1058    return stack_filter(order, 2, 6,
1059 		       'I', 0x0, 3, 1 + alpha, -2 * cosv, 1 - alpha,
1060 		       'F', 0x0, 3, 1 - alpha, -2 * cosv, 1 + alpha);
1061 }
1062 
1063 static FidFilter*
des_pkbq(double rate,double f0,double f1,int order,int n_arg,double * arg)1064 des_pkbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1065    double omega= 2 * M_PI * f0;
1066    double cosv= cos(omega);
1067    double alpha= sin(omega) / 2 / arg[0];
1068    double A= pow(10, arg[1]/40);
1069    return stack_filter(order, 2, 6,
1070 		       'I', 0x0, 3, 1 + alpha/A, -2 * cosv, 1 - alpha/A,
1071 		       'F', 0x0, 3, 1 + alpha*A, -2 * cosv, 1 - alpha*A);
1072 }
1073 
1074 static FidFilter*
des_lsbq(double rate,double f0,double f1,int order,int n_arg,double * arg)1075 des_lsbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1076    double omega= 2 * M_PI * f0;
1077    double cosv= cos(omega);
1078    double sinv= sin(omega);
1079    double A= pow(10, arg[1]/40);
1080    double beta= sqrt((A*A+1)/arg[0] - (A-1)*(A-1));
1081    return stack_filter(order, 2, 6,
1082 		       'I', 0x0, 3,
1083 		       (A+1) + (A-1)*cosv + beta*sinv,
1084 		       -2 * ((A-1) + (A+1)*cosv),
1085 		       (A+1) + (A-1)*cosv - beta*sinv,
1086 		       'F', 0x0, 3,
1087 		       A*((A+1) - (A-1)*cosv + beta*sinv),
1088 		       2*A*((A-1) - (A+1)*cosv),
1089 		       A*((A+1) - (A-1)*cosv - beta*sinv));
1090 }
1091 
1092 static FidFilter*
des_hsbq(double rate,double f0,double f1,int order,int n_arg,double * arg)1093 des_hsbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1094    double omega= 2 * M_PI * f0;
1095    double cosv= cos(omega);
1096    double sinv= sin(omega);
1097    double A= pow(10, arg[1]/40);
1098    double beta= sqrt((A*A+1)/arg[0] - (A-1)*(A-1));
1099    return stack_filter(order, 2, 6,
1100 		       'I', 0x0, 3,
1101 		       (A+1) - (A-1)*cosv + beta*sinv,
1102 		       2 * ((A-1) - (A+1)*cosv),
1103 		       (A+1) - (A-1)*cosv - beta*sinv,
1104 		       'F', 0x0, 3,
1105 		       A*((A+1) + (A-1)*cosv + beta*sinv),
1106 		       -2*A*((A-1) + (A+1)*cosv),
1107 		       A*((A+1) + (A-1)*cosv - beta*sinv));
1108 }
1109 
1110 static FidFilter*
des_lpbl(double rate,double f0,double f1,int order,int n_arg,double * arg)1111 des_lpbl(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1112    double wid= 0.4109205/f0;
1113    double tot, adj;
1114    int max= (int)floor(wid);
1115    int a;
1116    FidFilter *ff= Alloc(FFCSIZE(1, max*2+1));
1117    ff->typ= 'F';
1118    ff->cbm= 0;
1119    ff->len= max*2+1;
1120    ff->val[max]= tot= 1.0;
1121    for (a= 1; a<=max; a++) {
1122       double val= 0.42 +
1123 	 0.5 * cos(M_PI * a / wid) +
1124 	 0.08 * cos(M_PI * 2.0 * a / wid);
1125       ff->val[max-a]= val;
1126       ff->val[max+a]= val;
1127       tot += val * 2.0;
1128    }
1129    adj= 1/tot;
1130    for (a= 0; a<=max*2; a++) ff->val[a] *= adj;
1131    return ff;
1132 }
1133 
1134 static FidFilter*
des_lphm(double rate,double f0,double f1,int order,int n_arg,double * arg)1135 des_lphm(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1136    double wid= 0.3262096/f0;
1137    double tot, adj;
1138    int max= (int)floor(wid);
1139    int a;
1140    FidFilter *ff= Alloc(FFCSIZE(1, max*2+1));
1141    ff->typ= 'F';
1142    ff->cbm= 0;
1143    ff->len= max*2+1;
1144    ff->val[max]= tot= 1.0;
1145    for (a= 1; a<=max; a++) {
1146       double val= 0.54 +
1147 	 0.46 * cos(M_PI * a / wid);
1148       ff->val[max-a]= val;
1149       ff->val[max+a]= val;
1150       tot += val * 2.0;
1151    }
1152    adj= 1/tot;
1153    for (a= 0; a<=max*2; a++) ff->val[a] *= adj;
1154    return ff;
1155 }
1156 
1157 static FidFilter*
des_lphn(double rate,double f0,double f1,int order,int n_arg,double * arg)1158 des_lphn(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1159    double wid= 0.360144/f0;
1160    double tot, adj;
1161    int max= (int)floor(wid);
1162    int a;
1163    FidFilter *ff= Alloc(FFCSIZE(1, max*2+1));
1164    ff->typ= 'F';
1165    ff->cbm= 0;
1166    ff->len= max*2+1;
1167    ff->val[max]= tot= 1.0;
1168    for (a= 1; a<=max; a++) {
1169       double val= 0.5 +
1170 	 0.5 * cos(M_PI * a / wid);
1171       ff->val[max-a]= val;
1172       ff->val[max+a]= val;
1173       tot += val * 2.0;
1174    }
1175    adj= 1/tot;
1176    for (a= 0; a<=max*2; a++) ff->val[a] *= adj;
1177    return ff;
1178 }
1179 
1180 static FidFilter*
des_lpba(double rate,double f0,double f1,int order,int n_arg,double * arg)1181 des_lpba(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1182    double wid= 0.3189435/f0;
1183    double tot, adj;
1184    int max= (int)floor(wid);
1185    int a;
1186    FidFilter *ff= Alloc(FFCSIZE(1, max*2+1));
1187    ff->typ= 'F';
1188    ff->cbm= 0;
1189    ff->len= max*2+1;
1190    ff->val[max]= tot= 1.0;
1191    for (a= 1; a<=max; a++) {
1192       double val= 1.0 - a/wid;
1193       ff->val[max-a]= val;
1194       ff->val[max+a]= val;
1195       tot += val * 2.0;
1196    }
1197    adj= 1/tot;
1198    for (a= 0; a<=max*2; a++) ff->val[a] *= adj;
1199    return ff;
1200 }
1201 
1202 
1203 //
1204 //	Filter table
1205 //
1206 
1207 static struct {
1208    FidFilter *(*rout)(double,double,double,int,int,double*); // Designer routine address
1209    char *fmt;	// Format for spec-string
1210    char *txt;	// Human-readable description of filter
1211 } filter[]= {
1212    { des_bpre, "BpRe/#V/#F",
1213      "Bandpass resonator, Q=#V (0 means Inf), frequency #F" },
1214    { des_bsre, "BsRe/#V/#F",
1215      "Bandstop resonator, Q=#V (0 means Inf), frequency #F" },
1216    { des_apre, "ApRe/#V/#F",
1217      "Allpass resonator, Q=#V (0 means Inf), frequency #F" },
1218    { des_pi, "Pi/#F",
1219      "Proportional-integral filter, frequency #F" },
1220    { des_piz, "PiZ/#F",
1221      "Proportional-integral filter, matched z-transform, frequency #F" },
1222    { des_lpbe, "LpBe#O/#F",
1223      "Lowpass Bessel filter, order #O, -3.01dB frequency #F" },
1224    { des_hpbe, "HpBe#O/#F",
1225      "Highpass Bessel filter, order #O, -3.01dB frequency #F" },
1226    { des_bpbe, "BpBe#O/#R",
1227      "Bandpass Bessel filter, order #O, -3.01dB frequencies #R" },
1228    { des_bsbe, "BsBe#O/#R",
1229      "Bandstop Bessel filter, order #O, -3.01dB frequencies #R" },
1230    { des_lpbu, "LpBu#O/#F",
1231      "Lowpass Butterworth filter, order #O, -3.01dB frequency #F" },
1232    { des_hpbu, "HpBu#O/#F",
1233      "Highpass Butterworth filter, order #O, -3.01dB frequency #F" },
1234    { des_bpbu, "BpBu#O/#R",
1235      "Bandpass Butterworth filter, order #O, -3.01dB frequencies #R" },
1236    { des_bsbu, "BsBu#O/#R",
1237      "Bandstop Butterworth filter, order #O, -3.01dB frequencies #R" },
1238    { des_lpch, "LpCh#O/#V/#F",
1239      "Lowpass Chebyshev filter, order #O, passband ripple #VdB, -3.01dB frequency #F" },
1240    { des_hpch, "HpCh#O/#V/#F",
1241      "Highpass Chebyshev filter, order #O, passband ripple #VdB, -3.01dB frequency #F" },
1242    { des_bpch, "BpCh#O/#V/#R",
1243      "Bandpass Chebyshev filter, order #O, passband ripple #VdB, -3.01dB frequencies #R" },
1244    { des_bsch, "BsCh#O/#V/#R",
1245      "Bandstop Chebyshev filter, order #O, passband ripple #VdB, -3.01dB frequencies #R" },
1246    { des_lpbez, "LpBeZ#O/#F",
1247      "Lowpass Bessel filter, matched z-transform, order #O, -3.01dB frequency #F" },
1248    { des_hpbez, "HpBeZ#O/#F",
1249      "Highpass Bessel filter, matched z-transform, order #O, -3.01dB frequency #F" },
1250    { des_bpbez, "BpBeZ#O/#R",
1251      "Bandpass Bessel filter, matched z-transform, order #O, -3.01dB frequencies #R" },
1252    { des_bsbez, "BsBeZ#O/#R",
1253      "Bandstop Bessel filter, matched z-transform, order #O, -3.01dB frequencies #R" },
1254    { des_lpbuz, "LpBuZ#O/#F",
1255      "Lowpass Butterworth filter, matched z-transform, order #O, -3.01dB frequency #F" },
1256    { des_hpbuz, "HpBuZ#O/#F",
1257      "Highpass Butterworth filter, matched z-transform, order #O, -3.01dB frequency #F" },
1258    { des_bpbuz, "BpBuZ#O/#R",
1259      "Bandpass Butterworth filter, matched z-transform, order #O, -3.01dB frequencies #R" },
1260    { des_bsbuz, "BsBuZ#O/#R",
1261      "Bandstop Butterworth filter, matched z-transform, order #O, -3.01dB frequencies #R" },
1262    { des_lpchz, "LpChZ#O/#V/#F",
1263      "Lowpass Chebyshev filter, matched z-transform, order #O, "
1264      "passband ripple #VdB, -3.01dB frequency #F" },
1265    { des_hpchz, "HpChZ#O/#V/#F",
1266      "Highpass Chebyshev filter, matched z-transform, order #O, "
1267      "passband ripple #VdB, -3.01dB frequency #F" },
1268    { des_bpchz, "BpChZ#O/#V/#R",
1269      "Bandpass Chebyshev filter, matched z-transform, order #O, "
1270      "passband ripple #VdB, -3.01dB frequencies #R" },
1271    { des_bschz, "BsChZ#O/#V/#R",
1272      "Bandstop Chebyshev filter, matched z-transform, order #O, "
1273      "passband ripple #VdB, -3.01dB frequencies #R" },
1274    { des_lpbube, "LpBuBe#O/#V/#F",
1275      "Lowpass Butterworth-Bessel #V% cross, order #O, -3.01dB frequency #F" },
1276    { des_lpbq, "LpBq#o/#V/#F",
1277      "Lowpass biquad filter, order #O, Q=#V, -3.01dB frequency #F" },
1278    { des_hpbq, "HpBq#o/#V/#F",
1279      "Highpass biquad filter, order #O, Q=#V, -3.01dB frequency #F" },
1280    { des_bpbq, "BpBq#o/#V/#F",
1281      "Bandpass biquad filter, order #O, Q=#V, centre frequency #F" },
1282    { des_bsbq, "BsBq#o/#V/#F",
1283      "Bandstop biquad filter, order #O, Q=#V, centre frequency #F" },
1284    { des_apbq, "ApBq#o/#V/#F",
1285      "Allpass biquad filter, order #O, Q=#V, centre frequency #F" },
1286    { des_pkbq, "PkBq#o/#V/#V/#F",
1287      "Peaking biquad filter, order #O, Q=#V, dBgain=#V, frequency #F" },
1288    { des_lsbq, "LsBq#o/#V/#V/#F",
1289      "Lowpass shelving biquad filter, S=#V, dBgain=#V, frequency #F" },
1290    { des_hsbq, "HsBq#o/#V/#V/#F",
1291      "Highpass shelving biquad filter, S=#V, dBgain=#V, frequency #F" },
1292    { des_lpbl, "LpBl/#F",
1293      "Lowpass Blackman window, -3.01dB frequency #F" },
1294    { des_lphm, "LpHm/#F",
1295      "Lowpass Hamming window, -3.01dB frequency #F" },
1296    { des_lphn, "LpHn/#F",
1297      "Lowpass Hann window, -3.01dB frequency #F" },
1298    { des_lpba, "LpBa/#F",
1299      "Lowpass Bartlet (triangular) window, -3.01dB frequency #F" },
1300    { 0, 0, 0 }
1301 };
1302 
1303 //
1304 //	Design a filter.  Spec and range are passed as arguments.  The
1305 //	return value is a pointer to a FidFilter as documented earlier
1306 //	in this file.  This needs to be free()d once finished with.
1307 //
1308 //	If 'f_adj' is set, then the frequencies fed to the design code
1309 //	are adjusted automatically to get true sqrt(0.5) (-3.01dB)
1310 //	values at the provided frequencies.  (This is obviously a
1311 //	slower operation)
1312 //
1313 //	If 'descp' is non-0, then a long description of the filter is
1314 //	generated and returned as a strdup'd string at the given
1315 //	location.
1316 //
1317 //	Any problem with the spec causes the program to die with an
1318 //	error message.
1319 //
1320 //	'spec' gives the specification string.  The 'rate' argument
1321 //	gives the sampling rate for the data that will be passed to
1322 //	the filter.  This is only used to interpret the frequencies
1323 //	given in the spec or given in 'freq0' and 'freq1'.  Use 1.0 if
1324 //	the frequencies are given as a proportion of the sampling
1325 //	rate, in the range 0 to 0.5.  'freq0' and 'freq1' provide the
1326 //	default frequency or frequency range if this is not included
1327 //	in the specification string.  These should be -ve if there is
1328 //	no default range (causing an error if they are omitted from
1329 //	the 'spec').
1330 //
1331 
1332 typedef struct Spec Spec;
1333 static char* parse_spec(Spec*);
1334 static FidFilter *auto_adjust_single(Spec *sp, double rate, double f0);
1335 static FidFilter *auto_adjust_dual(Spec *sp, double rate, double f0, double f1);
1336 struct Spec {
1337 #define MAXARG 10
1338    char *spec;
1339    double in_f0, in_f1;
1340    int in_adj;
1341    double argarr[MAXARG];
1342    double f0, f1;
1343    int adj;
1344    int n_arg;
1345    int order;
1346    int minlen;		// Minimum length of spec-string, assuming f0/f1 passed separately
1347    int n_freq;		// Number of frequencies provided: 0,1,2
1348    int fi;		// Filter index (filter[fi])
1349 };
1350 
1351 FidFilter *
fid_design(char * spec,double rate,double freq0,double freq1,int f_adj,char ** descp)1352 fid_design(char *spec, double rate, double freq0, double freq1, int f_adj, char **descp) {
1353    FidFilter *rv;
1354    Spec sp;
1355    double f0, f1;
1356    char *err;
1357 
1358    // Parse the filter-spec
1359    sp.spec= spec;
1360    sp.in_f0= freq0;
1361    sp.in_f1= freq1;
1362    sp.in_adj= f_adj;
1363    err= parse_spec(&sp);
1364    if (err) error("%s", err);
1365    f0= sp.f0;
1366    f1= sp.f1;
1367 
1368    // Adjust frequencies to range 0-0.5, and check them
1369    f0 /= rate;
1370    if (f0 > 0.5) error("Frequency of %gHz out of range with sampling rate of %gHz", f0*rate, rate);
1371    f1 /= rate;
1372    if (f1 > 0.5) error("Frequency of %gHz out of range with sampling rate of %gHz", f1*rate, rate);
1373 
1374    // Okay we now have a successful spec-match to filter[sp.fi], and sp.n_arg
1375    // args are now in sp.argarr[]
1376 
1377    // Generate the filter
1378    if (!sp.adj)
1379       rv= filter[sp.fi].rout(rate, f0, f1, sp.order, sp.n_arg, sp.argarr);
1380    else if (strstr(filter[sp.fi].fmt, "#R"))
1381       rv= auto_adjust_dual(&sp, rate, f0, f1);
1382    else
1383       rv= auto_adjust_single(&sp, rate, f0);
1384 
1385    // Generate a long description if required
1386    if (descp) {
1387       char *fmt= filter[sp.fi].txt;
1388       int max= strlen(fmt) + 60 + sp.n_arg * 20;
1389       char *desc= Alloc(max);
1390       char *p= desc;
1391       char ch;
1392       double *arg= sp.argarr;
1393       int n_arg= sp.n_arg;
1394 
1395       while ((ch= *fmt++)) {
1396 	 if (ch != '#') {
1397 	    *p++= ch;
1398 	    continue;
1399 	 }
1400 
1401 	 switch (*fmt++) {
1402 	  case 'O':
1403 	     p += sprintf(p, "%d", sp.order);
1404 	     break;
1405 	  case 'F':
1406 	     p += sprintf(p, "%g", f0*rate);
1407 	     break;
1408 	  case 'R':
1409 	     p += sprintf(p, "%g-%g", f0*rate, f1*rate);
1410 	     break;
1411 	  case 'V':
1412 	     if (n_arg <= 0)
1413 		error("Internal error -- disagreement between filter short-spec\n"
1414 		      " and long-description over number of arguments");
1415 	     n_arg--;
1416 	     p += sprintf(p, "%g", *arg++);
1417 	     break;
1418 	  default:
1419 	     error("Internal error: unknown format in long description: #%c", fmt[-1]);
1420 	 }
1421       }
1422       *p++= 0;
1423       if (p-desc >= max) error("Internal error: exceeded estimated description buffer");
1424       *descp= desc;
1425    }
1426 
1427    return rv;
1428 }
1429 
1430 //
1431 //	Auto-adjust input frequency to give correct sqrt(0.5)
1432 //	(~-3.01dB) point to 6 figures
1433 //
1434 
1435 #define M301DB (0.707106781186548)
1436 
1437 static FidFilter *
auto_adjust_single(Spec * sp,double rate,double f0)1438 auto_adjust_single(Spec *sp, double rate, double f0) {
1439    double a0, a1, a2;
1440    FidFilter *(*design)(double,double,double,int,int,double*)= filter[sp->fi].rout;
1441    FidFilter *rv= 0;
1442    double resp;
1443    double r0, r2;
1444    int incr;		// Increasing (1) or decreasing (0)
1445    int a;
1446 
1447 #define DESIGN(aa) design(rate, aa, aa, sp->order, sp->n_arg, sp->argarr)
1448 #define TEST(aa) { if (rv) {free(rv);rv= 0;} rv= DESIGN(aa); resp= fid_response(rv, f0); }
1449 
1450    // Try and establish a range within which we can find the point
1451    a0= f0; TEST(a0); r0= resp;
1452    for (a= 2; 1; a*=2) {
1453       a2= f0/a; TEST(a2); r2= resp;
1454       if ((r0 < M301DB) != (r2 < M301DB)) break;
1455       a2= 0.5-((0.5-f0)/a); TEST(a2); r2= resp;
1456       if ((r0 < M301DB) != (r2 < M301DB)) break;
1457       if (a == 32) 	// No success
1458 	 error("auto_adjust_single internal error -- can't establish enclosing range");
1459    }
1460 
1461    incr= r2 > r0;
1462    if (a0 > a2) {
1463       a1= a0; a0= a2; a2= a1;
1464       incr= !incr;
1465    }
1466 
1467    // Binary search
1468    while (1) {
1469       a1= 0.5 * (a0 + a2);
1470       if (a1 == a0 || a1 == a2) break;		// Limit of double, sanity check
1471       TEST(a1);
1472       if (resp >= 0.9999995 * M301DB && resp < 1.0000005 * M301DB) break;
1473       if (incr == (resp > M301DB))
1474 	 a2= a1;
1475       else
1476 	 a0= a1;
1477    }
1478 
1479 #undef TEST
1480 #undef DESIGN
1481 
1482    return rv;
1483 }
1484 
1485 
1486 //
1487 //	Auto-adjust input frequencies to give response of sqrt(0.5)
1488 //	(~-3.01dB) correct to 6sf at the given frequency-points
1489 //
1490 
1491 static FidFilter *
auto_adjust_dual(Spec * sp,double rate,double f0,double f1)1492 auto_adjust_dual(Spec *sp, double rate, double f0, double f1) {
1493    double mid= 0.5 * (f0+f1);
1494    double wid= 0.5 * fabs(f1-f0);
1495    FidFilter *(*design)(double,double,double,int,int,double*)= filter[sp->fi].rout;
1496    FidFilter *rv= 0;
1497    int bpass= -1;
1498    double delta;
1499    double mid0, mid1;
1500    double wid0, wid1;
1501    double r0, r1, err0, err1;
1502    double perr;
1503    int cnt;
1504    int cnt_design= 0;
1505 
1506 #define DESIGN(mm,ww) { if (rv) {free(rv);rv= 0;} \
1507    rv= design(rate, mm-ww, mm+ww, sp->order, sp->n_arg, sp->argarr); \
1508    r0= fid_response(rv, f0); r1= fid_response(rv, f1); \
1509    err0= fabs(M301DB-r0); err1= fabs(M301DB-r1); cnt_design++; }
1510 
1511 #define INC_WID ((r0+r1 < 1.0) == bpass)
1512 #define INC_MID ((r0 > r1) == bpass)
1513 #define MATCH (err0 < 0.000000499 && err1 < 0.000000499)
1514 #define PERR (err0+err1)
1515 
1516    DESIGN(mid, wid);
1517    bpass= (fid_response(rv, 0) < 0.5);
1518    delta= wid * 0.5;
1519 
1520    // Try delta changes until we get there
1521    for (cnt= 0; 1; cnt++, delta *= 0.51) {
1522       DESIGN(mid, wid);		// I know -- this is redundant
1523       perr= PERR;
1524 
1525       mid0= mid;
1526       wid0= wid;
1527       mid1= mid + (INC_MID ? delta : -delta);
1528       wid1= wid + (INC_WID ? delta : -delta);
1529 
1530       if (mid0 - wid1 > 0.0 && mid0 + wid1 < 0.5) {
1531 	 DESIGN(mid0, wid1);
1532 	 if (MATCH) break;
1533 	 if (PERR < perr) { perr= PERR; mid= mid0; wid= wid1; }
1534       }
1535 
1536       if (mid1 - wid0 > 0.0 && mid1 + wid0 < 0.5) {
1537 	 DESIGN(mid1, wid0);
1538 	 if (MATCH) break;
1539 	 if (PERR < perr) { perr= PERR; mid= mid1; wid= wid0; }
1540       }
1541 
1542       if (mid1 - wid1 > 0.0 && mid1 + wid1 < 0.5) {
1543 	 DESIGN(mid1, wid1);
1544 	 if (MATCH) break;
1545 	 if (PERR < perr) { perr= PERR; mid= mid1; wid= wid1; }
1546       }
1547 
1548       if (cnt > 1000)
1549 	 error("auto_adjust_dual -- design not converging");
1550    }
1551 
1552 #undef INC_WID
1553 #undef INC_MID
1554 #undef MATCH
1555 #undef PERR
1556 #undef DESIGN
1557 
1558    return rv;
1559 }
1560 
1561 
1562 //
1563 //	Expand a specification string to the given buffer; if out of
1564 //	space, drops dead
1565 //
1566 
1567 static void
expand_spec(char * buf,char * bufend,char * str)1568 expand_spec(char *buf, char *bufend, char *str) {
1569    int ch;
1570    char *p= buf;
1571 
1572    while ((ch= *str++)) {
1573       if (p + 10 >= bufend)
1574 	 error("Buffer overflow in fidlib expand_spec()");
1575       if (ch == '#') {
1576 	 switch (*str++) {
1577 	  case 'o': p += sprintf(p, "<optional-order>"); break;
1578 	  case 'O': p += sprintf(p, "<order>"); break;
1579 	  case 'F': p += sprintf(p, "<freq>"); break;
1580 	  case 'R': p += sprintf(p, "<range>"); break;
1581 	  case 'V': p += sprintf(p, "<value>"); break;
1582 	  default: p += sprintf(p, "<%c>", str[-1]); break;
1583 	 }
1584       } else {
1585 	 *p++= ch;
1586       }
1587    }
1588    *p= 0;
1589 }
1590 
1591 //
1592 //	Design a filter and reduce it to a list of all the non-const
1593 //	coefficients.  Arguments are as for fid_filter().  The
1594 //	coefficients are written into the given double array.  If the
1595 //	number of coefficients doesn't match the array length given,
1596 //	then a fatal error is generated.
1597 //
1598 //	Note that all 1-element FIRs and IIR first-coefficients are
1599 //	merged into a single gain coefficient, which is returned
1600 //	rather than being included in the coefficient list.  This is
1601 //	to allow it to be merged with other gains within a stack of
1602 //	filters.
1603 //
1604 //	The algorithm used here (merging 1-element FIRs and adjusting
1605 //	IIR first-coefficients) must match that used in the code-
1606 //	generating code, or else the coefficients won't match up.  The
1607 //	'n_coef' argument provides a partial safeguard.
1608 //
1609 
1610 double
fid_design_coef(double * coef,int n_coef,char * spec,double rate,double freq0,double freq1,int adj)1611 fid_design_coef(double *coef, int n_coef, char *spec, double rate,
1612 		double freq0, double freq1, int adj) {
1613    FidFilter *filt= fid_design(spec, rate, freq0, freq1, adj, 0);
1614    FidFilter *ff= filt;
1615    int a, len;
1616    int cnt= 0;
1617    double gain= 1.0;
1618    double *iir, *fir, iir_adj;
1619    static double const_one= 1;
1620    int n_iir, n_fir;
1621    int iir_cbm, fir_cbm;
1622 
1623    while (ff->typ) {
1624       if (ff->typ == 'F' && ff->len == 1) {
1625 	 gain *= ff->val[0];
1626 	 ff= FFNEXT(ff);
1627 	 continue;
1628       }
1629 
1630       if (ff->typ != 'I' && ff->typ != 'F')
1631 	 error("fid_design_coef can't handle FidFilter type: %c", ff->typ);
1632 
1633       // Initialise to safe defaults
1634       iir= fir= &const_one;
1635       n_iir= n_fir= 1;
1636       iir_cbm= fir_cbm= ~0;
1637 
1638       // See if we have an IIR filter
1639       if (ff->typ == 'I') {
1640 	 iir= ff->val;
1641 	 n_iir= ff->len;
1642 	 iir_cbm= ff->cbm;
1643 	 iir_adj= 1.0 / ff->val[0];
1644 	 ff= FFNEXT(ff);
1645 	 gain *= iir_adj;
1646       }
1647 
1648       // See if we have an FIR filter
1649       if (ff->typ == 'F') {
1650 	 fir= ff->val;
1651 	 n_fir= ff->len;
1652 	 fir_cbm= ff->cbm;
1653 	 ff= FFNEXT(ff);
1654       }
1655 
1656       // Dump out all non-const coefficients in reverse order
1657       len= n_fir > n_iir ? n_fir : n_iir;
1658       for (a= len-1; a>=0; a--) {
1659 	 // Output IIR if present and non-const
1660 	 if (a < n_iir && a>0 &&
1661 	     !(iir_cbm & (1<<(a<15?a:15)))) {
1662 	    if (cnt++ < n_coef) *coef++= iir_adj * iir[a];
1663 	 }
1664 
1665 	 // Output FIR if present and non-const
1666 	 if (a < n_fir &&
1667 	     !(fir_cbm & (1<<(a<15?a:15)))) {
1668 	    if (cnt++ < n_coef) *coef++= fir[a];
1669 	 }
1670       }
1671    }
1672 
1673    if (cnt != n_coef)
1674       error("fid_design_coef called with the wrong number of coefficients.\n"
1675 	    "  Given %d, expecting %d: (\"%s\",%g,%g,%g,%d)",
1676 	    n_coef, cnt, spec, rate, freq0, freq1, adj);
1677 
1678    free(filt);
1679    return gain;
1680 }
1681 
1682 //
1683 //	List all the known filters to the given file handle
1684 //
1685 
1686 void
fid_list_filters(FILE * out)1687 fid_list_filters(FILE *out) {
1688    int a;
1689 
1690    for (a= 0; filter[a].fmt; a++) {
1691       char buf[4096];
1692       expand_spec(buf, buf+sizeof(buf), filter[a].fmt);
1693       fprintf(out, "%s\n    ", buf);
1694       expand_spec(buf, buf+sizeof(buf), filter[a].txt);
1695       fprintf(out, "%s\n", buf);
1696    }
1697 }
1698 
1699 //
1700 //	List all the known filters to the given buffer; the buffer is
1701 //	NUL-terminated; returns 1 okay, 0 not enough space
1702 //
1703 
1704 int
fid_list_filters_buf(char * buf,char * bufend)1705 fid_list_filters_buf(char *buf, char *bufend) {
1706    int a, cnt;
1707    char tmp[4096];
1708 
1709    for (a= 0; filter[a].fmt; a++) {
1710       expand_spec(tmp, tmp+sizeof(tmp), filter[a].fmt);
1711       buf += (cnt= snprintf(buf, bufend-buf, "%s\n    ", tmp));
1712       if (cnt < 0 || buf >= bufend) return 0;
1713       expand_spec(tmp, tmp+sizeof(tmp), filter[a].txt);
1714       buf += (cnt= snprintf(buf, bufend-buf, "%s\n", tmp));
1715       if (cnt < 0 || buf >= bufend) return 0;
1716    }
1717    return 1;
1718 }
1719 
1720 //
1721 //      Do a convolution of parameters in place
1722 //
1723 
1724 STATIC_INLINE int
convolve(double * dst,int n_dst,double * src,int n_src)1725 convolve(double *dst, int n_dst, double *src, int n_src) {
1726    int len= n_dst + n_src - 1;
1727    int a, b;
1728 
1729    for (a= len-1; a>=0; a--) {
1730       double val= 0;
1731       for (b= 0; b<n_src; b++)
1732          if (a-b >= 0 && a-b < n_dst)
1733             val += src[b] * dst[a-b];
1734       dst[a]= val;
1735    }
1736 
1737    return len;
1738 }
1739 
1740 //
1741 //	Generate a combined filter -- merge all the IIR/FIR
1742 //	sub-filters into a single IIR/FIR pair, and make sure the IIR
1743 //	first coefficient is 1.0.
1744 //
1745 
1746 FidFilter *
fid_flatten(FidFilter * filt)1747 fid_flatten(FidFilter *filt) {
1748    int m_fir= 1;	// Maximum values
1749    int m_iir= 1;
1750    int n_fir, n_iir;	// Stored counts during convolution
1751    FidFilter *ff;
1752    FidFilter *rv;
1753    double *fir, *iir;
1754    double adj;
1755    int a;
1756 
1757    // Find the size of the output filter
1758    ff= filt;
1759    while (ff->len) {
1760       if (ff->typ == 'I')
1761 	 m_iir += ff->len-1;
1762       else if (ff->typ == 'F')
1763 	 m_fir += ff->len-1;
1764       else
1765 	 error("fid_flatten doesn't know about type %d", ff->typ);
1766       ff= FFNEXT(ff);
1767    }
1768 
1769    // Setup the output array
1770    rv= FFALLOC(2, m_iir + m_fir);
1771    rv->typ= 'I';
1772    rv->len= m_iir;
1773    iir= rv->val;
1774    ff= FFNEXT(rv);
1775    ff->typ= 'F';
1776    ff->len= m_fir;
1777    fir= ff->val;
1778 
1779    iir[0]= 1.0; n_iir= 1;
1780    fir[0]= 1.0; n_fir= 1;
1781 
1782    // Do the convolution
1783    ff= filt;
1784    while (ff->len) {
1785       if (ff->typ == 'I')
1786 	 n_iir= convolve(iir, n_iir, ff->val, ff->len);
1787       else
1788 	 n_fir= convolve(fir, n_fir, ff->val, ff->len);
1789       ff= FFNEXT(ff);
1790    }
1791 
1792    // Sanity check
1793    if (n_iir != m_iir ||
1794        n_fir != m_fir)
1795       error("Internal error in fid_combine() -- array under/overflow");
1796 
1797    // Fix iir[0]
1798    adj= 1.0/iir[0];
1799    for (a= 0; a<n_iir; a++) iir[a] *= adj;
1800    for (a= 0; a<n_fir; a++) fir[a] *= adj;
1801 
1802    return rv;
1803 }
1804 
1805 //
1806 //	Parse a filter-spec and freq0/freq1 arguments.  Returns a
1807 //	strdup'd error string on error, or else 0.
1808 //
1809 
1810 static char *
parse_spec(Spec * sp)1811 parse_spec(Spec *sp) {
1812    double *arg;
1813    int a;
1814 
1815    arg= sp->argarr;
1816    sp->n_arg= 0;
1817    sp->order= 0;
1818    sp->f0= 0;
1819    sp->f1= 0;
1820    sp->adj= 0;
1821    sp->minlen= -1;
1822    sp->n_freq= 0;
1823 
1824    for (a= 0; 1; a++) {
1825       char *fmt= filter[a].fmt;
1826       char *p= sp->spec;
1827       char ch, *q;
1828 
1829       if (!fmt) return strdupf("Spec-string \"%s\" matches no known format", sp->spec);
1830 
1831       while (*p && (ch= *fmt++)) {
1832 	 if (ch != '#') {
1833 	    if (ch == *p++) continue;
1834 	    p= 0; break;
1835 	 }
1836 
1837 	 if (isalpha(*p)) { p= 0; break; }
1838 
1839 	 // Handling a format character
1840 	 switch (ch= *fmt++) {
1841 	  default:
1842 	     return strdupf("Internal error: Unknown format #%c in format: %s",
1843 			    fmt[-1], filter[a].fmt);
1844 	  case 'o':
1845 	  case 'O':
1846 	     sp->order= (int)strtol(p, &q, 10);
1847 	     if (p == q) {
1848 		if (ch == 'O') goto bad;
1849 		sp->order= 1;
1850 	     }
1851 	     if (sp->order <= 0)
1852 		return strdupf("Bad order %d in spec-string \"%s\"", sp->order, sp->spec);
1853 	     p= q; break;
1854 	  case 'V':
1855 	     sp->n_arg++;
1856 	     *arg++= strtod(p, &q);
1857 	     if (p == q) goto bad;
1858 	     p= q; break;
1859 	  case 'F':
1860 	     sp->minlen= p-1-sp->spec;
1861 	     sp->n_freq= 1;
1862 	     sp->adj= (p[0] == '=');
1863 	     if (sp->adj) p++;
1864 	     sp->f0= strtod(p, &q);
1865 	     sp->f1= 0;
1866 	     if (p == q) goto bad;
1867 	     p= q; break;
1868 	  case 'R':
1869 	     sp->minlen= p-1-sp->spec;
1870 	     sp->n_freq= 2;
1871 	     sp->adj= (p[0] == '=');
1872 	     if (sp->adj) p++;
1873 	     sp->f0= strtod(p, &q);
1874 	     if (p == q) goto bad;
1875 	     p= q;
1876 	     if (*p++ != '-') goto bad;
1877 	     sp->f1= strtod(p, &q);
1878 	     if (p == q) goto bad;
1879 	     if (sp->f0 > sp->f1)
1880 		return strdupf("Backwards frequency range in spec-string \"%s\"", sp->spec);
1881 	     p= q; break;
1882 	 }
1883       }
1884 
1885       if (p == 0) continue;
1886 
1887       if (fmt[0] == '/' && fmt[1] == '#' && fmt[2] == 'F') {
1888 	 sp->minlen= p-sp->spec;
1889 	 sp->n_freq= 1;
1890 	 if (sp->in_f0 < 0.0)
1891 	    return strdupf("Frequency omitted from filter-spec, and no default provided");
1892 	 sp->f0= sp->in_f0;
1893 	 sp->f1= 0;
1894 	 sp->adj= sp->in_adj;
1895 	 fmt += 3;
1896       } else if (fmt[0] == '/' && fmt[1] == '#' && fmt[2] == 'R') {
1897 	 sp->minlen= p-sp->spec;
1898 	 sp->n_freq= 2;
1899 	 if (sp->in_f0 < 0.0 || sp->in_f1 < 0.0)
1900 	    return strdupf("Frequency omitted from filter-spec, and no default provided");
1901 	 sp->f0= sp->in_f0;
1902 	 sp->f1= sp->in_f1;
1903 	 sp->adj= sp->in_adj;
1904 	 fmt += 3;
1905       }
1906 
1907       // Check for trailing unmatched format characters
1908       if (*fmt) {
1909       bad:
1910 	 return strdupf("Bad match of spec-string \"%s\" to format \"%s\"",
1911 			sp->spec, filter[a].fmt);
1912       }
1913       if (sp->n_arg > MAXARG)
1914 	 return strdupf("Internal error -- maximum arguments exceeded");
1915 
1916       // Set the minlen to the whole string if unset
1917       if (sp->minlen < 0) sp->minlen= p-sp->spec;
1918 
1919       // Save values, return
1920       sp->fi= a;
1921       return 0;
1922    }
1923    return 0;
1924 }
1925 
1926 
1927 //
1928 //	Parse a filter-spec and freq0/freq1 arguments and rewrite them
1929 //	to give an all-in-one filter spec and/or a minimum spec plus
1930 //	separate freq0/freq1 arguments.  The all-in-one spec is
1931 //	returned in *spec1p (strdup'd), and the minimum separated-out
1932 //	spec is returned in *spec2p (strdup'd), *freq0p and *freq1p.
1933 //	If either of spec1p or spec2p is 0, then that particular
1934 //	spec-string is not generated.
1935 //
1936 
1937 void
fid_rewrite_spec(char * spec,double freq0,double freq1,int adj,char ** spec1p,char ** spec2p,double * freq0p,double * freq1p,int * adjp)1938 fid_rewrite_spec(char *spec, double freq0, double freq1, int adj,
1939 		 char **spec1p,
1940 		 char **spec2p, double *freq0p, double *freq1p, int *adjp) {
1941    Spec sp;
1942    char *err;
1943    sp.spec= spec;
1944    sp.in_f0= freq0;
1945    sp.in_f1= freq1;
1946    sp.in_adj= adj;
1947    err= parse_spec(&sp);
1948    if (err) error("%s", err);
1949 
1950    if (spec1p) {
1951       char buf[128];
1952       int len;
1953       char *rv;
1954       switch (sp.n_freq) {
1955        case 1: sprintf(buf, "/%s%.15g", sp.adj ? "=" : "", sp.f0); break;
1956        case 2: sprintf(buf, "/%s%.15g-%.15g", sp.adj ? "=" : "", sp.f0, sp.f1); break;
1957        default: buf[0]= 0;
1958       }
1959       len= strlen(buf);
1960       rv= Alloc(sp.minlen + len + 1);
1961       memcpy(rv, spec, sp.minlen);
1962       strcpy(rv+sp.minlen, buf);
1963       *spec1p= rv;
1964    }
1965 
1966    if (spec2p) {
1967       char *rv= Alloc(sp.minlen + 1);
1968       memcpy(rv, spec, sp.minlen);
1969       *spec2p= rv;
1970       *freq0p= sp.f0;
1971       *freq1p= sp.f1;
1972       *adjp= sp.adj;
1973    }
1974 }
1975 
1976 //
1977 //	Create a FidFilter from the given double array.  The double[]
1978 //	should contain one or more sections, each starting with the
1979 //	filter type (either 'I' or 'F', as a double), then a count of
1980 //	the number of coefficients following, then the coefficients
1981 //	themselves.  The end of the list is marked with a type of 0.
1982 //
1983 //	This is really just a convenience function, allowing a filter
1984 //	to be conveniently dumped to C source code and then
1985 //	reconstructed.
1986 //
1987 //	Note that for more general filter generation, FidFilter
1988 //	instances can be created simply by allocating the memory and
1989 //	filling them in (see fidlib.h).
1990 //
1991 
1992 FidFilter *
fid_cv_array(double * arr)1993 fid_cv_array(double *arr) {
1994    double *dp;
1995    FidFilter *ff, *rv;
1996    int n_head= 0;
1997    int n_val= 0;
1998 
1999    // Scan through for sizes
2000    for (dp= arr; *dp; ) {
2001       int len, typ;
2002 
2003       typ= (int)(*dp++);
2004       if (typ != 'F' && typ != 'I')
2005 	 error("Bad type in array passed to fid_cv_array: %g", dp[-1]);
2006 
2007       len= (int)(*dp++);
2008       if (len < 1)
2009 	 error("Bad length in array passed to fid_cv_array: %g", dp[-1]);
2010 
2011       n_head++;
2012       n_val += len;
2013       dp += len;
2014    }
2015 
2016    rv= ff= Alloc(FFCSIZE(n_head, n_val));
2017 
2018    // Scan through to fill in FidFilter
2019    for (dp= arr; *dp; ) {
2020       int len, typ;
2021       typ= (int)(*dp++);
2022       len= (int)(*dp++);
2023 
2024       ff->typ= typ;
2025       ff->cbm= ~0;
2026       ff->len= len;
2027       memcpy(ff->val, dp, len * sizeof(double));
2028       dp += len;
2029       ff= FFNEXT(ff);
2030    }
2031 
2032    // Final element already zero'd thanks to allocation
2033 
2034    return rv;
2035 }
2036 
2037 //
2038 //	Create a single filter from the given list of filters in
2039 //	order.  If 'freeme' is set, then all the listed filters are
2040 //	free'd once read; otherwise they are left untouched.  The
2041 //	newly allocated resultant filter is returned, which should be
2042 //	released with free() when finished with.
2043 //
2044 
2045 FidFilter *
fid_cat(int freeme,...)2046 fid_cat(int freeme, ...) {
2047    va_list ap;
2048    FidFilter *rv, *ff, *ff0;
2049    int len= 0;
2050    int cnt;
2051    char *dst;
2052 
2053    // Find the memory required to store the combined filter
2054    va_start(ap, freeme);
2055    while ((ff0= va_arg(ap, FidFilter*))) {
2056       for (ff= ff0; ff->typ; ff= FFNEXT(ff))
2057 	 ;
2058       len += ((char*)ff) - ((char*)ff0);
2059    }
2060    va_end(ap);
2061 
2062    rv= Alloc(FFCSIZE(0,0) + len);
2063    dst= (char*)rv;
2064 
2065    va_start(ap, freeme);
2066    while ((ff0= va_arg(ap, FidFilter*))) {
2067       for (ff= ff0; ff->typ; ff= FFNEXT(ff))
2068 	 ;
2069       cnt= ((char*)ff) - ((char*)ff0);
2070       memcpy(dst, ff0, cnt);
2071       dst += cnt;
2072       if (freeme) free(ff0);
2073    }
2074    va_end(ap);
2075 
2076    // Final element already zero'd
2077    return rv;
2078 }
2079 
2080 //
2081 //	Support for fid_parse
2082 //
2083 
2084 // Skip white space (including comments)
2085 static void
skipWS(char ** pp)2086 skipWS(char **pp) {
2087    char *p= *pp;
2088 
2089    while (*p) {
2090       if (isspace(*p)) { p++; continue; }
2091       if (*p == '#') {
2092          while (*p && *p != '\n') p++;
2093          continue;
2094       }
2095       break;
2096    }
2097    *pp= p;
2098 }
2099 
2100 // Grab a word from the input into the given buffer.  Returns 0: end
2101 // of file or error, else 1: success.  Error is indicated when the
2102 // word doesn't fit in the buffer.
2103 static int
grabWord(char ** pp,char * buf,int buflen)2104 grabWord(char **pp, char *buf, int buflen) {
2105    char *p, *q;
2106    int len;
2107 
2108    skipWS(pp);
2109    p= *pp;
2110    if (!*p) return 0;
2111 
2112    q= p;
2113    if (*q == ',' || *q == ';' || *q == ')' || *q == ']' || *q == '}') {
2114       q++;
2115    } else {
2116       while (*q && *q != '#' && !isspace(*q) &&
2117 	     (*q != ',' && *q != ';' && *q != ')' && *q != ']' && *q != '}'))
2118 	 q++;
2119    }
2120    len= q-p;
2121    if (len >= buflen) return 0;
2122 
2123    memcpy(buf, p, len);
2124    buf[len]= 0;
2125 
2126    *pp= q;
2127    return 1;
2128 }
2129 
2130 //
2131 //	Parse an entire filter specification, perhaps consisting of
2132 //	several FIR, IIR and predefined filters.  Stops at the first
2133 //	,; or unmatched )]}.  Returns either 0 on success, or else a
2134 //	strdup'd error string.
2135 //
2136 //	This duplicates code from Fiview filter.c, I know, but this
2137 //	may have to expand in the future to handle '+' operations, and
2138 //	special filter types like tunable heterodyne filters.  At that
2139 //	point, the filter.c code will have to be modified to call a
2140 //	version of this routine.
2141 //
2142 
2143 char *
fid_parse(double rate,char ** pp,FidFilter ** ffp)2144 fid_parse(double rate, char **pp, FidFilter **ffp) {
2145    char buf[128];
2146    char *p= *pp, *rew;
2147 #define INIT_LEN 128
2148    char *rv= Alloc(INIT_LEN);
2149    char *rvend= rv + INIT_LEN;
2150    char *rvp= rv;
2151    char *tmp;
2152 #undef INIT_LEN
2153    FidFilter *curr;
2154    int xtra= FFCSIZE(0,0);
2155    int typ= -1;		// First time through
2156    double val;
2157    char dmy;
2158 
2159 #define ERR(ptr, msg) { *pp= ptr; *ffp= 0; return msg; }
2160 #define INCBUF { tmp= realloc(rv, (rvend-rv) * 2); if (!tmp) error("Out of memory"); \
2161  rvend= (rvend-rv) * 2 + tmp; rvp= (rvp-rv) + tmp; \
2162  curr= (void*)(((char*)curr) - rv + tmp); rv= tmp; }
2163 
2164    while (1) {
2165       rew= p;
2166       if (!grabWord(&p, buf, sizeof(buf))) {
2167 	 if (*p) ERR(p, strdupf("Filter element unexpectedly long -- syntax error?"));
2168 	 buf[0]= 0;
2169       }
2170       if (!buf[0] || !buf[1]) switch (buf[0]) {
2171        default:
2172 	  break;
2173        case 0:
2174        case ',':
2175        case ';':
2176        case ')':
2177        case ']':
2178        case '}':
2179 	  // End of filter, return it
2180 	  tmp= realloc(rv, (rvp-rv) + xtra);
2181 	  if (!tmp) error("Out of memory");
2182 	  curr= (void*)((rvp-rv) + tmp);
2183 	  curr->typ= 0; curr->cbm= 0; curr->len= 0;
2184 	  *pp= buf[0] ? (p-1) : p;
2185 	  *ffp= (FidFilter*)tmp;
2186 	  return 0;
2187        case '/':
2188 	  if (typ > 0) ERR(rew, strdupf("Filter syntax error; unexpected '/'"));
2189 	  typ= 'I';
2190 	  continue;
2191        case 'x':
2192 	  if (typ > 0) ERR(rew, strdupf("Filter syntax error; unexpected 'x'"));
2193 	  typ= 'F';
2194 	  continue;
2195       }
2196 
2197       if (typ < 0) typ= 'F';		// Assume 'x' if missing
2198       if (!typ) ERR(p, strdupf("Expecting a 'x' or '/' before this"));
2199 
2200       if (1 != sscanf(buf, "%lf %c", &val, &dmy)) {
2201 	 // Must be a predefined filter
2202 	 FidFilter *ff;
2203 	 FidFilter *ff1;
2204 	 Spec sp;
2205 	 double f0, f1;
2206 	 char *err;
2207 	 int len;
2208 
2209 	 if (typ != 'F') ERR(rew, strdupf("Predefined filters cannot be used with '/'"));
2210 
2211 	 // Parse the filter-spec
2212 	 memset(&sp, 0, sizeof(sp));
2213 	 sp.spec= buf;
2214 	 sp.in_f0= sp.in_f1= -1;
2215 	 if ((err= parse_spec(&sp))) ERR(rew, err);
2216 	 f0= sp.f0;
2217 	 f1= sp.f1;
2218 
2219 	 // Adjust frequencies to range 0-0.5, and check them
2220 	 f0 /= rate;
2221 	 if (f0 > 0.5) ERR(rew, strdupf("Frequency of %gHz out of range with "
2222 					"sampling rate of %gHz", f0*rate, rate));
2223 	 f1 /= rate;
2224 	 if (f1 > 0.5) ERR(rew, strdupf("Frequency of %gHz out of range with "
2225 					"sampling rate of %gHz", f1*rate, rate));
2226 
2227 	 // Okay we now have a successful spec-match to filter[sp.fi], and sp.n_arg
2228 	 // args are now in sp.argarr[]
2229 
2230 	 // Generate the filter
2231 	 if (!sp.adj)
2232 	    ff= filter[sp.fi].rout(rate, f0, f1, sp.order, sp.n_arg, sp.argarr);
2233 	 else if (strstr(filter[sp.fi].fmt, "#R"))
2234 	    ff= auto_adjust_dual(&sp, rate, f0, f1);
2235 	 else
2236 	    ff= auto_adjust_single(&sp, rate, f0);
2237 
2238 	 // Append it to our FidFilter to return
2239 	 for (ff1= ff; ff1->typ; ff1= FFNEXT(ff1)) ;
2240 	 len= ((char*)ff1-(char*)ff);
2241 	 while (rvp + len + xtra >= rvend) INCBUF;
2242 	 memcpy(rvp, ff, len); rvp += len;
2243 	 free(ff);
2244 	 typ= 0;
2245 	 continue;
2246       }
2247 
2248       // Must be a list of coefficients
2249       curr= (void*)rvp;
2250       rvp += xtra;
2251       while (rvp + sizeof(double) >= rvend) INCBUF;
2252       curr->typ= typ;
2253       curr->cbm= ~0;
2254       curr->len= 1;
2255       *(double*)rvp= val;
2256       rvp += sizeof(double);
2257 
2258       // See how many more coefficients we can pick up
2259       while (1) {
2260 	 rew= p;
2261 	 if (!grabWord(&p, buf, sizeof(buf))) {
2262 	    if (*p) ERR(p, strdupf("Filter element unexpectedly long -- syntax error?"));
2263 	    buf[0]= 0;
2264 	 }
2265 	 if (1 != sscanf(buf, "%lf %c", &val, &dmy)) {
2266 	    p= rew;
2267 	    break;
2268 	 }
2269 	 while (rvp + sizeof(double) >= rvend) INCBUF;
2270 	 curr->len++;
2271 	 *(double*)rvp= val;
2272 	 rvp += sizeof(double);
2273       }
2274       typ= 0;
2275       continue;
2276    }
2277 
2278 #undef INCBUF
2279 #undef ERR
2280 
2281    return strdupf("Internal error, shouldn't reach here");
2282 }
2283 
2284 
2285 //
2286 //	Filter-running code
2287 //
2288 
2289 #ifdef RF_COMBINED
2290 #include "fidrf_combined.h"
2291 #endif
2292 
2293 #ifdef RF_CMDLIST
2294 #include "fidrf_cmdlist.h"
2295 #endif
2296 
2297 #ifdef RF_JIT
2298 #include "fidrf_jit.h"
2299 #endif
2300 
2301 
2302 // END //
2303