Initial commit of mpg123-1.29.3
This commit is contained in:
Sam Lantinga
373
src/libsyn123/filter.c
Normal file
373
src/libsyn123/filter.c
Normal file
@@ -0,0 +1,373 @@
|
||||
/*
|
||||
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;
|
||||
}
|
||||
Reference in New Issue
Block a user