422 lines
15 KiB
C
422 lines
15 KiB
C
/***************************************************************************
|
|
*
|
|
* Copyright 2015-2019 BES.
|
|
* All rights reserved. All unpublished rights reserved.
|
|
*
|
|
* No part of this work may be used or reproduced in any form or by any
|
|
* means, or stored in a database or retrieval system, without prior written
|
|
* permission of BES.
|
|
*
|
|
* Use of this work is governed by a license granted by BES.
|
|
* This work contains confidential and proprietary information of
|
|
* BES. which is protected by copyright, trade secret,
|
|
* trademark and other intellectual property rights.
|
|
*
|
|
****************************************************************************/
|
|
#include "plat_types.h"
|
|
#include "tgt_hardware.h"
|
|
#include "filter_debug_trace.h"
|
|
#include "iir_process.h"
|
|
#include "eq_cfg.h"
|
|
#include "hw_codec_iir_process.h"
|
|
#include "hw_iir_process.h"
|
|
#include "math.h"
|
|
|
|
#define IIR_FIXED_Q (1<<27)
|
|
|
|
#define PI (3.14159265358979)
|
|
|
|
static HW_CODEC_IIR_CFG_T hw_codec_iir_eq;
|
|
static HW_IIR_CFG_T hw_iir_eq;
|
|
|
|
void iir_lowshelf_coefs_generate(float gain, float fn, float Q, float *coefs)
|
|
{
|
|
float A = (float)sqrtf(powf(10, gain / 20));
|
|
float w0 = 2 * PI*fn;
|
|
float alpha = (float)sinf(w0)/(2*Q);
|
|
|
|
float a0 = (A + 1) + (A - 1)*(float)cosf(w0) + 2 * (float)sqrtf(A)*alpha;
|
|
float a1 = (-2 * ((A - 1) + (A + 1)*(float)cosf(w0)));
|
|
float a2 = ((A + 1) + (A - 1)*(float)cosf(w0) - 2 * (float)sqrtf(A)*alpha);
|
|
float b0 = (A*((A + 1) - (A - 1)*(float)cosf(w0) + 2 *(float)sqrtf(A)*alpha));
|
|
float b1 = (2 * A*((A - 1) - (A + 1)*(float)cosf(w0)));
|
|
float b2 = (A*((A + 1) - (A - 1)*(float)cosf(w0) - 2 * (float)sqrtf(A)*alpha));
|
|
|
|
coefs[0] = 1;
|
|
coefs[1] = a1 / a0;
|
|
coefs[2] = a2 / a0;
|
|
coefs[3] = b0 / a0;
|
|
coefs[4] = b1 / a0;
|
|
coefs[5] = b2 / a0;
|
|
|
|
// IIR_TRACE(6,"fn = %f, gain = %f, Q = %f, A = %f, w0 = %f, alpha = %f", fn, gain, Q, A, w0, alpha);
|
|
// FILTER_DEBUG_TRACE(7,"[%s] %f, %f, %f, %f, %f, %f", __func__, (double)coefs[0], (double)coefs[1], (double)coefs[2], (double)coefs[3], (double)coefs[4], (double)coefs[5]);
|
|
}
|
|
|
|
void iir_highshelf_coefs_generate(float gain, float fn, float Q, float *coefs)
|
|
{
|
|
float A = (float)sqrtf(powf(10, gain / 20));
|
|
float w0 = 2 * PI*fn;
|
|
float alpha = (float)sinf(w0) / (2 * Q);
|
|
|
|
float a0 = (A + 1) - (A - 1)*(float)cosf(w0) + 2 * (float)sqrtf(A)*alpha; // a0
|
|
float a1 = (2 * ((A - 1) - (A + 1)*(float)cosf(w0))); // a1
|
|
float a2 = ((A + 1) - (A - 1)*(float)cosf(w0) - 2 * (float)sqrtf(A)*alpha); //a2
|
|
float b0 = (A*((A + 1) + (A - 1)*(float)cosf(w0) + 2 * (float)sqrtf(A)*alpha)); //b0
|
|
float b1 = (-2 * A*((A - 1) + (A + 1)*(float)cosf(w0))); // b1
|
|
float b2 = (A*((A + 1) + (A - 1)*(float)cosf(w0) - 2 * (float)sqrtf(A)*alpha)); // b2
|
|
|
|
coefs[0] = 1;
|
|
coefs[1] = a1 / a0;
|
|
coefs[2] = a2 / a0;
|
|
coefs[3] = b0 / a0;
|
|
coefs[4] = b1 / a0;
|
|
coefs[5] = b2 / a0;
|
|
|
|
// IIR_TRACE(6,"fn = %f, gain = %f, Q = %f, A = %f, w0 = %f, alpha = %f", fn, gain, Q, A, w0, alpha);
|
|
// FILTER_DEBUG_TRACE(7,"[%s] %f, %f, %f, %f, %f, %f", __func__, (double)coefs[0], (double)coefs[1], (double)coefs[2], (double)coefs[3], (double)coefs[4], (double)coefs[5]);
|
|
}
|
|
|
|
// fn: Normalized frequency
|
|
void iir_peak_notch_coefs_generate(float gain, float fn, float Q, float *coefs)
|
|
{
|
|
// float Fs = 1000.0 / 48000.0;
|
|
// float gain = -5.0;
|
|
// float fo = 1000.0;
|
|
// float Q = 0.7;
|
|
float A = (float)sqrtf(powf(10, gain/20));
|
|
float w0 = 2*PI*fn;
|
|
float alpha = (float)sinf(w0)/(2*Q);
|
|
|
|
float a0 = 1 + alpha/A;
|
|
float a1 = -2*(float)cosf(w0);
|
|
float a2 = 1 - alpha/A;
|
|
float b0 = 1 + alpha*A;
|
|
float b1 = -2*(float)cosf(w0);
|
|
float b2 = 1 - alpha*A;
|
|
|
|
coefs[0] = 1;
|
|
coefs[1] = a1 / a0;
|
|
coefs[2] = a2 / a0;
|
|
coefs[3] = b0 / a0;
|
|
coefs[4] = b1 / a0;
|
|
coefs[5] = b2 / a0;
|
|
|
|
// IIR_TRACE(6,"fn = %f, gain = %f, Q = %f, A = %f, w0 = %f, alpha = %f", fn, gain, Q, A, w0, alpha);
|
|
// FILTER_DEBUG_TRACE(7,"[%s] %f, %f, %f, %f, %f, %f", __func__, (double)coefs[0], (double)coefs[1], (double)coefs[2], (double)coefs[3], (double)coefs[4], (double)coefs[5]);
|
|
}
|
|
|
|
void iir_highpass_coefs_generate(float gain, float fn, float Q, float *coefs)
|
|
{
|
|
float w0 = 2 * PI*fn;
|
|
float alpha = (float)sinf(w0) / (2 * Q);
|
|
|
|
float a0 = 1 + alpha;
|
|
float a1 = -2 * (float)cosf(w0);
|
|
float a2 = 1 - alpha;
|
|
float b0 = (1 + (float)cosf(w0))/2;
|
|
float b1 = -(1 + (float)cosf(w0));
|
|
float b2 = (1 + (float)cosf(w0))/2;
|
|
|
|
coefs[0] = 1;
|
|
coefs[1] = a1/a0;
|
|
coefs[2] = a2/a0;
|
|
coefs[3] = b0/a0;
|
|
coefs[4] = b1/a0;
|
|
coefs[5] = b2/a0;
|
|
}
|
|
|
|
void iir_lowspass_coefs_generate(float gain, float fn, float Q, float *coefs)
|
|
{
|
|
float w0 = 2 * PI*fn;
|
|
float alpha = (float)sinf(w0) / (2 * Q);
|
|
|
|
float a0 = 1 + alpha;
|
|
float a1 = -2 * (float)cosf(w0);
|
|
float a2 = 1 - alpha;
|
|
float b0 = (1 - (float)cosf(w0))/2;
|
|
float b1 = 1 - (float)cosf(w0);
|
|
float b2 = (1 - (float)cosf(w0))/2;
|
|
|
|
coefs[0] = 1;
|
|
coefs[1] = a1/a0;
|
|
coefs[2] = a2/a0;
|
|
coefs[3] = b0/a0;
|
|
coefs[4] = b1/a0;
|
|
coefs[5] = b2/a0;
|
|
}
|
|
|
|
float iir_convert_db_to_multiple(float db)
|
|
{
|
|
return (float)powf(10, db/20);
|
|
}
|
|
|
|
void iir_coefs_generate(IIR_TYPE_T type, float gain, float fn, float Q, float *coefs)
|
|
{
|
|
// As it is meaningless to set an eq at or above fs/2, just skip it when we receive
|
|
// such request.
|
|
if (fn >= 0.5f)
|
|
{
|
|
coefs[0] = 1;
|
|
coefs[1] = 0;
|
|
coefs[2] = 0;
|
|
coefs[3] = 1;
|
|
coefs[4] = 0;
|
|
coefs[5] = 0;
|
|
|
|
FILTER_DEBUG_TRACE(1,"[%s] center frequency is above nyquist frequency, skip this band", __FUNCTION__);
|
|
return;
|
|
}
|
|
|
|
if (type == IIR_TYPE_LOW_SHELF)
|
|
{
|
|
iir_lowshelf_coefs_generate(gain, fn, Q, coefs);
|
|
}
|
|
else if (type == IIR_TYPE_PEAK)
|
|
{
|
|
iir_peak_notch_coefs_generate(gain, fn, Q, coefs);
|
|
}
|
|
else if (type == IIR_TYPE_HIGH_SHELF)
|
|
{
|
|
iir_highshelf_coefs_generate(gain, fn, Q, coefs);
|
|
}
|
|
else if (type == IIR_TYPE_LOW_PASS)
|
|
{
|
|
iir_lowspass_coefs_generate(gain, fn, Q, coefs);
|
|
}
|
|
else if (type == IIR_TYPE_HIGH_PASS)
|
|
{
|
|
iir_highpass_coefs_generate(gain, fn, Q, coefs);
|
|
}
|
|
else
|
|
{
|
|
ASSERT(false, "[%s] %d is not supported", __func__, type);
|
|
}
|
|
}
|
|
|
|
HW_CODEC_IIR_CFG_T *hw_codec_iir_get_cfg(enum AUD_SAMPRATE_T sample_rate, const IIR_CFG_T *ext_cfg)
|
|
{
|
|
const IIR_CFG_T *cfg;
|
|
float coefs_l[6],coefs_r[6];
|
|
float gain0,gain1;
|
|
int sample_rate_iir=sample_rate;
|
|
|
|
#if defined(CHIP_BEST2001)
|
|
#ifdef __AUDIO_RESAMPLE__
|
|
sample_rate_iir=50781.25f*8;
|
|
#else
|
|
switch(sample_rate)
|
|
{
|
|
case AUD_SAMPRATE_22050:
|
|
case AUD_SAMPRATE_44100:
|
|
case AUD_SAMPRATE_88200:
|
|
case AUD_SAMPRATE_176400:
|
|
case AUD_SAMPRATE_352800:
|
|
case AUD_SAMPRATE_705600:
|
|
sample_rate_iir=44100*8;
|
|
break;
|
|
|
|
default:
|
|
sample_rate_iir=48000*8 ;
|
|
break;
|
|
};
|
|
#endif
|
|
#endif
|
|
FILTER_DEBUG_TRACE(2,"%s,sample_rate:%d\n", __func__,sample_rate_iir);
|
|
|
|
hw_codec_iir_eq.iir_filtes_l.iir_bypass_flag=1;
|
|
hw_codec_iir_eq.iir_filtes_r.iir_bypass_flag=1;
|
|
|
|
if (ext_cfg)
|
|
{
|
|
cfg = ext_cfg;
|
|
}
|
|
else
|
|
{
|
|
FILTER_DEBUG_TRACE(0,"Error HW IIR CODEC cfg!");
|
|
return NULL;
|
|
}
|
|
|
|
// Check parameter
|
|
ASSERT(cfg->num <= AUD_DAC_IIR_NUM_EQ, "[%s] num(%d) is too large", __func__, cfg->num);
|
|
|
|
gain0 = iir_convert_db_to_multiple(cfg->gain0);
|
|
gain1 = iir_convert_db_to_multiple(cfg->gain1);
|
|
|
|
// FILTER_DEBUG_TRACE(3,"[%s] gain0 = %f, gain1 = %f", __func__, (double)gain0, (double)gain1);
|
|
FILTER_DEBUG_TRACE(2,"[%s] len = %d", __func__, cfg->num);
|
|
|
|
for(int i=0; i<cfg->num; i++)
|
|
{
|
|
if (cfg->param[i].fc<sample_rate_iir/2)
|
|
{
|
|
iir_coefs_generate(cfg->param[i].type,
|
|
cfg->param[i].gain,
|
|
cfg->param[i].fc / sample_rate_iir,
|
|
cfg->param[i].Q,
|
|
coefs_l);
|
|
}
|
|
else
|
|
{
|
|
FILTER_DEBUG_TRACE(2,"[%s]Error iir parameters i = %d", __func__, i);
|
|
coefs_l[0]=1.0f;
|
|
coefs_l[1]=0.0f;
|
|
coefs_l[2]=0.0f;
|
|
coefs_l[3]=1.0f;
|
|
coefs_l[4]=0.0f;
|
|
coefs_l[5]=0.0f;
|
|
}
|
|
|
|
coefs_r[0]=coefs_l[0];
|
|
coefs_r[1]=coefs_l[1];
|
|
coefs_r[2]=coefs_l[2];
|
|
coefs_r[3]=coefs_l[3];
|
|
coefs_r[4]=coefs_l[4];
|
|
coefs_r[5]=coefs_l[5];
|
|
|
|
if(i==0)
|
|
{
|
|
coefs_l[3] = coefs_l[3]*gain0;
|
|
coefs_l[4] = coefs_l[4]*gain0;
|
|
coefs_l[5] = coefs_l[5]*gain0;
|
|
|
|
coefs_r[3] = coefs_r[3]*gain1;
|
|
coefs_r[4] = coefs_r[4]*gain1;
|
|
coefs_r[5] = coefs_r[5]*gain1;
|
|
}
|
|
|
|
hw_codec_iir_eq.iir_filtes_l.iir_coef[i].coef_a[0]=(int32_t)((double)coefs_l[0]*(int32_t)IIR_FIXED_Q);
|
|
hw_codec_iir_eq.iir_filtes_l.iir_coef[i].coef_a[1]=(int32_t)((double)coefs_l[1]*(int32_t)IIR_FIXED_Q);
|
|
hw_codec_iir_eq.iir_filtes_l.iir_coef[i].coef_a[2]=(int32_t)((double)coefs_l[2]*(int32_t)IIR_FIXED_Q);
|
|
hw_codec_iir_eq.iir_filtes_l.iir_coef[i].coef_b[0]=(int32_t)((double)coefs_l[3]*(int32_t)IIR_FIXED_Q);
|
|
hw_codec_iir_eq.iir_filtes_l.iir_coef[i].coef_b[1]=(int32_t)((double)coefs_l[4]*(int32_t)IIR_FIXED_Q);
|
|
hw_codec_iir_eq.iir_filtes_l.iir_coef[i].coef_b[2]=(int32_t)((double)coefs_l[5]*(int32_t)IIR_FIXED_Q);
|
|
|
|
hw_codec_iir_eq.iir_filtes_r.iir_coef[i].coef_a[0]=(int32_t)((double)coefs_r[0]*(int32_t)IIR_FIXED_Q);
|
|
hw_codec_iir_eq.iir_filtes_r.iir_coef[i].coef_a[1]=(int32_t)((double)coefs_r[1]*(int32_t)IIR_FIXED_Q);
|
|
hw_codec_iir_eq.iir_filtes_r.iir_coef[i].coef_a[2]=(int32_t)((double)coefs_r[2]*(int32_t)IIR_FIXED_Q);
|
|
hw_codec_iir_eq.iir_filtes_r.iir_coef[i].coef_b[0]=(int32_t)((double)coefs_r[3]*(int32_t)IIR_FIXED_Q);
|
|
hw_codec_iir_eq.iir_filtes_r.iir_coef[i].coef_b[1]=(int32_t)((double)coefs_r[4]*(int32_t)IIR_FIXED_Q);
|
|
hw_codec_iir_eq.iir_filtes_r.iir_coef[i].coef_b[2]=(int32_t)((double)coefs_r[5]*(int32_t)IIR_FIXED_Q);
|
|
|
|
}
|
|
|
|
/*
|
|
FILTER_DEBUG_TRACE(2,"\n[%s] coef_a = %d\n", __func__, hw_codec_iir_eq.iir_filtes_l.iir_coef[0].coef_a[0]);
|
|
FILTER_DEBUG_TRACE(2,"[%s] coef_a = %d\n", __func__, hw_codec_iir_eq.iir_filtes_l.iir_coef[0].coef_a[1]);
|
|
FILTER_DEBUG_TRACE(2,"[%s] coef_a = %d\n", __func__, hw_codec_iir_eq.iir_filtes_l.iir_coef[0].coef_a[2]);
|
|
FILTER_DEBUG_TRACE(2,"[%s] coef_a = %d\n", __func__, hw_codec_iir_eq.iir_filtes_l.iir_coef[0].coef_b[0]);
|
|
FILTER_DEBUG_TRACE(2,"[%s] coef_a = %d\n", __func__, hw_codec_iir_eq.iir_filtes_l.iir_coef[0].coef_b[1]);
|
|
FILTER_DEBUG_TRACE(2,"[%s] coef_a = %d\n", __func__, hw_codec_iir_eq.iir_filtes_l.iir_coef[0].coef_b[2]);
|
|
*/
|
|
if(cfg->num>=0)
|
|
{
|
|
hw_codec_iir_eq.iir_filtes_l.iir_bypass_flag=0;
|
|
hw_codec_iir_eq.iir_filtes_r.iir_bypass_flag=0;
|
|
|
|
hw_codec_iir_eq.iir_filtes_l.iir_counter=cfg->num;
|
|
hw_codec_iir_eq.iir_filtes_r.iir_counter=cfg->num;
|
|
}
|
|
|
|
return &hw_codec_iir_eq;
|
|
}
|
|
|
|
HW_IIR_CFG_T *hw_iir_get_cfg(enum AUD_SAMPRATE_T sample_rate, const IIR_CFG_T* ext_cfg)
|
|
{
|
|
const IIR_CFG_T *cfg;
|
|
float coefs_l[6],coefs_r[6];
|
|
float gain0,gain1;
|
|
|
|
if (ext_cfg)
|
|
{
|
|
cfg = ext_cfg;
|
|
}
|
|
else
|
|
{
|
|
FILTER_DEBUG_TRACE(0,"Error HW IIR cfg!");
|
|
return NULL;
|
|
}
|
|
|
|
FILTER_DEBUG_TRACE(3,"[%s]sample_rate:%d,iir num:%d.", __func__,sample_rate,cfg->num);
|
|
|
|
// Check parameter
|
|
ASSERT(cfg->num <= AUD_IIR_NUM_EQ, "[%s] num(%d) is too large", __func__, cfg->num);
|
|
|
|
gain0 = iir_convert_db_to_multiple(cfg->gain0);
|
|
gain1 = iir_convert_db_to_multiple(cfg->gain1);
|
|
|
|
// FILTER_DEBUG_TRACE(3,"[%s] gain0 = %f, gain1 = %f", __func__, (double)gain0, (double)gain1);
|
|
|
|
for(int i=0; i<cfg->num; i++)
|
|
{
|
|
if (cfg->param[i].fc<sample_rate/2)
|
|
{
|
|
iir_coefs_generate(cfg->param[i].type,
|
|
cfg->param[i].gain,
|
|
cfg->param[i].fc / sample_rate,
|
|
cfg->param[i].Q,
|
|
coefs_l);
|
|
}
|
|
else
|
|
{
|
|
FILTER_DEBUG_TRACE(2,"[%s]Error iir parameters i = %d", __func__, i);
|
|
coefs_l[0]=1.0f;
|
|
coefs_l[1]=0.0f;
|
|
coefs_l[2]=0.0f;
|
|
coefs_l[3]=1.0f;
|
|
coefs_l[4]=0.0f;
|
|
coefs_l[5]=0.0f;
|
|
}
|
|
|
|
coefs_r[0]=coefs_l[0];
|
|
coefs_r[1]=coefs_l[1];
|
|
coefs_r[2]=coefs_l[2];
|
|
coefs_r[3]=coefs_l[3];
|
|
coefs_r[4]=coefs_l[4];
|
|
coefs_r[5]=coefs_l[5];
|
|
|
|
if(i==0)
|
|
{
|
|
coefs_l[3] = coefs_l[3]*gain0;
|
|
coefs_l[4] = coefs_l[4]*gain0;
|
|
coefs_l[5] = coefs_l[5]*gain0;
|
|
|
|
coefs_r[3] = coefs_r[3]*gain1;
|
|
coefs_r[4] = coefs_r[4]*gain1;
|
|
coefs_r[5] = coefs_r[5]*gain1;
|
|
}
|
|
|
|
hw_iir_eq.iir_filtes_l.iir_coef[i].coef_a[0]=(int32_t)((double)coefs_l[0]*(int32_t)IIR_FIXED_Q);
|
|
hw_iir_eq.iir_filtes_l.iir_coef[i].coef_a[1]=(int32_t)((double)coefs_l[1]*(int32_t)IIR_FIXED_Q);
|
|
hw_iir_eq.iir_filtes_l.iir_coef[i].coef_a[2]=(int32_t)((double)coefs_l[2]*(int32_t)IIR_FIXED_Q);
|
|
hw_iir_eq.iir_filtes_l.iir_coef[i].coef_b[0]=(int32_t)((double)coefs_l[3]*(int32_t)IIR_FIXED_Q);
|
|
hw_iir_eq.iir_filtes_l.iir_coef[i].coef_b[1]=(int32_t)((double)coefs_l[4]*(int32_t)IIR_FIXED_Q);
|
|
hw_iir_eq.iir_filtes_l.iir_coef[i].coef_b[2]=(int32_t)((double)coefs_l[5]*(int32_t)IIR_FIXED_Q);
|
|
|
|
hw_iir_eq.iir_filtes_r.iir_coef[i].coef_a[0]=(int32_t)((double)coefs_r[0]*(int32_t)IIR_FIXED_Q);
|
|
hw_iir_eq.iir_filtes_r.iir_coef[i].coef_a[1]=(int32_t)((double)coefs_r[1]*(int32_t)IIR_FIXED_Q);
|
|
hw_iir_eq.iir_filtes_r.iir_coef[i].coef_a[2]=(int32_t)((double)coefs_r[2]*(int32_t)IIR_FIXED_Q);
|
|
hw_iir_eq.iir_filtes_r.iir_coef[i].coef_b[0]=(int32_t)((double)coefs_r[3]*(int32_t)IIR_FIXED_Q);
|
|
hw_iir_eq.iir_filtes_r.iir_coef[i].coef_b[1]=(int32_t)((double)coefs_r[4]*(int32_t)IIR_FIXED_Q);
|
|
hw_iir_eq.iir_filtes_r.iir_coef[i].coef_b[2]=(int32_t)((double)coefs_r[5]*(int32_t)IIR_FIXED_Q);
|
|
|
|
}
|
|
|
|
/*
|
|
FILTER_DEBUG_TRACE(2,"\n[%s] coef_a0 = %d\n", __func__, hw_iir_eq.iir_filtes_l.iir_coef[0].coef_a[0]);
|
|
FILTER_DEBUG_TRACE(2,"[%s] coef_a1 = %d\n", __func__, hw_iir_eq.iir_filtes_l.iir_coef[0].coef_a[1]);
|
|
FILTER_DEBUG_TRACE(2,"[%s] coef_a2 = %d\n", __func__, hw_iir_eq.iir_filtes_l.iir_coef[0].coef_a[2]);
|
|
FILTER_DEBUG_TRACE(2,"[%s] coef_b0 = %d\n", __func__, hw_iir_eq.iir_filtes_l.iir_coef[0].coef_b[0]);
|
|
FILTER_DEBUG_TRACE(2,"[%s] coef_b1 = %d\n", __func__, hw_iir_eq.iir_filtes_l.iir_coef[0].coef_b[1]);
|
|
FILTER_DEBUG_TRACE(2,"[%s] coef_b2 = %d\n", __func__, hw_iir_eq.iir_filtes_l.iir_coef[0].coef_b[2]);
|
|
*/
|
|
if(cfg->num>=0)
|
|
{
|
|
hw_iir_eq.iir_filtes_l.iir_counter=cfg->num;
|
|
hw_iir_eq.iir_filtes_r.iir_counter=cfg->num;
|
|
}
|
|
|
|
return &hw_iir_eq;
|
|
}
|
|
|