Debian Codegraph @

src/libsyn123/filter.c

raw
1 /*
2 filter: apply IIR/FIR filters using Direct Form II
3
4 copyright 2019 by the mpg123 project
5 licensed under the terms of the LGPL 2.1
6 see COPYING and AUTHORS files in distribution or http://mpg123.org
7
8 initially written by Thomas Orgis
9
10 Since I spent a lot of time tuning filters for my resampler, I figured
11 I should add generic digital filters to the syn123 API. There are
12 multiple independent implementations of filters inside the resampler,
13 using hardcoded parameters. They should be functionally equivalent to
14 this generic code. Let it serve as a reference.
15
16 The code should be straightforward to read, but is messed up a bit
17 by the task to be runtime-tunable for single or double precision.
18 Also, the coefficients are stored multiple times in a shifted manner
19 to avoid shifting the filter history around and keeping efficient
20 coefficient access. I guess this is a pretty standard optimization
21 which worked well for the resampler.
22
23 I use macros. Don't be scared.
24 */
25
26 #define NO_GROW_BUF
27 #define NO_SMAX
28 #define NO_SMIN
29 #include "syn123_int.h"
30
31 // 0 ... ORDER
32 // b0 stored
33 // a0 == 1
34 // ringbuffer-optimized storage of the others
35 // b[order][order]
36 // a[order][order]
37 // historic values, ringbuffer
38 // w[channels][order]
39 // So, one piece of storage of size order*(2*order+channels).
40
41 // Theoretical limit on maximum order: data block must fit into size_t.
42 // Checking overflow of order*order*2 + order*channels, in that order.
43 #define ORDER_TOO_BIG(order, channels) \
44 ((order) == 0 || (channels) == 0) \
45 ? 0 \
46 : ( (order) > SIZE_MAX/2/(order) \
47 ? 1 \
48 : ( (order) > (SIZE_MAX-(order)*(order)*2)/(channels) \
49 ? 1 \
50 : 0 \
51 )\
52 )
53
54 // size of filter data block
55 #define F_DATABLOCK(f, channels) ((f).order*(2*(f).order+channels))
56
57 // base addresses of b and a coefficient sets, ringbuffered
58 #define Fb(f) ((f).data)
59 #define Fa(f) ((f).data+(f).order*(f).order)
60 // address of currently active coefficient set
61 // the base is either f.b or f.a (which may be NULL)
62 #define Fc(f, base) ((base) ? ((base)+(f).order*(f).n1) : NULL)
63 // base address of current channel's history values
64 #define Fw(f, c) ((f).data+F_DATABLOCK((f), (c)))
65
66 #define FILTER_STRUCT(name, type) \
67 struct name \
68 { \
69 int flow; /* if the filter has had first data and is in flow now */ \
70 unsigned int order; /* filter order */ \
71 unsigned int n1; /* current position of most recent past */ \
72 type init_scale; /* scale for first sample as endless history */ \
73 type b0; /* coefficient b_0 */ \
74 type *data; /* dynamic coefficient and history data */ \
75 type *b; /* b coefficients b_1 ... b_N, pointer into the above */ \
76 type *a; /* same for a, or NULL if non-recursive */ \
77 };
78
79 FILTER_STRUCT(f_filter, float)
80 FILTER_STRUCT(d_filter, double)
81
82 // Initialization of Direct Form II history.
83 // Return a constant value for w[n] that represents an endless history of given insample.
84 // Given the recursive definition: w[n] = x[n] - sum_i(a[i] * w[n-i])
85 // Given my ansatz of endless stream of x[n-i] = x[n] and hence w[n-i] = w[n], it follows
86 // that
87 // w[n] = x[n] - sum_i(a[i] * w[n]) = x[n] - sum_i(a[i]) * w[n]
88 // <=> (1 + sum_i(a[i])) w[n] = x[n]
89 // <=> w[n] = 1 / (1 + sum_i(a[i])) * x[n]
90 // Looks simple. Just a scale value.
91 #define INIT_SCALE(name, type) \
92 static type name(unsigned int order, type *filter_a) \
93 { \
94 type asum = 1.; \
95 if(filter_a) \
96 for(unsigned int i=0; i<order; ++i) \
97 asum += filter_a[i]; \
98 return (asum > 1e-12 || asum < -1e-12) ? (type)1./asum : 0; \
99 }
100
101 INIT_SCALE(d_init_scale, double)
102 INIT_SCALE(f_init_scale, float)
103
104 // Static ring buffer index used for filter history and coefficients.
105 // i: offset
106 // n1: current position of the first entry
107 // s: size of the ring buffer
108 #define RING_INDEX(i, n1, s) ( ((n1)+(i)) % s )
109 // In the context of a filter: stored offset n1, configured order.
110 #define F_RING_INDEX(f, i) RING_INDEX(i, (f).n1, (f).order)
111
112 int attribute_align_arg
113 syn123_setup_filter( syn123_handle *sh
114 , int append, unsigned int order, double *b, double *a
115 , int mixenc, int channels, int init_firstval )
116 {
117 if(!sh)
118 return SYN123_BAD_HANDLE;
119 if(!append)
120 {
121 syn123_drop_filter(sh, sh->fc.count);
122 if(sh->fc.count)
123 return SYN123_WEIRD;
124 }
125 if(sh->fc.count)
126 {
127 if(!mixenc)
128 mixenc = sh->fc.mixenc;
129 if(!channels)
130 channels = sh->fc.channels;
131 }
132 if(channels < 1)
133 return SYN123_BAD_FMT;
134 if(mixenc != MPG123_ENC_FLOAT_32 && mixenc != MPG123_ENC_FLOAT_64)
135 return SYN123_BAD_ENC;
136 if(!b)
137 return SYN123_NO_DATA;
138 if(a && a[0] != 1.)
139 return SYN123_BAD_DATA;
140 if(ORDER_TOO_BIG(order, channels) || sh->fc.count == SIZE_MAX)
141 return SYN123_OVERFLOW;
142
143 if(sh->fc.maxcount == sh->fc.count)
144 {
145 #define EXPAND_FILTERS(list, stype) \
146 { \
147 struct stype *tmp = realloc( list \
148 , sizeof(struct stype)*(sh->fc.maxcount+1) ); \
149 if(!tmp) \
150 return SYN123_DOOM; \
151 list = tmp; \
152 }
153 if(mixenc == MPG123_ENC_FLOAT_32)
154 EXPAND_FILTERS(sh->fc.ff, f_filter)
155 else
156 EXPAND_FILTERS(sh->fc.df, d_filter)
157 // Storage for filter structs increased, nobody can take that away now.
158 #undef EXPAND_FILTERS
159 ++sh->fc.maxcount;
160 }
161 // Got enough struct storage, try to fill the filter.
162 #define MAKE_FILTER(list, stype, scaler, type) \
163 { \
164 struct stype *tmp = list+sh->fc.count; \
165 tmp->flow = 0; \
166 tmp->order = order; \
167 tmp->b0 = b[0]; \
168 tmp->data = malloc(sizeof(type)*F_DATABLOCK(*tmp, channels)); \
169 if(!tmp->data) \
170 return SYN123_DOOM; \
171 tmp->b = Fb(*tmp); \
172 tmp->a = a ? Fa(*tmp) : NULL; \
173 for(tmp->n1=0; tmp->n1<order; ++tmp->n1) \
174 { \
175 type *f_b = Fc(*tmp, tmp->b); \
176 type *f_a = Fc(*tmp, tmp->a); \
177 for(unsigned int o=0; o<order; ++o) \
178 { \
179 unsigned int ri = F_RING_INDEX(*tmp, o); \
180 f_b[ri] = b[o+1]; \
181 if(f_a) \
182 f_a[ri] = a[o+1]; \
183 } \
184 } \
185 tmp->n1 = 0; \
186 tmp->init_scale = init_firstval ? scaler(order, tmp->a) : 0.; \
187 if(tmp->init_scale == 0.) \
188 { \
189 for(int c=0; c<channels; ++c) \
190 { \
191 type *w = Fw(*tmp, c); \
192 for(unsigned int o=0; o<order; ++o) \
193 w[o] = (type)0.; \
194 } \
195 tmp->flow = 1; \
196 } \
197 }
198 if(mixenc == MPG123_ENC_FLOAT_32)
199 MAKE_FILTER(sh->fc.ff, f_filter, f_init_scale, float)
200 else
201 MAKE_FILTER(sh->fc.df, d_filter, d_init_scale, double)
202 #undef MAKE_FILTER
203 // Got fresh filter with memory. Count it.
204 ++sh->fc.count;
205 // Finish off things that cannot error out.
206 sh->fc.mixenc = mixenc;
207 sh->fc.channels = channels;
208
209 return SYN123_OK;
210 }
211
212 int attribute_align_arg
213 syn123_query_filter( syn123_handle *sh, size_t position
214 , size_t *count, unsigned int *order, double *b, double *a
215 , int *mixenc, int *channels, int *init_firstval )
216 {
217 if(!sh)
218 return SYN123_BAD_HANDLE;
219 if( (order || b || a || mixenc || channels || init_firstval)
220 && position >= sh->fc.count )
221 return SYN123_NO_DATA;
222 if(count)
223 *count = sh->fc.count;
224 if(!sh->fc.count)
225 return SYN123_OK;
226 if(channels)
227 *channels = sh->fc.channels;
228 if(mixenc)
229 *mixenc = sh->fc.mixenc;
230
231 #define FILTER_INFO(filter, type) \
232 { \
233 if(order) \
234 *order = filter.order; \
235 if(init_firstval) \
236 *init_firstval = filter.init_scale != 0. ? 1 : 0; \
237 if(b || a) \
238 { \
239 if(b) \
240 b[0] = filter.b0; \
241 if(a) \
242 a[0] = 1.; \
243 for(unsigned int o=0; o<filter.order; ++o) \
244 { \
245 if(b) \
246 b[1+o] = filter.b[o]; \
247 if(a) \
248 a[1+o] = filter.a ? filter.a[o] : 0.; \
249 } \
250 } \
251 }
252 if(sh->fc.mixenc == MPG123_ENC_FLOAT_32)
253 FILTER_INFO(sh->fc.ff[position], float)
254 else
255 FILTER_INFO(sh->fc.df[position], double)
256 #undef FILTER_INFO
257 return SYN123_OK;
258 }
259
260 // This just forgets the filter's own storage, not the storage for
261 // the filter structs. That would need realloc(), which might fail.
262 void attribute_align_arg
263 syn123_drop_filter(syn123_handle *sh, size_t count)
264 {
265 if(!sh)
266 return;
267 if(count > sh->fc.count)
268 count = sh->fc.count;
269 // Free data of indicated filters.
270 for(size_t i=0; i<count; ++i)
271 free( sh->fc.mixenc == MPG123_ENC_FLOAT_32
272 ? (void*)(sh->fc.ff[--sh->fc.count].data)
273 : (void*)(sh->fc.df[--sh->fc.count].data) );
274 // count decreased now, maxcount still there
275 }
276
277 #define APPLY_ONE_FILTER(name, stype, type) \
278 static void name( struct stype *f \
279 , int channels, type *audio, size_t samples ) \
280 { \
281 if(!samples) \
282 return; \
283 if(!f->flow) \
284 { \
285 for(int c=0; c<channels; ++c) \
286 { \
287 type iv = f->init_scale*audio[c]; \
288 type *w = Fw(*f, c); \
289 for(unsigned int o=0; o<f->order; ++o) \
290 w[o] = iv; \
291 } \
292 f->n1 = 0; \
293 f->flow = 1; \
294 } \
295 /* Leave loop ordering and mono/stereo optimization to the future.*/ \
296 /* This is what is most logical for interleaved audio and the n1 logic */ \
297 if(f->a) /* IIR, possibly */ \
298 { \
299 for(size_t s=0; s<samples; ++s) \
300 { \
301 /* Working with the current value of n1. */ \
302 type *b = Fc(*f, f->b); \
303 type *a = Fc(*f, f->a); \
304 unsigned int next_n1 = F_RING_INDEX(*f, f->order-1); \
305 for(int c=0; c<channels; ++c) \
306 { \
307 type ny = 0; \
308 type nw = 0; \
309 type *w = Fw(*f, c); \
310 for(unsigned int i=0; i<f->order; ++i) \
311 { \
312 ny += w[i]*b[i]; \
313 nw -= w[i]*a[i]; \
314 } \
315 nw += audio[c]; \
316 ny += f->b0 * nw; \
317 w[next_n1] = nw; \
318 audio[c] = ny; \
319 } \
320 f->n1 = next_n1; \
321 audio += channels; \
322 } \
323 } else /* FIR */ \
324 { \
325 for(size_t s=0; s<samples; ++s) \
326 { \
327 /* Working with the current value of n1. */ \
328 type *b = Fc(*f, f->b); \
329 unsigned int next_n1 = F_RING_INDEX(*f, f->order-1); \
330 for(int c=0; c<channels; ++c) \
331 { \
332 type ny = 0; \
333 type nw = audio[c]; \
334 type *w = Fw(*f, c); \
335 for(unsigned int i=0; i<f->order; ++i) \
336 ny += w[i]*b[i]; \
337 ny += f->b0 * nw; \
338 w[next_n1] = nw; \
339 audio[c] = ny; \
340 } \
341 f->n1 = next_n1; \
342 audio += channels; \
343 } \
344 } \
345 }
346
347 APPLY_ONE_FILTER(apply_filter_float, f_filter, float)
348 APPLY_ONE_FILTER(apply_filter_double, d_filter, double)
349
350 // Finally, the filter application.
351 int attribute_align_arg
352 syn123_filter(syn123_handle *sh, void* buf, int encoding, size_t samples)
353 {
354 if(!sh)
355 return SYN123_BAD_HANDLE;
356 if(!sh->fc.count) // No filter means no change, all good.
357 return SYN123_OK;
358 #define APPLY_FILTERS(list, func) \
359 for(unsigned int i=0; i<sh->fc.count; ++i) \
360 func(list+i, sh->fc.channels, buf, samples)
361 if(encoding == sh->fc.mixenc)
362 {
363 if(sh->fc.mixenc == MPG123_ENC_FLOAT_32)
364 APPLY_FILTERS(sh->fc.ff, apply_filter_float);
365 else
366 APPLY_FILTERS(sh->fc.df, apply_filter_double);
367 } else
368 {
369 return SYN123_BAD_ENC;
370 }
371 #undef APPLY_FILTERS
372 return SYN123_OK;
373 }