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