| 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 |
} |