374 lines
10 KiB
C
374 lines
10 KiB
C
/*
|
|
filter: apply IIR/FIR filters using Direct Form II
|
|
|
|
copyright 2019 by the mpg123 project
|
|
licensed under the terms of the LGPL 2.1
|
|
see COPYING and AUTHORS files in distribution or http://mpg123.org
|
|
|
|
initially written by Thomas Orgis
|
|
|
|
Since I spent a lot of time tuning filters for my resampler, I figured
|
|
I should add generic digital filters to the syn123 API. There are
|
|
multiple independent implementations of filters inside the resampler,
|
|
using hardcoded parameters. They should be functionally equivalent to
|
|
this generic code. Let it serve as a reference.
|
|
|
|
The code should be straightforward to read, but is messed up a bit
|
|
by the task to be runtime-tunable for single or double precision.
|
|
Also, the coefficients are stored multiple times in a shifted manner
|
|
to avoid shifting the filter history around and keeping efficient
|
|
coefficient access. I guess this is a pretty standard optimization
|
|
which worked well for the resampler.
|
|
|
|
I use macros. Don't be scared.
|
|
*/
|
|
|
|
#define NO_GROW_BUF
|
|
#define NO_SMAX
|
|
#define NO_SMIN
|
|
#include "syn123_int.h"
|
|
|
|
// 0 ... ORDER
|
|
// b0 stored
|
|
// a0 == 1
|
|
// ringbuffer-optimized storage of the others
|
|
// b[order][order]
|
|
// a[order][order]
|
|
// historic values, ringbuffer
|
|
// w[channels][order]
|
|
// So, one piece of storage of size order*(2*order+channels).
|
|
|
|
// Theoretical limit on maximum order: data block must fit into size_t.
|
|
// Checking overflow of order*order*2 + order*channels, in that order.
|
|
#define ORDER_TOO_BIG(order, channels) \
|
|
((order) == 0 || (channels) == 0) \
|
|
? 0 \
|
|
: ( (order) > SIZE_MAX/2/(order) \
|
|
? 1 \
|
|
: ( (order) > (SIZE_MAX-(order)*(order)*2)/(channels) \
|
|
? 1 \
|
|
: 0 \
|
|
)\
|
|
)
|
|
|
|
// size of filter data block
|
|
#define F_DATABLOCK(f, channels) ((f).order*(2*(f).order+channels))
|
|
|
|
// base addresses of b and a coefficient sets, ringbuffered
|
|
#define Fb(f) ((f).data)
|
|
#define Fa(f) ((f).data+(f).order*(f).order)
|
|
// address of currently active coefficient set
|
|
// the base is either f.b or f.a (which may be NULL)
|
|
#define Fc(f, base) ((base) ? ((base)+(f).order*(f).n1) : NULL)
|
|
// base address of current channel's history values
|
|
#define Fw(f, c) ((f).data+F_DATABLOCK((f), (c)))
|
|
|
|
#define FILTER_STRUCT(name, type) \
|
|
struct name \
|
|
{ \
|
|
int flow; /* if the filter has had first data and is in flow now */ \
|
|
unsigned int order; /* filter order */ \
|
|
unsigned int n1; /* current position of most recent past */ \
|
|
type init_scale; /* scale for first sample as endless history */ \
|
|
type b0; /* coefficient b_0 */ \
|
|
type *data; /* dynamic coefficient and history data */ \
|
|
type *b; /* b coefficients b_1 ... b_N, pointer into the above */ \
|
|
type *a; /* same for a, or NULL if non-recursive */ \
|
|
};
|
|
|
|
FILTER_STRUCT(f_filter, float)
|
|
FILTER_STRUCT(d_filter, double)
|
|
|
|
// Initialization of Direct Form II history.
|
|
// Return a constant value for w[n] that represents an endless history of given insample.
|
|
// Given the recursive definition: w[n] = x[n] - sum_i(a[i] * w[n-i])
|
|
// Given my ansatz of endless stream of x[n-i] = x[n] and hence w[n-i] = w[n], it follows
|
|
// that
|
|
// w[n] = x[n] - sum_i(a[i] * w[n]) = x[n] - sum_i(a[i]) * w[n]
|
|
// <=> (1 + sum_i(a[i])) w[n] = x[n]
|
|
// <=> w[n] = 1 / (1 + sum_i(a[i])) * x[n]
|
|
// Looks simple. Just a scale value.
|
|
#define INIT_SCALE(name, type) \
|
|
static type name(unsigned int order, type *filter_a) \
|
|
{ \
|
|
type asum = 1.; \
|
|
if(filter_a) \
|
|
for(unsigned int i=0; i<order; ++i) \
|
|
asum += filter_a[i]; \
|
|
return (asum > 1e-12 || asum < -1e-12) ? (type)1./asum : 0; \
|
|
}
|
|
|
|
INIT_SCALE(d_init_scale, double)
|
|
INIT_SCALE(f_init_scale, float)
|
|
|
|
// Static ring buffer index used for filter history and coefficients.
|
|
// i: offset
|
|
// n1: current position of the first entry
|
|
// s: size of the ring buffer
|
|
#define RING_INDEX(i, n1, s) ( ((n1)+(i)) % s )
|
|
// In the context of a filter: stored offset n1, configured order.
|
|
#define F_RING_INDEX(f, i) RING_INDEX(i, (f).n1, (f).order)
|
|
|
|
int attribute_align_arg
|
|
syn123_setup_filter( syn123_handle *sh
|
|
, int append, unsigned int order, double *b, double *a
|
|
, int mixenc, int channels, int init_firstval )
|
|
{
|
|
if(!sh)
|
|
return SYN123_BAD_HANDLE;
|
|
if(!append)
|
|
{
|
|
syn123_drop_filter(sh, sh->fc.count);
|
|
if(sh->fc.count)
|
|
return SYN123_WEIRD;
|
|
}
|
|
if(sh->fc.count)
|
|
{
|
|
if(!mixenc)
|
|
mixenc = sh->fc.mixenc;
|
|
if(!channels)
|
|
channels = sh->fc.channels;
|
|
}
|
|
if(channels < 1)
|
|
return SYN123_BAD_FMT;
|
|
if(mixenc != MPG123_ENC_FLOAT_32 && mixenc != MPG123_ENC_FLOAT_64)
|
|
return SYN123_BAD_ENC;
|
|
if(!b)
|
|
return SYN123_NO_DATA;
|
|
if(a && a[0] != 1.)
|
|
return SYN123_BAD_DATA;
|
|
if(ORDER_TOO_BIG(order, channels) || sh->fc.count == SIZE_MAX)
|
|
return SYN123_OVERFLOW;
|
|
|
|
if(sh->fc.maxcount == sh->fc.count)
|
|
{
|
|
#define EXPAND_FILTERS(list, stype) \
|
|
{ \
|
|
struct stype *tmp = realloc( list \
|
|
, sizeof(struct stype)*(sh->fc.maxcount+1) ); \
|
|
if(!tmp) \
|
|
return SYN123_DOOM; \
|
|
list = tmp; \
|
|
}
|
|
if(mixenc == MPG123_ENC_FLOAT_32)
|
|
EXPAND_FILTERS(sh->fc.ff, f_filter)
|
|
else
|
|
EXPAND_FILTERS(sh->fc.df, d_filter)
|
|
// Storage for filter structs increased, nobody can take that away now.
|
|
#undef EXPAND_FILTERS
|
|
++sh->fc.maxcount;
|
|
}
|
|
// Got enough struct storage, try to fill the filter.
|
|
#define MAKE_FILTER(list, stype, scaler, type) \
|
|
{ \
|
|
struct stype *tmp = list+sh->fc.count; \
|
|
tmp->flow = 0; \
|
|
tmp->order = order; \
|
|
tmp->b0 = b[0]; \
|
|
tmp->data = malloc(sizeof(type)*F_DATABLOCK(*tmp, channels)); \
|
|
if(!tmp->data) \
|
|
return SYN123_DOOM; \
|
|
tmp->b = Fb(*tmp); \
|
|
tmp->a = a ? Fa(*tmp) : NULL; \
|
|
for(tmp->n1=0; tmp->n1<order; ++tmp->n1) \
|
|
{ \
|
|
type *f_b = Fc(*tmp, tmp->b); \
|
|
type *f_a = Fc(*tmp, tmp->a); \
|
|
for(unsigned int o=0; o<order; ++o) \
|
|
{ \
|
|
unsigned int ri = F_RING_INDEX(*tmp, o); \
|
|
f_b[ri] = b[o+1]; \
|
|
if(f_a) \
|
|
f_a[ri] = a[o+1]; \
|
|
} \
|
|
} \
|
|
tmp->n1 = 0; \
|
|
tmp->init_scale = init_firstval ? scaler(order, tmp->a) : 0.; \
|
|
if(tmp->init_scale == 0.) \
|
|
{ \
|
|
for(int c=0; c<channels; ++c) \
|
|
{ \
|
|
type *w = Fw(*tmp, c); \
|
|
for(unsigned int o=0; o<order; ++o) \
|
|
w[o] = 0.; \
|
|
} \
|
|
tmp->flow = 1; \
|
|
} \
|
|
}
|
|
if(mixenc == MPG123_ENC_FLOAT_32)
|
|
MAKE_FILTER(sh->fc.ff, f_filter, f_init_scale, float)
|
|
else
|
|
MAKE_FILTER(sh->fc.df, d_filter, d_init_scale, double)
|
|
#undef MAKE_FILTER
|
|
// Got fresh filter with memory. Count it.
|
|
++sh->fc.count;
|
|
// Finish off things that cannot error out.
|
|
sh->fc.mixenc = mixenc;
|
|
sh->fc.channels = channels;
|
|
|
|
return SYN123_OK;
|
|
}
|
|
|
|
int attribute_align_arg
|
|
syn123_query_filter( syn123_handle *sh, size_t position
|
|
, size_t *count, unsigned int *order, double *b, double *a
|
|
, int *mixenc, int *channels, int *init_firstval )
|
|
{
|
|
if(!sh)
|
|
return SYN123_BAD_HANDLE;
|
|
if( (order || b || a || mixenc || channels || init_firstval)
|
|
&& position >= sh->fc.count )
|
|
return SYN123_NO_DATA;
|
|
if(count)
|
|
*count = sh->fc.count;
|
|
if(!sh->fc.count)
|
|
return SYN123_OK;
|
|
if(channels)
|
|
*channels = sh->fc.channels;
|
|
if(mixenc)
|
|
*mixenc = sh->fc.mixenc;
|
|
|
|
#define FILTER_INFO(filter, type) \
|
|
{ \
|
|
if(order) \
|
|
*order = filter.order; \
|
|
if(init_firstval) \
|
|
*init_firstval = filter.init_scale != 0. ? 1 : 0; \
|
|
if(b || a) \
|
|
{ \
|
|
if(b) \
|
|
b[0] = filter.b0; \
|
|
if(a) \
|
|
a[0] = 1.; \
|
|
for(unsigned int o=0; o<filter.order; ++o) \
|
|
{ \
|
|
if(b) \
|
|
b[1+o] = filter.b[o]; \
|
|
if(a) \
|
|
a[1+o] = filter.a ? filter.a[o] : 0.; \
|
|
} \
|
|
} \
|
|
}
|
|
if(sh->fc.mixenc == MPG123_ENC_FLOAT_32)
|
|
FILTER_INFO(sh->fc.ff[position], float)
|
|
else
|
|
FILTER_INFO(sh->fc.df[position], double)
|
|
#undef FILTER_INFO
|
|
return SYN123_OK;
|
|
}
|
|
|
|
// This just forgets the filter's own storage, not the storage for
|
|
// the filter structs. That would need realloc(), which might fail.
|
|
void attribute_align_arg
|
|
syn123_drop_filter(syn123_handle *sh, size_t count)
|
|
{
|
|
if(!sh)
|
|
return;
|
|
if(count > sh->fc.count)
|
|
count = sh->fc.count;
|
|
// Free data of indicated filters.
|
|
for(size_t i=0; i<count; ++i)
|
|
free( sh->fc.mixenc == MPG123_ENC_FLOAT_32
|
|
? (void*)(sh->fc.ff[--sh->fc.count].data)
|
|
: (void*)(sh->fc.df[--sh->fc.count].data) );
|
|
// count decreased now, maxcount still there
|
|
}
|
|
|
|
#define APPLY_ONE_FILTER(name, stype, type) \
|
|
static void name( struct stype *f \
|
|
, int channels, type *audio, size_t samples ) \
|
|
{ \
|
|
if(!samples) \
|
|
return; \
|
|
if(!f->flow) \
|
|
{ \
|
|
for(int c=0; c<channels; ++c) \
|
|
{ \
|
|
type iv = f->init_scale*audio[c]; \
|
|
type *w = Fw(*f, c); \
|
|
for(unsigned int o=0; o<f->order; ++o) \
|
|
w[o] = iv; \
|
|
} \
|
|
f->n1 = 0; \
|
|
f->flow = 1; \
|
|
} \
|
|
/* Leave loop ordering and mono/stereo optimization to the future.*/ \
|
|
/* This is what is most logical for interleaved audio and the n1 logic */ \
|
|
if(f->a) /* IIR, possibly */ \
|
|
{ \
|
|
for(size_t s=0; s<samples; ++s) \
|
|
{ \
|
|
/* Working with the current value of n1. */ \
|
|
type *b = Fc(*f, f->b); \
|
|
type *a = Fc(*f, f->a); \
|
|
unsigned int next_n1 = F_RING_INDEX(*f, f->order-1); \
|
|
for(int c=0; c<channels; ++c) \
|
|
{ \
|
|
type ny = 0; \
|
|
type nw = 0; \
|
|
type *w = Fw(*f, c); \
|
|
for(unsigned int i=0; i<f->order; ++i) \
|
|
{ \
|
|
ny += w[i]*b[i]; \
|
|
nw -= w[i]*a[i]; \
|
|
} \
|
|
nw += audio[c]; \
|
|
ny += f->b0 * nw; \
|
|
w[next_n1] = nw; \
|
|
audio[c] = ny; \
|
|
} \
|
|
f->n1 = next_n1; \
|
|
audio += channels; \
|
|
} \
|
|
} else /* FIR */ \
|
|
{ \
|
|
for(size_t s=0; s<samples; ++s) \
|
|
{ \
|
|
/* Working with the current value of n1. */ \
|
|
type *b = Fc(*f, f->b); \
|
|
unsigned int next_n1 = F_RING_INDEX(*f, f->order-1); \
|
|
for(int c=0; c<channels; ++c) \
|
|
{ \
|
|
type ny = 0; \
|
|
type nw = audio[c]; \
|
|
type *w = Fw(*f, c); \
|
|
for(unsigned int i=0; i<f->order; ++i) \
|
|
ny += w[i]*b[i]; \
|
|
ny += f->b0 * nw; \
|
|
w[next_n1] = nw; \
|
|
audio[c] = ny; \
|
|
} \
|
|
f->n1 = next_n1; \
|
|
audio += channels; \
|
|
} \
|
|
} \
|
|
}
|
|
|
|
APPLY_ONE_FILTER(apply_filter_float, f_filter, float)
|
|
APPLY_ONE_FILTER(apply_filter_double, d_filter, double)
|
|
|
|
// Finally, the filter application.
|
|
int attribute_align_arg
|
|
syn123_filter(syn123_handle *sh, void* buf, int encoding, size_t samples)
|
|
{
|
|
if(!sh)
|
|
return SYN123_BAD_HANDLE;
|
|
if(!sh->fc.count) // No filter means no change, all good.
|
|
return SYN123_OK;
|
|
#define APPLY_FILTERS(list, func) \
|
|
for(unsigned int i=0; i<sh->fc.count; ++i) \
|
|
func(list+i, sh->fc.channels, buf, samples)
|
|
if(encoding == sh->fc.mixenc)
|
|
{
|
|
if(sh->fc.mixenc == MPG123_ENC_FLOAT_32)
|
|
APPLY_FILTERS(sh->fc.ff, apply_filter_float);
|
|
else
|
|
APPLY_FILTERS(sh->fc.df, apply_filter_double);
|
|
} else
|
|
{
|
|
return SYN123_BAD_ENC;
|
|
}
|
|
#undef APPLY_FILTERS
|
|
return SYN123_OK;
|
|
}
|