/*
* iaxc_preprocess.c
*
* Copyrights:
* Copyright (C) 2005, Tipic, Inc.
*
* Contributors:
* Francesco Delfino <pluto@tipic.com>
*
*
* This program is free software, distributed under the terms of
* the GNU Lesser (Library) General Public License
*/
//#define USE_AEC_NLMS
#include "iaxclient_lib.h"
#include "pablio.h"
#ifdef USE_AEC_NLMS
#include "aec_nlms/aec_cwrap.h"
#endif
#ifndef WIN32
#define min(x,y) (x) >= (y) ? (y) : (x)
#endif
// Robert Bristow-Johnson 's Cookbook formulae for audio EQ biquad filter coefficients
struct biquad_filter
{
float b0, b1, b2, a0, a1, a2, x1, x2, y1, y2;
};
struct preprocess_state {
// Freeing this pointer, frees all the buffers
char *ptr_to_free ;
SpeexEchoState *st1 ;
SpeexEchoState *st2 ;
SpeexEchoState *st3 ;
SpeexPreprocessState *den ;
#ifdef WIN32
long long pref, pe1, pe2, pe3, penlms;
#else
long pref, pe1, pe2, pe3, penlms;
#endif
#ifdef USE_AEC_NLMS
struct aec_nlms *nlms;
#endif
// in/out buffers
char *outRingBuf;
char *inRingBuf;
short *echo_buf, *ref_buf, *e_buf, *e1_buf, *e2_buf, *e3_buf, *e_nlms;
float *noise1, *noise2, *noise3;
float /* *e_buf, */ *noise;
RingBuffer outRing;
RingBuffer inRing;
//bias
long in_bias, out_bias;
//bass_boost filter
struct biquad_filter out_bass_boost;
};
static int compute_biquad_filter(struct biquad_filter *f, short *buffer, int n)
{
/* initialise the filter */
float out, in = 0;
int i;
for (i = 0; i < n; i++) {
in = buffer[i];
out = (f->b0 * in + f->b1 * f->x1 + f->b2 * f->x2 - f->a1 * f->y1 - f->a2 * f->y2) / f->a0;
f->x2 = f->x1;
f->x1 = in;
f->y2 = f->y1;
f->y1 = out;
if (out < -32767)
out = (float)-32767;
else if (out > 32768)
out = (float)32768; //Prevents clipping
buffer[i] = out ;
}
return 0;
}
static int init_lowpass(struct biquad_filter *f,float frequency, float sample_rate)
{
float omega, sn, cs, alpha;
/* Compute parameters */
omega = 2 * 3.141592653589 * frequency / (sample_rate);
sn = sin(omega);
cs = cos(omega);
alpha = sn / 2;
/* Init coefficients */
f->b0 = (1- cs)/2;
f->b1 = 1-cs;
f->b2 = (1-cs)/2;
f->a0 = 1 + alpha;
f->a1 = -2 * cs;
f->a2 = 1 - alpha;
return 0;
}
static int init_bass_boost(struct biquad_filter *f,float frequency, float dB_boost, float sample_rate)
{
float omega, sn, cs, A, shape, beta;
if (dB_boost == 0) {
f->b0 = 1;
f->b1 = f->b2 = 0;
f->a0 = 1;
f->a1 = f->a2 = 0;
}
else {
/* Compute parameters */
omega = 2 * 3.141592653589 * frequency / (sample_rate);
sn = sin(omega);
cs = cos(omega);
A = exp(log(10.0f) * dB_boost / 40);
shape = (float)1.0;
beta = sqrt((A * A + 1) / shape - (pow((A - 1), 2)));
/* Init coefficients */
f->b0 = A * ((A + 1) - (A - 1) * cs + beta * sn);
f->b1 = 2 * A * ((A - 1) - (A + 1) * cs);
f->b2 = A * ((A + 1) - (A - 1) * cs - beta * sn);
f->a0 = ((A+ 1) + (A - 1) * cs + beta * sn);
f->a1 = -2 * ((A - 1) + (A + 1) * cs);
f->a2 = (A + 1) + (A - 1) * cs - beta * sn;
}
return 0;
}
#ifdef _MSC_VER
#define open _open
#define write _write
#define O_RDONLY _O_RDONLY
#define O_WRONLY _O_WRONLY
#define O_CREAT _O_CREAT
#define O_TRUNC _O_TRUNC
#define O_BINARY _O_BINARY
#endif
static void iaxc_preprocessor_reset(struct iaxc_preprocessor *c, int filters)
{
int i;
float f;
struct preprocess_state *s = c->state;
//i = 1; /* always make VAD decision */
//speex_preprocess_ctl(s->den, SPEEX_PREPROCESS_SET_VAD, &i);
i = (filters & IAXC_FILTER_AGC) ? 1 : 0;
speex_preprocess_ctl(s->den, SPEEX_PREPROCESS_SET_AGC, &i);
i = (filters & IAXC_FILTER_DENOISE) ? 1 : 0;
speex_preprocess_ctl(s->den, SPEEX_PREPROCESS_SET_DENOISE, &i);
if ( !(c->filters & IAXC_FILTER_ECHO) && (filters & IAXC_FILTER_ECHO) )
{
speex_echo_state_reset(s->st1);
speex_echo_state_reset(s->st2);
speex_echo_state_reset(s->st3);
}
if ( (c->filters & IAXC_FILTER_BASS_BOOST) != (filters & IAXC_FILTER_BASS_BOOST) )
{
if (filters & IAXC_FILTER_BASS_BOOST)
init_bass_boost(&(s->out_bass_boost), 400, 6, c->bitrate);
else
init_bass_boost(&(s->out_bass_boost), 400, 0, c->bitrate);
//init_lowpass(&(s->out_bass_boost), 8000, c->bitrate);
}
c->filters = filters;
}
static void iaxc_preprocessor_destroy(struct iaxc_preprocessor *c)
{
if (c->state) {
struct preprocess_state *s = c->state;
if (s->ptr_to_free)
{
free(s->ptr_to_free);
s->ptr_to_free = 0;
}
if (s->st1) {
speex_echo_state_destroy(s->st1);
s->st1 = NULL;
}
if (s->st2) {
speex_echo_state_destroy(s->st2);
s->st2 = NULL;
}
if (s->st3) {
speex_echo_state_destroy(s->st3);
s->st3 = NULL;
}
if (s->den) {
speex_preprocess_state_destroy(s->den);
s->den = NULL;
}
}
free(c->state);
free(c);
}
static void iaxc_preprocessor_preprocess(struct iaxc_preprocessor *c, short *inputBuffer, short *outputBuffer, int n, void *udata)
{
struct preprocess_state *s = c->state;
int i;
if (inputBuffer!=NULL) {
for(i = 0; i < n; i++) {
s->in_bias += ((((long)inputBuffer[i]) << 15) - s->in_bias) >> 14;
inputBuffer[i] -= (s->in_bias >> 15);
}
RingBuffer_Write(&(s->inRing), inputBuffer, n * sizeof(short) );
}
if (outputBuffer!=NULL) {
for(i = 0; i < n; i++) {
s->out_bias += ((((long)outputBuffer[i]) << 15) - s->out_bias) >> 14;
outputBuffer[i] -= (s->out_bias >> 15);
//if(c->filters & IAXC_FILTER_BASS_BOOST)
// outputBuffer[i] /= 15;
}
compute_biquad_filter(&(s->out_bass_boost), outputBuffer, n);
RingBuffer_Write(&(s->outRing), outputBuffer, n * sizeof(short) );
}
if (inputBuffer) {
int inLen =RingBuffer_GetReadAvailable(&(s->inRing));
int outLen = RingBuffer_GetReadAvailable(&(s->outRing));
float maxgain = 1, maxgain1 = 1, maxgain2 = 1, maxgain3 = 1;
while (inLen >= c->NN*sizeof(short) && outLen >= c->NN*sizeof(short))
{
RingBuffer_Read(&(s->inRing),s->ref_buf, c->NN*sizeof(short));
RingBuffer_Read(&(s->outRing),s->echo_buf, c->NN*sizeof(short));
if (c->filters & IAXC_FILTER_ECHO) {
float pe1= 0, pe2 = 0, pe3 =0, pref = 0;
speex_echo_cancel(s->st1, s->ref_buf, s->echo_buf, s->e1_buf, s->noise1);
//speex_echo_cancel(s->st2, s->ref_buf, s->echo_buf, s->e2_buf, s->noise2);
speex_echo_cancel(s->st3, s->ref_buf, s->echo_buf, s->e3_buf, s->noise3);
for(i=0; i<c->NN; i++)
pe1 += s->e1_buf[i]*s->e1_buf[i];
//for(i=0; i<c->NN; i++)
// pe2 += s->e2_buf[i]*s->e2_buf[i];
for(i=0; i<c->NN; i++)
pe3 += s->e3_buf[i]*s->e3_buf[i];
for(i=0; i<c->NN; i++)
pref += s->ref_buf[i]*s->ref_buf[i];
s->pe1 = c->power_decay * s->pe1 + (1- c->power_decay) * pe1;
//s->pe2 = c->power_decay * s->pe2 + (1- c->power_decay) * pe2;
s->pe3 = c->power_decay * s->pe3 + (1- c->power_decay) * pe3;
s->pref = c->power_decay * s->pref + (1- c->power_decay) * pref;
maxgain1 = 1 + 1/fabs(((double)s->pref-s->pe1) / s->pe1) * (sqrt(s->pe1)/c->NN) / 32768;
maxgain2 = maxgain1;// 1 + 1/fabs(((double)s->pref-s->pe2) / s->pe2) * (sqrt(s->pe2)/c->NN) / 32768;
maxgain3 = 1 + 1/fabs(((double)s->pref-s->pe3) / s->pe3) * (sqrt(s->pe3)/c->NN) / 32768;
maxgain = min( maxgain1, min(maxgain2,maxgain3));
speex_preprocess_ctl(s->den, SPEEX_PREPROCESS_SET_AGC_MAXGAIN, &maxgain);
if (maxgain == maxgain1)
{
s->e_buf = s->e1_buf; s->noise = s->noise1;
} /*else if (maxgain == maxgain2)
{
s->e_buf = s->e2_buf; s->noise = s->noise2;
} else if (maxgain == maxgain3) */
{
s->e_buf = s->e3_buf; s->noise = s->noise3;
}
speex_preprocess(s->den, s->e_buf, s->noise);
#ifdef USE_AEC_NLMS
s->nlms->setambientdb(s->nlms, -50 );
s->nlms->process(s->nlms, s->echo_buf, s->ref_buf, c->NN);
#endif
c->callback( udata, s->e_buf, c->NN );
}
else {
maxgain = 200; //this is the maximum gain hard-coded in speex
speex_preprocess_ctl(s->den, SPEEX_PREPROCESS_SET_AGC_MAXGAIN, &maxgain);
speex_preprocess(s->den, s->ref_buf, NULL);
c->callback( udata, s->ref_buf, c->NN );
}
inLen =RingBuffer_GetReadAvailable(&(s->inRing));
outLen = RingBuffer_GetReadAvailable(&(s->outRing));
}
}
}
struct iaxc_preprocessor *iaxc_preprocessor_initialize(int NN, int delay, int echo_tail, int bitrate, float power_decay, void (*callback) ( void *udata, short *inputBuffer, int n ) )
{
struct iaxc_preprocessor *c = calloc(sizeof(struct iaxc_preprocessor),1);
struct preprocess_state *s = NULL;
int ring_buffer_size = 1;
char *tmpptr;
if(!c) return NULL;
// init base struct
c->destroy = iaxc_preprocessor_destroy;
c->reset = iaxc_preprocessor_reset;
c->preprocess = iaxc_preprocessor_preprocess;
c->callback = callback;
c->NN = NN;
c->delay = delay;
c->echo_tail = echo_tail;
c->bitrate = bitrate;
c->power_decay = power_decay;
s = calloc(sizeof(struct preprocess_state),1);
if(!s) {
c->destroy(c);
return NULL;
}
c->state = s;
//create memory for all the buffer with just one call
while(ring_buffer_size < 4*delay)
ring_buffer_size *=2;
s->ptr_to_free = calloc(
2 * ring_buffer_size * sizeof(short)+ // inRingBuf && outRingBuf
5 * NN * sizeof(short)+ // echo_buf,ref_buf, e_buf, e2_buf, e3_buf
NN * sizeof(short)+ // e_nlms
3 * (NN+1)* sizeof(float) //noise1, noise2, noise3
, 1);
if (s->ptr_to_free==NULL) {
c->destroy(c);
return NULL;
}
// Init buffers
tmpptr = s->ptr_to_free ;
s->outRingBuf = tmpptr;
tmpptr +=ring_buffer_size*sizeof(short);
s->inRingBuf = tmpptr;
tmpptr +=ring_buffer_size*sizeof(short);
s->echo_buf = (short *)tmpptr;
tmpptr +=(NN)*sizeof(short);
s->ref_buf = (short *)tmpptr;
tmpptr +=(NN)*sizeof(short);
s->e1_buf = (short *)tmpptr;
tmpptr +=(NN)*sizeof(short);
s->e2_buf = (short *)tmpptr;
tmpptr +=(NN)*sizeof(short);
s->e3_buf = (short *)tmpptr;
tmpptr +=(NN)*sizeof(short);
s->e_nlms = (short *)tmpptr;
tmpptr +=(NN)*sizeof(short);
s->noise1 = (float *)tmpptr;
tmpptr +=(NN+1)*sizeof(float);
s->noise2 = (float *)tmpptr;
tmpptr +=(NN+1)*sizeof(float);
s->noise3 = (float *)tmpptr;
tmpptr +=(NN+1)*sizeof(float);
RingBuffer_Init(&(s->outRing), ring_buffer_size, s->outRingBuf);
memset(s->inRingBuf, 0, (delay) * sizeof(short) );
RingBuffer_Write(&(s->outRing), s->inRingBuf, (delay) * sizeof(short) );
RingBuffer_Init(&(s->inRing), ring_buffer_size, (s->inRingBuf));
// Init speex structs
s->st1 = speex_echo_state_init(NN, echo_tail);
s->st2 = speex_echo_state_init(NN, echo_tail * 3);
s->st3 = speex_echo_state_init(NN, echo_tail * 9);
#ifdef USE_AEC_NLMS
s->nlms = aec_nlms_init();
#endif
s->den = speex_preprocess_state_init(NN, bitrate);
if (!s->st1 || !s->st2 || !s->st3 || !s->den)
{
c->destroy(c);
return NULL;
}
// initing bass boost struture to off
init_bass_boost(&(s->out_bass_boost),200,0,bitrate);
return c;
}
/*
#include "fcntl.h"
#ifdef _MSC_VER
#define open _open
#define write _write
#define O_RDONLY _O_RDONLY
#define O_WRONLY _O_WRONLY
#define O_CREAT _O_CREAT
#define O_TRUNC _O_TRUNC
#define O_BINARY _O_BINARY
#define longlong __int64
#endif
#define NN 160
#define DELAY (NN*12/2)
int iaxc_echo_can_reset = 0;
static void iaxc_echo_can (short *inputBuffer, short *outputBuffer, int n, RingBuffer *resultBuffer)
{
static RingBuffer outRing;
static char outRingBuf[EC_RING_SZ];
static RingBuffer inRing;
static char inRingBuf[EC_RING_SZ];
FILE *echo_fd, *ref_fd, *e_fd, *net_fd;
float noise[NN+1];
short echo_buf[NN], ref_buf[NN], e_buf[NN], net_buf[NN];
static SpeexEchoState *st = NULL;
static SpeexPreprocessState *den = NULL;
static SpeexPreprocessState *den2 = NULL;
static longlong pe=0, pref=0;
int i;
if (iaxc_echo_can_reset == TRUE)
{
if (st!=NULL) {
speex_echo_state_destroy(st);
st = NULL;
}
if (den!=NULL) {
speex_preprocess_state_destroy(den);
den = NULL;
}
iaxc_echo_can_reset = FALSE;
}
if (st == NULL ) {
st = speex_echo_state_init(NN, 6 * NN);
RingBuffer_Init(&outRing, EC_RING_SZ, &outRingBuf);
// use zero-ed inRingBuf to put some data on the outBuffer
memset(inRingBuf, 0, (DELAY) * sizeof(short) );
RingBuffer_Write(&outRing, inRingBuf, (DELAY) * sizeof(short) );
RingBuffer_Init(&inRing, EC_RING_SZ, &inRingBuf);
remove("speaker_last.sw");
remove("mic_last.sw");
remove("cancelled_last.sw");
remove("net_last.sw");
rename("mic.sw","mic_last.sw");
rename("speaker.sw","speaker_last.sw");
rename("cancelled.sw","cancelled_last.sw");
rename("net.sw","net_last.sw");
}
if (den == NULL ) {
den = speex_preprocess_state_init(NN, 8000);
i = 1;
speex_preprocess_ctl(den, SPEEX_PREPROCESS_SET_DENOISE, &i);
i = 1;
speex_preprocess_ctl(den, SPEEX_PREPROCESS_SET_AGC, &i);
}
if (inputBuffer!=NULL)
RingBuffer_Write(&inRing, inputBuffer, n * sizeof(short) );
if (outputBuffer!=NULL)
RingBuffer_Write(&outRing, outputBuffer, n * sizeof(short) );
if (inputBuffer) {
int inLen =RingBuffer_GetReadAvailable(&inRing);
int outLen = RingBuffer_GetReadAvailable(&outRing);
static int count = 1;
static int diff = 0 ;
static int meanIn = 0 ;
static int meanOut = 0 ;
while (inLen >= NN*sizeof(short) && outLen >= NN*sizeof(short))
{
ref_fd = fopen ("mic.sw", "ab");
echo_fd = fopen ("speaker.sw", "ab" );
e_fd = fopen ("cancelled.sw", "ab" );
net_fd = fopen ("net.sw", "ab" );
RingBuffer_Read(&inRing,ref_buf, NN*sizeof(short));
//memcpy(ref_buf, tmp_buf, NN*sizeof(short));
//memcpy(tmp_buf, echo_buf, NN*sizeof(short));
RingBuffer_Read(&outRing,echo_buf, NN*sizeof(short));
//speex_preprocess(den2, ref_buf, NULL);
speex_echo_cancel(st, ref_buf, echo_buf, e_buf, noise);
//speex_preprocess(den, e_buf, noise);
{
float maxgain = 1;
pe /= 1.1;
pref /= 1.1;
for(i=0; i<NN; i++)
pe += e_buf[i]*e_buf[i];
for(i=0; i<NN; i++)
pref += ref_buf[i]*ref_buf[i];
maxgain = 1 + 2 * 1/fabs(((double)pref-pe) / pe) * (sqrt(pe)/NN) / 32768;
speex_preprocess_ctl(den, SPEEX_PREPROCESS_SET_AGC_MAXGAIN, &maxgain);
/*if ( (sqrt(pe)/NN)<(32768/512) || ( ((double)pref-pe) / pe) >= 0.5 )
{
}
else
{
i = 1;
speex_preprocess_ctl(den, SPEEX_PREPROCESS_SET_AGC, &i);
}*/
/* for (i=0; i<NN; i++)
net_buf[i] = (sqrt(pref)-sqrt(pe))/sqrt(pe)*4096;*//*
meanIn += inLen / sizeof(short);
meanOut += outLen / sizeof(short);
diff += abs(inLen-outLen) / sizeof(short);
if (count%100==0) {
printf("> samples: diff %f (in: %f out:%f) pe: %f pref: %f pdiff: %f pratio:%f\n", diff / 100.0, meanIn / 100.0, meanOut / 100.0, (double)pe, (double)pref, (double)(pref-pe), ((double)(pref-pe)/(pe+1)) );
diff = 0;
meanIn = 0;
meanOut = 0;
}
count ++;
}
speex_preprocess(den, e_buf, noise);
fwrite(echo_buf, NN*sizeof(short),1,echo_fd);
fwrite(ref_buf, NN*sizeof(short),1,ref_fd);
fwrite(e_buf, NN*sizeof(short),1,e_fd);
fwrite(net_buf, NN*sizeof(short),1,net_fd);
RingBuffer_Write(resultBuffer, e_buf, NN * sizeof(short) );
inLen =RingBuffer_GetReadAvailable(&inRing);
outLen = RingBuffer_GetReadAvailable(&outRing);
fclose(net_fd);
fclose(e_fd);
fclose(echo_fd);
fclose(ref_fd);
}
}
}
*/
syntax highlighted by Code2HTML, v. 0.9.1