From 463ff5869ebd6d212316fa9fb8a351082a9de546 Mon Sep 17 00:00:00 2001 From: Johannes Schriewer Date: Fri, 1 Mar 2024 01:30:28 +0100 Subject: [PATCH] Implement Parametric EQ filter --- audio/audio_filter_param_eq.c | 437 ++++++++++++++++++++++++++ audio/include/audio_filter_param_eq.h | 39 +++ audio/include/todo/audio_filter_eq.h | 0 3 files changed, 476 insertions(+) create mode 100644 audio/audio_filter_param_eq.c create mode 100644 audio/include/audio_filter_param_eq.h delete mode 100644 audio/include/todo/audio_filter_eq.h diff --git a/audio/audio_filter_param_eq.c b/audio/audio_filter_param_eq.c new file mode 100644 index 0000000..bf3bbcb --- /dev/null +++ b/audio/audio_filter_param_eq.c @@ -0,0 +1,437 @@ +#include +#include +#include +#include +#include + +#include "audio.h" +#include "audio_internal.h" +#include "audio_filter_param_eq.h" + +#ifdef FIXEDPOINT +# define PRECISION int32_t +#elif defined(LOW_PRECISION) +# define PRECISION float +#else +# define PRECISION double +#endif + +typedef PRECISION biquad_type_t; + +typedef struct _BiQuad { + biquad_type_t b0, b1, b2, a1, a2; // coefficients + biquad_type_t x1, x2; // history +#ifdef SECOND_ORDER_FILTER + biquad_type_t y1, y2; // history for second stage +#endif +} BiQuad; + +typedef struct _FilterParamEQContext { + EQBand *bands; + uint8_t numBands; + BiQuad **elements; // [channel][band] +} FilterParamEQContext; + +#ifdef SECOND_ORDER_FILTER +static inline int16_t biquad_apply(BiQuad *element, int16_t sample) { +#ifdef FIXEDPOINT + int32_t result; + + result = \ + ((element->b0 * sample) >> 15) \ + + ((element->b1 * element->x1) >> 15) \ + + ((element->b2 * element->x2) >> 15) \ + - ((element->a1 * element->y1) >> 15) \ + - ((element->a2 * element->y2) >> 15); + + // clip output + int16_t out = result; + + if (result > INT16_MAX) { + out = INT16_MAX; + } + if (result < INT16_MIN) { + out = INT16_MIN; + } + + /* shift x1 to x2, sample to x1 */ + element->x2 = element->x1; + element->x1 = sample; + + /* shift y1 to y2, result to y1 */ + element->y2 = element->y1; + element->y1 = out; + + return out; +#else + biquad_type_t result; + + /* compute result */ + result = \ + element->b0 * (biquad_type_t)sample \ + + element->b1 * element->x1 \ + + element->b2 * element->x2 \ + - element->a1 * element->y1 \ + - element->a2 * element->y2; + + /* shift x1 to x2, sample to x1 */ + element->x2 = element->x1; + element->x1 = (double)sample; + + /* shift y1 to y2, result to y1 */ + element->y2 = element->y1; + element->y1 = result; + + result = round(result); + if (result > INT16_MAX) { + return INT16_MAX - 1; + } + if (result < INT16_MIN) { + return INT16_MIN + 1; + } + return (int16_t)result; +#endif +} + +static BiQuad biquad_initialize(EQBand band, uint32_t sampleRate) { + BiQuad b; + double A, omega, sn, cs, alpha, beta; + double a0, a1, a2, b0, b1, b2; + + double dbGain = band.gain; + double freq = band.frequency; + double bandwidth = band.Q; + + /* setup variables */ + A = pow(10, dbGain / 40); + omega = 2 * M_PI * freq / (double)sampleRate; + sn = sin(omega); + cs = cos(omega); + alpha = sn * sinh(M_LN2 / 2 * bandwidth * omega / sn); + beta = sqrt(A + A); + + switch (band.type) { + case EQTypeLowPass: + b0 = (1 - cs) /2; + b1 = 1 - cs; + b2 = (1 - cs) /2; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; + break; + case EQTypeHighPass: + b0 = (1 + cs) /2; + b1 = -(1 + cs); + b2 = (1 + cs) /2; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; + break; + case EQTypeBandPass: + b0 = alpha; + b1 = 0; + b2 = -alpha; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; + break; + case EQTypeNotch: + b0 = 1; + b1 = -2 * cs; + b2 = 1; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; + break; + case EQTypePeak: + b0 = 1 + (alpha * A); + b1 = -2 * cs; + b2 = 1 - (alpha * A); + a0 = 1 + (alpha /A); + a1 = -2 * cs; + a2 = 1 - (alpha /A); + break; + case EQTypeLowShelf: + b0 = A * ((A + 1) - (A - 1) * cs + beta * sn); + b1 = 2 * A * ((A - 1) - (A + 1) * cs); + b2 = A * ((A + 1) - (A - 1) * cs - beta * sn); + a0 = (A + 1) + (A - 1) * cs + beta * sn; + a1 = -2 * ((A - 1) + (A + 1) * cs); + a2 = (A + 1) + (A - 1) * cs - beta * sn; + break; + case EQTypeHighShelf: + b0 = A * ((A + 1) + (A - 1) * cs + beta * sn); + b1 = -2 * A * ((A - 1) + (A + 1) * cs); + b2 = A * ((A + 1) + (A - 1) * cs - beta * sn); + a0 = (A + 1) - (A - 1) * cs + beta * sn; + a1 = 2 * ((A - 1) - (A + 1) * cs); + a2 = (A + 1) - (A - 1) * cs - beta * sn; + break; + default: + break; + } + + /* precompute the coefficients */ +#ifdef FIXEDPOINT + b.b0 = b0 / a0 * INT16_MAX; + b.b1 = b1 / a0 * INT16_MAX; + b.b2 = b2 / a0 * INT16_MAX; + b.a1 = a1 / a0 * INT16_MAX; + b.a2 = a2 / a0 * INT16_MAX; +#else + b.b0 = b0 / a0; + b.b1 = b1 / a0; + b.b2 = b2 / a0; + b.a1 = a1 / a0; + b.a2 = a2 / a0; +#endif + + /* zero initial samples */ + b.x1 = b.x2 = 0; + b.y1 = b.y2 = 0; + + return b; +} +#else +static BiQuad biquad_initialize(EQBand band, uint32_t sampleRate) { + BiQuad b; + double b0, b1, b2, a1, a2; + double Fc = band.frequency / sampleRate; + + double norm; + double V = pow(10, fabs(band.gain) / 20.0); + double K = tan(M_PI * Fc); + + switch (band.type) { + case EQTypeLowPass: + norm = 1 / (1 + K / band.Q + K * K); + b0 = K * K * norm; + b1 = 2 * b0; + b2 = b0; + a1 = 2 * (K * K - 1) * norm; + a2 = (1 - K / band.Q + K * K) * norm; + break; + + case EQTypeHighPass: + norm = 1 / (1 + K / band.Q + K * K); + b0 = 1 * norm; + b1 = -2 * b0; + b2 = b0; + a1 = 2 * (K * K - 1) * norm; + a2 = (1 - K / band.Q + K * K) * norm; + break; + + case EQTypeBandPass: + norm = 1 / (1 + K / band.Q + K * K); + b0 = K / band.Q * norm; + b1 = 0; + b2 = -b0; + a1 = 2 * (K * K - 1) * norm; + a2 = (1 - K / band.Q + K * K) * norm; + break; + + case EQTypeNotch: + norm = 1 / (1 + K / band.Q + K * K); + b0 = (1 + K * K) * norm; + b1 = 2 * (K * K - 1) * norm; + b2 = b0; + a1 = b1; + a2 = (1 - K / band.Q + K * K) * norm; + break; + + case EQTypePeak: + if (band.gain >= 0) { // boost + norm = 1 / (1 + 1/band.Q * K + K * K); + b0 = (1 + V/band.Q * K + K * K) * norm; + b1 = 2 * (K * K - 1) * norm; + b2 = (1 - V/band.Q * K + K * K) * norm; + a1 = b1; + a2 = (1 - 1/band.Q * K + K * K) * norm; + } + else { // cut + norm = 1 / (1 + V/band.Q * K + K * K); + b0 = (1 + 1/band.Q * K + K * K) * norm; + b1 = 2 * (K * K - 1) * norm; + b2 = (1 - 1/band.Q * K + K * K) * norm; + a1 = b1; + a2 = (1 - V/band.Q * K + K * K) * norm; + } + break; + case EQTypeLowShelf: + if (band.gain >= 0) { // boost + norm = 1 / (1 + sqrt(2) * K + K * K); + b0 = (1 + sqrt(2*V) * K + V * K * K) * norm; + b1 = 2 * (V * K * K - 1) * norm; + b2 = (1 - sqrt(2*V) * K + V * K * K) * norm; + a1 = 2 * (K * K - 1) * norm; + a2 = (1 - sqrt(2) * K + K * K) * norm; + } + else { // cut + norm = 1 / (1 + sqrt(2*V) * K + V * K * K); + b0 = (1 + sqrt(2) * K + K * K) * norm; + b1 = 2 * (K * K - 1) * norm; + b2 = (1 - sqrt(2) * K + K * K) * norm; + a1 = 2 * (V * K * K - 1) * norm; + a2 = (1 - sqrt(2*V) * K + V * K * K) * norm; + } + break; + case EQTypeHighShelf: + if (band.gain >= 0) { // boost + norm = 1 / (1 + sqrt(2) * K + K * K); + b0 = (V + sqrt(2*V) * K + K * K) * norm; + b1 = 2 * (K * K - V) * norm; + b2 = (V - sqrt(2*V) * K + K * K) * norm; + a1 = 2 * (K * K - 1) * norm; + a2 = (1 - sqrt(2) * K + K * K) * norm; + } + else { // cut + norm = 1 / (V + sqrt(2*V) * K + K * K); + b0 = (1 + sqrt(2) * K + K * K) * norm; + b1 = 2 * (K * K - 1) * norm; + b2 = (1 - sqrt(2) * K + K * K) * norm; + a1 = 2 * (K * K - V) * norm; + a2 = (V - sqrt(2*V) * K + K * K) * norm; + } + break; + } + +#ifdef FIXEDPOINT + b.b0 = b0 * INT16_MAX; + b.b1 = b1 * INT16_MAX; + b.b2 = b2 * INT16_MAX; + b.a1 = a1 * INT16_MAX; + b.a2 = a2 * INT16_MAX; +#else + b.b0 = b0; + b.b1 = b1; + b.b2 = b2; + b.a1 = a1; + b.a2 = a2; +#endif + + return b; +} + +static inline int16_t biquad_apply(BiQuad *element, int16_t sample) { +#ifdef FIXEDPOINT + int32_t result = ((sample * element->b0) >> 15) + element->x1; + + // clip output + int16_t out = result; + + if (result > INT16_MAX) { + out = INT16_MAX; + } + if (result < INT16_MIN) { + out = INT16_MIN; + } + + // save delayed state for recursion + element->x1 = ((sample * element->b1) >> 15) + element->x2 - ((element->a1 * out) >> 15); + element->x2 = ((sample * element->b2) >> 15) - ((element->a2 * out) >> 15); + + return out; +#else + // Calculate filter + biquad_type_t result = sample * element->b0 + element->x1; + element->x1 = sample * element->b1 + element->x2 - element->a1 * result; + element->x2 = sample * element->b2 - element->a2 * result; + + // Round and clip + result = round(result); + if (result > INT16_MAX) { + return INT16_MAX - 1; + } + if (result < INT16_MIN) { + return INT16_MIN + 1; + } + return (int16_t)result; +#endif +} +#endif + +AudioPipelineStatus filter_param_eq_push(AudioPipelineElement *self, AudioBuffer *buffer) { + FilterParamEQContext *context = (FilterParamEQContext *)self->ctx; + int16_t *buf = (int16_t *)buffer->data; + + for (uint32_t s = 0; s < buffer->buf_size / 2; s+= self->channels) { + for (uint8_t ch = 0; ch < self->channels; ch++) { + int16_t result = buf[s + ch]; + for (uint8_t band = 0; band < context->numBands; band++) { + result = biquad_apply(&context->elements[ch][band], result); + } + buf[s + ch] = result; + } + } + + // run next element of the pipeline + return self->next->push(self->next, buffer); +} + +AudioPipelineStatus filter_param_eq_link(AudioPipelineElement *self, AudioPipelineElement *source) { + FilterParamEQContext *context = (FilterParamEQContext *)self->ctx; + + if (source->bits_per_sample != 16) { + fprintf(stderr, "ERROR: Parametric EQ only works on 16 bits/sample\n"); + return PipelineError; + } + self->sample_rate = source->sample_rate; + self->bits_per_sample = source->bits_per_sample; + self->channels = source->channels; + + // Initialize Coefficients and zero out delay line + if (context->elements != NULL) { + free(context->elements); + context->elements = NULL; + } + context->elements = calloc(self->channels, sizeof(BiQuad *)); + for (uint8_t ch = 0; ch < self->channels; ch++) { + context->elements[ch] = calloc(context->numBands, sizeof(BiQuad)); + for (uint8_t band = 0; band < context->numBands; band++) { + context->elements[ch][band] = biquad_initialize(context->bands[band], self->sample_rate); + } + } + + // link next element + source->next = self; + if (self->next != NULL) { + // relinking after samplerate change + return self->next->link(self->next, self); + } + return PipelineStopped; +} + +char *filter_param_eq_describe(AudioPipelineElement *self) { + return "biquad parametric EQ"; +} + +void filter_param_eq_destroy(AudioPipelineElement *self) { + FilterParamEQContext *context = (FilterParamEQContext *)self->ctx; + free(context); + free(self); +} + +AudioPipelineElement *audio_filter_param_eq(EQBand *bands, int numBands, float prescale) { + AudioPipelineElement *self = calloc(1, sizeof(AudioPipelineElement)); + FilterParamEQContext *context = calloc(1, sizeof(FilterParamEQContext)); + + context->bands = malloc(numBands * sizeof(EQBand)); + memcpy(context->bands, bands, numBands * sizeof(EQBand)); + for (uint8_t band = 0; band < numBands; band++) { + context->bands[band].gain += prescale; + } + context->numBands = numBands; + context->elements = NULL; + + self->ctx = context; + self->describe = filter_param_eq_describe; + self->start = filter_start_nop; + self->reset = filter_reset_nop; + self->stop = filter_stop_nop; + self->push = filter_param_eq_push; + self->link = filter_param_eq_link; + self->destroy = filter_param_eq_destroy; + self->type = AudioElementFilter; + + return self; +} diff --git a/audio/include/audio_filter_param_eq.h b/audio/include/audio_filter_param_eq.h new file mode 100644 index 0000000..a7c77c1 --- /dev/null +++ b/audio/include/audio_filter_param_eq.h @@ -0,0 +1,39 @@ +#ifndef AUDIOPIPELINE_AUDIO_FILTER_PARAM_EQ_H__INCLUDED +#define AUDIOPIPELINE_AUDIO_FILTER_PARAM_EQ_H__INCLUDED + +#include "audio.h" + +typedef enum _EQBandType { + EQTypeLowPass, /** simple low pass filter */ + EQTypeLowShelf, /** low shelf filter, basically all values before the center frequency get modified by the gain*/ + EQTypeBandPass, /** only frequencies around the center frequency will pass through */ + EQTypePeak, /** amplify frequencies around the center frequency */ + EQTypeNotch, /** duck frequencies around the center frequency */ + EQTypeHighPass, /** simple high pass filter */ + EQTypeHighShelf /** high shelf, like the low shelf filter but affects all frequencies after the center */ +} EQBandType; + +typedef struct _EQBand { + EQBandType type; /** type of filter for this band */ + float frequency; /** EQ band center frequency */ + float Q; /** Filter bandwidth, not used by shelf filters */ + float gain; /** Only used for Peak, Notch and Shelf filters*/ +} EQBand; + +/** + * Creates a filter element which applies a parametric EQ to the audio samples + * + * @param bands Array of EQ bands to apply, start with low shelf and end on high shelf if possible + * @param numBands number of bands in the array + * @param prescale prescale all bands by this value + * + * @returns Initialized `AudioPipelineElement` that can be used in call to `audio_pipeline_assemble` + */ +AudioPipelineElement *audio_filter_param_eq(EQBand *bands, int numBands, float prescale); + +#undef SECOND_ORDER_FILTER /* use a second biquad stage for steeper cutoff frequencies, needs more processing power */ +#undef FIXEDPOINT /* run in fixed point mode, floats are used only on initialization, filter unstable below 300 Hz! */ +#define LOW_PRECISION /* run filter on float32 instead of double precision */ + +#endif /* AUDIOPIPELINE_AUDIO_FILTER_PARAM_EQ_H__INCLUDED */ + diff --git a/audio/include/todo/audio_filter_eq.h b/audio/include/todo/audio_filter_eq.h deleted file mode 100644 index e69de29..0000000