1136 lines
28 KiB
C
1136 lines
28 KiB
C
/* Copyright (C) 2007 Hong Zhiqian */
|
|
/**
|
|
@file preprocess_tm.h
|
|
@author Hong Zhiqian
|
|
@brief Various compatibility routines for Speex (TriMedia version)
|
|
*/
|
|
/*
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions
|
|
are met:
|
|
|
|
- Redistributions of source code must retain the above copyright
|
|
notice, this list of conditions and the following disclaimer.
|
|
|
|
- Redistributions in binary form must reproduce the above copyright
|
|
notice, this list of conditions and the following disclaimer in the
|
|
documentation and/or other materials provided with the distribution.
|
|
|
|
- Neither the name of the Xiph.org Foundation nor the names of its
|
|
contributors may be used to endorse or promote products derived from
|
|
this software without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
|
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR
|
|
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
|
|
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
|
|
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
#include <ops/custom_defs.h>
|
|
#include "profile_tm.h"
|
|
|
|
#ifdef FIXED_POINT
|
|
#define OVERRIDE_PREPROCESS_ANALYSIS
|
|
static void preprocess_analysis(SpeexPreprocessState * restrict st, spx_int16_t * restrict x)
|
|
{
|
|
register int i, j, framesize = st->frame_size;
|
|
register int N = st->ps_size;
|
|
register int N3 = 2*N - framesize;
|
|
register int N4 = framesize - N3;
|
|
register int * restrict ps = st->ps;
|
|
register int * restrict frame;
|
|
register int * restrict inbuf;
|
|
register int * restrict ptr;
|
|
register int max_val;
|
|
|
|
frame = (int*)(st->frame);
|
|
inbuf = (int*)(st->inbuf);
|
|
ptr = (int*)(st->frame+N3);
|
|
|
|
TMDEBUG_ALIGNMEM(x);
|
|
TMDEBUG_ALIGNMEM(frame);
|
|
TMDEBUG_ALIGNMEM(inbuf);
|
|
|
|
PREPROCESSANAYLSIS_START();
|
|
|
|
N3 >>= 1;
|
|
framesize >>= 1;
|
|
max_val = 0;
|
|
|
|
for ( i=0,j=0 ; i<N3 ; i+=2,j+=8 )
|
|
{ register int r1, r2;
|
|
|
|
r1 = ld32x(inbuf,i);
|
|
r2 = ld32x(inbuf,i+1);
|
|
|
|
st32d(j, frame, r1);
|
|
st32d(j+4, frame, r2);
|
|
}
|
|
|
|
for ( i=0,j=0 ; i<framesize ; i+=2,j+=8 )
|
|
{ register int r1, r2;
|
|
|
|
r1 = ld32x(x, i);
|
|
r2 = ld32x(x, i+1);
|
|
|
|
st32d(j, ptr, r1);
|
|
st32d(j+4,ptr, r2);
|
|
}
|
|
|
|
for ( i=0,j=0,ptr=(int*)(x+N4) ; i<N3 ; i+=2,j+=8 )
|
|
{ register int r1, r2;
|
|
|
|
r1 = ld32x(ptr, i);
|
|
r2 = ld32x(ptr, i+1);
|
|
|
|
st32d(j, inbuf, r1);
|
|
st32d(j+4,inbuf, r2);
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unroll=2
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0,j=0,ptr=(int*)st->window ; i<N ; ++i,j+=4 )
|
|
{ register int f10, w10, r0, r1;
|
|
|
|
f10 = ld32x(frame, i);
|
|
w10 = ld32x(ptr, i);
|
|
|
|
r0 = (sex16(f10) * sex16(w10)) >> 15;
|
|
r1 = (asri(16,f10) * asri(16,w10)) >> 15;
|
|
|
|
max_val = imax(iabs(sex16(r0)), max_val);
|
|
max_val = imax(iabs(sex16(r1)), max_val);
|
|
|
|
r0 = pack16lsb(r1, r0);
|
|
st32d(j, frame, r0);
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
max_val = 14 - spx_ilog2(max_val);
|
|
st->frame_shift = max_val;
|
|
|
|
if ( max_val != 0 )
|
|
{
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0,j=0 ; i<N ; ++i,j+=4 )
|
|
{ register int f10;
|
|
|
|
f10 = ld32x(frame, i);
|
|
f10 = dualasl(f10, max_val);
|
|
st32d(j, frame, f10);
|
|
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
}
|
|
|
|
spx_fft(st->fft_lookup, st->frame, st->ft);
|
|
power_spectrum(st->ft, ps, N << 1);
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0,ptr=(int*)st->ps,max_val<<=1,j=((1<<((max_val))>>1)) ;i<N ; ++i )
|
|
{
|
|
ps[i] = (ps[i] + j) >> max_val;
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
filterbank_compute_bank32(st->bank, ps, ps+N);
|
|
|
|
PREPROCESSANAYLSIS_STOP();
|
|
}
|
|
|
|
#define _MULT16_32_Q15(a,b,c) ADD32(MULT16_16((a),(b)), SHR(MULT16_16((a),(c)),15))
|
|
|
|
#define OVERRIDE_UPDATE_NOISE_PROB
|
|
static void update_noise_prob(SpeexPreprocessState * restrict st)
|
|
{
|
|
register int i;
|
|
register int min_range, nb_adapt;
|
|
register int N = st->ps_size;
|
|
register int * restrict Smin = (int*)st->Smin;
|
|
register int * restrict Stmp = (int*)st->Stmp;
|
|
register int * restrict S = (int*)st->S;
|
|
|
|
UPDATENOISEPROB_START();
|
|
|
|
{
|
|
register int psi_lsb, psi_msb, ips_lsb, ips_msb, psii_lsb, psii_msb;
|
|
register int psiii_lsb, psiii_msb;
|
|
register int q8, q05, q2, q1;
|
|
register int *ps = (int*)st->ps;
|
|
register int si_lsb, si_msb, sii_lsb, sii_msb;
|
|
|
|
q8 = QCONST16(.8f,15);
|
|
q05 = QCONST16(.05f,15);
|
|
q2 = QCONST16(.2f,15);
|
|
q1 = QCONST16(.1f,15);
|
|
|
|
ips_lsb = ps[0];
|
|
psi_lsb = ps[1];
|
|
si_lsb = S[0];
|
|
ips_msb = ips_lsb >> 15;
|
|
psi_msb = psi_lsb >> 15;
|
|
si_msb = si_lsb >> 15;
|
|
|
|
ips_lsb &= 0x00007fff;
|
|
psi_lsb &= 0x00007fff;
|
|
si_lsb &= 0x00007fff;
|
|
|
|
S[0] = _MULT16_32_Q15(q8,si_msb,si_lsb) + _MULT16_32_Q15(q2,ips_msb,ips_lsb);
|
|
|
|
for ( i=1 ; i<N-1 ; i+=2 )
|
|
{
|
|
psii_lsb = ps[i+1];
|
|
si_lsb = S[i];
|
|
|
|
psii_msb = psii_lsb >> 15;
|
|
si_msb = si_lsb >> 15;
|
|
si_lsb &= 0x00007fff;
|
|
psii_lsb &= 0x00007fff;
|
|
|
|
S[i]= _MULT16_32_Q15(q8,si_msb,si_lsb) +
|
|
_MULT16_32_Q15(q05,ips_msb,ips_lsb) +
|
|
_MULT16_32_Q15(q1,psi_msb,psi_lsb) +
|
|
_MULT16_32_Q15(q05,psii_msb,psii_lsb);
|
|
|
|
psiii_lsb = ps[i+2];
|
|
sii_lsb = S[i+1];
|
|
|
|
sii_msb = sii_lsb >> 15;
|
|
psiii_msb= psiii_lsb >> 15;
|
|
sii_lsb &= 0x00007fff;
|
|
psiii_lsb&= 0x00007fff;
|
|
|
|
S[i+1]= _MULT16_32_Q15(q8,sii_msb,sii_lsb) +
|
|
_MULT16_32_Q15(q05,psi_msb,psi_lsb) +
|
|
_MULT16_32_Q15(q1,psii_msb,psii_lsb) +
|
|
_MULT16_32_Q15(q05,psiii_msb,psiii_lsb);
|
|
|
|
ips_lsb = psii_lsb;
|
|
ips_msb = psii_msb;
|
|
psi_lsb = psiii_lsb;
|
|
psi_msb = psiii_msb;
|
|
}
|
|
|
|
S[N-1] = MULT16_32_Q15(q8,S[N-1]) + MULT16_32_Q15(q2,ps[N-1]);
|
|
}
|
|
|
|
nb_adapt = st->nb_adapt;
|
|
|
|
if ( nb_adapt==1 )
|
|
{ for ( i=0 ; i<N ; ++i )
|
|
Smin[i] = Stmp[i] = 0;
|
|
|
|
}
|
|
|
|
min_range = mux(nb_adapt < 100, 15,
|
|
mux(nb_adapt < 1000, 50,
|
|
mux(nb_adapt < 10000, 150, 300)));
|
|
|
|
if ( st->min_count > min_range )
|
|
{
|
|
st->min_count = 0;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unroll=2
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<N ; ++i )
|
|
{ register int si, stmpi;
|
|
|
|
si = S[i];
|
|
stmpi = Stmp[i];
|
|
|
|
Smin[i] = imin(stmpi,si);
|
|
Stmp[i] = si;
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
} else
|
|
{
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unroll=2
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<N ; ++i )
|
|
{ register int si, stmpi, smini;
|
|
|
|
si = S[i];
|
|
stmpi = Stmp[i];
|
|
smini = Smin[i];
|
|
|
|
Smin[i] = imin(smini,si);
|
|
Stmp[i] = imin(stmpi,si);
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
}
|
|
|
|
|
|
{
|
|
register int q4;
|
|
register int * restrict update_prob = (int*)st->update_prob;
|
|
|
|
q4 = QCONST16(.4f,15);
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<N ; ++i )
|
|
{ register int si;
|
|
register int smini;
|
|
|
|
si = S[i];
|
|
smini = Smin[i];
|
|
update_prob[i] = mux(MULT16_32_Q15(q4,si) > ADD32(smini,20), 1, 0);
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
}
|
|
|
|
UPDATENOISEPROB_STOP();
|
|
}
|
|
|
|
#else
|
|
|
|
#define OVERRIDE_PREPROCESS_ANALYSIS
|
|
static void preprocess_analysis(SpeexPreprocessState * restrict st, spx_int16_t * restrict x)
|
|
{
|
|
register int i;
|
|
register int framesize = st->frame_size;
|
|
register int N = st->ps_size;
|
|
register int N3 = 2*N - framesize;
|
|
register int N4 = framesize - N3;
|
|
register float * restrict ps = st->ps;
|
|
register float * restrict frame = st->frame;
|
|
register float * restrict inbuf = st->inbuf;
|
|
|
|
PREPROCESSANAYLSIS_START();
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<N3 ; ++i )
|
|
{
|
|
frame[i] = inbuf[i];
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<framesize ; ++i )
|
|
{ frame[N3+i] = x[i];
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0,x+=N4 ; i<N3 ; ++i )
|
|
{ inbuf[i] = x[i];
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
inbuf = st->window;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<2*N ; ++i )
|
|
{
|
|
frame[i] = frame[i] * inbuf[i];
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
spx_fft(st->fft_lookup, frame, st->ft);
|
|
power_spectrum(st->ft, ps, N << 1);
|
|
filterbank_compute_bank32(st->bank, ps, ps+N);
|
|
|
|
PREPROCESSANAYLSIS_STOP();
|
|
}
|
|
|
|
|
|
#define OVERRIDE_UPDATE_NOISE_PROB
|
|
static void update_noise_prob(SpeexPreprocessState * restrict st)
|
|
{
|
|
|
|
register float * restrict S = st->S;
|
|
register float * restrict ps = st->ps;
|
|
register int N = st->ps_size;
|
|
register int min_range;
|
|
register int i;
|
|
register int nb_adapt;
|
|
register float * restrict Smin = st->Smin;
|
|
register float * restrict Stmp = st->Stmp;
|
|
|
|
UPDATENOISEPROB_START();
|
|
|
|
{
|
|
register float ips, psi;
|
|
|
|
ips = ps[0];
|
|
psi = ps[1];
|
|
|
|
S[0] = .8f * S[0] + .2f * ips;
|
|
|
|
for ( i=1 ; i<N-1 ; i+=2 )
|
|
{
|
|
register float psii, psiii;
|
|
|
|
psii = ps[i+1];
|
|
psiii = ps[i+2];
|
|
S[i] = .8f * S[i] + .05f * ips + .1f * psi + .05f * psii;
|
|
S[i+1] = .8f * S[i+1] + .05f * psi + .1f * psii + .05f * psiii;
|
|
ips = psii;
|
|
psi = psiii;
|
|
}
|
|
|
|
S[N-1] = .8f * S[N-1] + .2f * ps[N-1];
|
|
}
|
|
|
|
nb_adapt = st->nb_adapt;
|
|
|
|
if ( nb_adapt==1 )
|
|
{
|
|
for (i=0;i<N;i++)
|
|
Smin[i] = st->Stmp[i] = 0;
|
|
}
|
|
|
|
min_range = mux(nb_adapt < 100, 15,
|
|
mux(nb_adapt < 1000, 50,
|
|
mux(nb_adapt < 10000, 150, 300)));
|
|
|
|
|
|
if ( st->min_count > min_range )
|
|
{
|
|
st->min_count = 0;
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<N ; ++i )
|
|
{
|
|
register float stmpi, si;
|
|
|
|
stmpi = Stmp[i];
|
|
si = S[i];
|
|
|
|
Smin[i] = fmin(stmpi,si);
|
|
Stmp[i] = si;
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
} else
|
|
{
|
|
register float * restrict Smin = st->Smin;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<N ; ++i )
|
|
{
|
|
register float stmpi, si, smini;
|
|
|
|
stmpi = Stmp[i];
|
|
si = S[i];
|
|
smini = Smin[i];
|
|
|
|
Smin[i] = fmin(smini,si);
|
|
Stmp[i] = fmin(stmpi,si);
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
}
|
|
|
|
{
|
|
register int * restrict update_prob = (int*)st->update_prob;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for (i=0;i<N;i++)
|
|
{ register float si;
|
|
register float smini;
|
|
|
|
si = S[i];
|
|
smini = Smin[i];
|
|
update_prob[i] = mux( (.4 * si) > (smini + 20.f), 1, 0);
|
|
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
}
|
|
|
|
UPDATENOISEPROB_STOP();
|
|
}
|
|
|
|
|
|
#define OVERRIDE_COMPUTE_GAIN_FLOOR
|
|
static void compute_gain_floor(
|
|
int noise_suppress,
|
|
int effective_echo_suppress,
|
|
float * restrict noise,
|
|
float * restrict echo,
|
|
float * gain_floor,
|
|
int len
|
|
)
|
|
{
|
|
register int i;
|
|
register float echo_floor;
|
|
register float noise_floor;
|
|
|
|
COMPUTEGAINFLOOR_START();
|
|
|
|
noise_floor = exp(.2302585f*noise_suppress);
|
|
echo_floor = exp(.2302585f*effective_echo_suppress);
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_COMPUTEGAINFLOOR)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for (i=0;i<len;i++)
|
|
{ register float noisei, echoi;
|
|
|
|
noisei = noise[i];
|
|
echoi = echo[i];
|
|
|
|
gain_floor[i] = FRAC_SCALING * sqrt(noise_floor * noisei + echo_floor * echoi) / sqrt(1+noisei+echoi);
|
|
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_COMPUTEGAINFLOOR)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
COMPUTEGAINFLOOR_STOP();
|
|
}
|
|
|
|
#endif
|
|
|
|
static inline spx_word32_t hypergeom_gain(spx_word32_t xx);
|
|
static inline spx_word16_t qcurve(spx_word16_t x);
|
|
static void compute_gain_floor(int noise_suppress, int effective_echo_suppress, spx_word32_t *noise, spx_word32_t *echo, spx_word16_t *gain_floor, int len);
|
|
void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *Yout, int len);
|
|
|
|
#ifndef FIXED_POINT
|
|
static void speex_compute_agc(SpeexPreprocessState *st, spx_word16_t Pframe, spx_word16_t *ft);
|
|
#endif
|
|
|
|
void preprocess_residue_echo(
|
|
SpeexPreprocessState * restrict st,
|
|
int N,
|
|
int NM
|
|
)
|
|
{
|
|
if (st->echo_state)
|
|
{
|
|
register spx_word32_t * restrict r_echo = st->residual_echo;
|
|
register spx_word32_t * restrict e_noise = st->echo_noise;
|
|
register int i;
|
|
|
|
#ifndef FIXED_POINT
|
|
register spx_word32_t r;
|
|
#endif
|
|
|
|
speex_echo_get_residual(st->echo_state, r_echo, N);
|
|
|
|
#ifndef FIXED_POINT
|
|
r = r_echo[0];
|
|
if (!(r >=0 && r < N*1e9f) )
|
|
{
|
|
memset(r_echo, 0, N * sizeof(spx_word32_t));
|
|
}
|
|
#endif
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for (i=0;i<N;i++)
|
|
{ register spx_word32_t eni = e_noise[i];
|
|
e_noise[i] = MAX32(MULT16_32_Q15(QCONST16(.6f,15),eni), r_echo[i]);
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
filterbank_compute_bank32(st->bank, e_noise, e_noise+N);
|
|
|
|
} else
|
|
{ memset(st->echo_noise, 0, (NM) * sizeof(spx_word32_t));
|
|
}
|
|
}
|
|
|
|
void preprocess_update_noise(
|
|
SpeexPreprocessState * restrict st,
|
|
spx_word32_t * restrict ps,
|
|
int N
|
|
)
|
|
{
|
|
register spx_word16_t beta, beta_1;
|
|
register int * restrict up = st->update_prob;
|
|
register spx_word32_t * restrict noise = st->noise;
|
|
register int i;
|
|
|
|
beta = MAX16(QCONST16(.03,15),DIV32_16(Q15_ONE,st->nb_adapt));
|
|
beta_1 = Q15_ONE-beta;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for (i=0;i<N;i++)
|
|
{ register spx_word32_t ni = noise[i];
|
|
register spx_word32_t psi = ps[i];
|
|
|
|
if ( !up[i] || psi < PSHR32(ni, NOISE_SHIFT) )
|
|
{ noise[i] = MAX32(EXTEND32(0),MULT16_32_Q15(beta_1,ni) +
|
|
MULT16_32_Q15(beta,SHL32(psi,NOISE_SHIFT)));
|
|
}
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
filterbank_compute_bank32(st->bank, noise, noise+N);
|
|
}
|
|
|
|
void preprocess_compute_SNR(
|
|
SpeexPreprocessState * restrict st,
|
|
spx_word32_t * restrict ps,
|
|
int NM
|
|
)
|
|
{
|
|
register spx_word32_t * restrict noise = st->noise;
|
|
register spx_word32_t * restrict echo = st->echo_noise;
|
|
register spx_word32_t * restrict reverb = st->reverb_estimate;
|
|
register spx_word16_t * restrict post = st->post;
|
|
register spx_word32_t * restrict old_ps = st->old_ps;
|
|
register spx_word16_t * restrict prior = st->prior;
|
|
register int i;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<NM ; i++)
|
|
{
|
|
register spx_word16_t gamma;
|
|
register spx_word32_t tot_noise;
|
|
register spx_word16_t posti;
|
|
register spx_word32_t opsi;
|
|
register spx_word16_t priori;
|
|
|
|
tot_noise = ADD32(ADD32(ADD32(EXTEND32(1), PSHR32(noise[i],NOISE_SHIFT)), echo[i]) , reverb[i]);
|
|
|
|
posti = SUB16(DIV32_16_Q8(ps[i],tot_noise), QCONST16(1.f,SNR_SHIFT));
|
|
posti = MIN16(posti, QCONST16(100.f,SNR_SHIFT));
|
|
post[i] = posti;
|
|
|
|
opsi = old_ps[i];
|
|
|
|
gamma = QCONST16(.1f,15)+MULT16_16_Q15(QCONST16(.89f,15),SQR16_Q15(DIV32_16_Q15(opsi,ADD32(opsi,tot_noise))));
|
|
|
|
priori = EXTRACT16(PSHR32(ADD32(MULT16_16(gamma,MAX16(0,posti)), MULT16_16(Q15_ONE-gamma,DIV32_16_Q8(opsi,tot_noise))), 15));
|
|
prior[i]=MIN16(priori, QCONST16(100.f,SNR_SHIFT));
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
}
|
|
|
|
spx_word32_t preprocess_smooth_SNR(
|
|
SpeexPreprocessState * restrict st,
|
|
int N,
|
|
int NM
|
|
)
|
|
{
|
|
register spx_word16_t * restrict zeta = st->zeta;
|
|
register spx_word16_t * restrict prior = st->prior;
|
|
register spx_word32_t Zframe;
|
|
register spx_word16_t iprior, priori;
|
|
register int _N = N-1;
|
|
register int i;
|
|
|
|
iprior = prior[0];
|
|
priori = prior[1];
|
|
zeta[0] = PSHR32(ADD32(MULT16_16(QCONST16(.7f,15),zeta[0]), MULT16_16(QCONST16(.3f,15),iprior)),15);
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unroll=2
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=1 ; i<_N ; i++)
|
|
{ register spx_word16_t zetai = zeta[i];
|
|
register spx_word16_t priorii = prior[i+1];
|
|
|
|
zeta[i] = PSHR32(ADD32(ADD32(ADD32(MULT16_16(QCONST16(.7f,15),zetai), MULT16_16(QCONST16(.15f,15),priori)),
|
|
MULT16_16(QCONST16(.075f,15),iprior)), MULT16_16(QCONST16(.075f,15),priorii)),15);
|
|
|
|
iprior = priori;
|
|
priori = priorii;
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
for (i=_N; i<NM ; i++)
|
|
{ register spx_word16_t zetai = zeta[i];
|
|
|
|
priori = prior[i];
|
|
zeta[i] = PSHR32(ADD32(MULT16_16(QCONST16(.7f,15),zetai), MULT16_16(QCONST16(.3f,15),priori)),15);
|
|
}
|
|
|
|
Zframe = 0;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=N ; i<NM ; i++ )
|
|
{ Zframe = ADD32(Zframe, EXTEND32(zeta[i]));
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
return Zframe;
|
|
}
|
|
|
|
void preprocess_compute_emgain(
|
|
SpeexPreprocessState * restrict st,
|
|
spx_word32_t * restrict ps,
|
|
spx_word16_t Pframe,
|
|
int NM
|
|
)
|
|
{
|
|
register spx_word16_t * restrict zeta = st->zeta;
|
|
register spx_word16_t * restrict prior = st->prior;
|
|
register spx_word16_t * restrict gain = st->gain;
|
|
register spx_word32_t * restrict old_ps = st->old_ps;
|
|
register spx_word16_t * restrict post = st->post;
|
|
register spx_word16_t * restrict gain2 = st->gain2;
|
|
register int i;
|
|
register int N=st->ps_size;
|
|
|
|
for ( i=N ; i<NM ; ++i )
|
|
{
|
|
register spx_word32_t theta;
|
|
register spx_word32_t MM;
|
|
register spx_word16_t prior_ratio;
|
|
register spx_word16_t P1;
|
|
register spx_word16_t q;
|
|
|
|
#ifdef FIXED_POINT
|
|
register spx_word16_t tmp;
|
|
#endif
|
|
register spx_word16_t priori = prior[i];
|
|
|
|
prior_ratio = PDIV32_16(SHL32(EXTEND32(priori), 15), ADD16(priori, SHL32(1,SNR_SHIFT)));
|
|
theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(post[i]),EXPIN_SHIFT-SNR_SHIFT));
|
|
|
|
MM = hypergeom_gain(theta);
|
|
gain[i] = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
|
|
old_ps[i] = MULT16_32_P15(QCONST16(.2f,15),old_ps[i]) + MULT16_32_P15(MULT16_16_P15(QCONST16(.8f,15),SQR16_Q15(gain[i])),ps[i]);
|
|
|
|
P1 = QCONST16(.199f,15)+MULT16_16_Q15(QCONST16(.8f,15),qcurve (zeta[i]));
|
|
q = Q15_ONE-MULT16_16_Q15(Pframe,P1);
|
|
|
|
#ifdef FIXED_POINT
|
|
theta = MIN32(theta, EXTEND32(32767));
|
|
tmp = MULT16_16_Q15((SHL32(1,SNR_SHIFT)+priori),EXTRACT16(MIN32(Q15ONE,SHR32(spx_exp(-EXTRACT16(theta)),1))));
|
|
tmp = MIN16(QCONST16(3.,SNR_SHIFT), tmp);
|
|
tmp = EXTRACT16(PSHR32(MULT16_16(PDIV32_16(SHL32(EXTEND32(q),8),(Q15_ONE-q)),tmp),8));
|
|
gain2[i]=DIV32_16(SHL32(EXTEND32(32767),SNR_SHIFT), ADD16(256,tmp));
|
|
#else
|
|
gain2[i]=1/(1.f + (q/(1.f-q))*(1+priori)*exp(-theta));
|
|
#endif
|
|
}
|
|
|
|
filterbank_compute_psd16(st->bank,gain2+N, gain2);
|
|
filterbank_compute_psd16(st->bank,gain+N, gain);
|
|
}
|
|
|
|
void preprocess_compute_linear_gain(
|
|
SpeexPreprocessState * restrict st,
|
|
spx_word32_t * restrict ps,
|
|
int N
|
|
)
|
|
{
|
|
register spx_word16_t * restrict gain_floor = st->gain_floor;
|
|
register spx_word16_t * restrict prior = st->prior;
|
|
register spx_word16_t * restrict gain = st->gain;
|
|
register spx_word32_t * restrict old_ps = st->old_ps;
|
|
register spx_word16_t * restrict post = st->post;
|
|
register spx_word16_t * restrict gain2 = st->gain2;
|
|
register int i;
|
|
|
|
filterbank_compute_psd16(st->bank,gain_floor+N,gain_floor);
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for (i=0;i<N;i++)
|
|
{
|
|
register spx_word32_t MM;
|
|
register spx_word32_t theta;
|
|
register spx_word16_t prior_ratio;
|
|
register spx_word16_t tmp;
|
|
register spx_word16_t p;
|
|
register spx_word16_t g;
|
|
register spx_word16_t gfi = gain_floor[i];
|
|
|
|
prior_ratio = PDIV32_16(SHL32(EXTEND32(st->prior[i]), 15), ADD16(prior[i], SHL32(1,SNR_SHIFT)));
|
|
theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(post[i]),EXPIN_SHIFT-SNR_SHIFT));
|
|
MM = hypergeom_gain(theta);
|
|
|
|
g = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
|
|
p = gain2[i];
|
|
|
|
g = VMUX( MULT16_16_Q15(QCONST16(.333f,15),g) > gain[i], MULT16_16(3,gain[i]), g);
|
|
|
|
old_ps[i]= MULT16_32_P15(QCONST16(.2f,15),old_ps[i]) +
|
|
MULT16_32_P15(MULT16_16_P15(QCONST16(.8f,15),SQR16_Q15(g)),ps[i]);
|
|
|
|
g = VMUX( g < gfi, gfi, g );
|
|
gain[i] = g;
|
|
|
|
tmp = MULT16_16_P15(p,spx_sqrt(SHL32(EXTEND32(g),15))) +
|
|
MULT16_16_P15(SUB16(Q15_ONE,p),spx_sqrt(SHL32(EXTEND32(gfi),15)));
|
|
|
|
gain2[i]=SQR16_Q15(tmp);
|
|
|
|
/* Use this if you want a log-domain MMSE estimator instead */
|
|
/* gain2[i] = pow(g, p) * pow(gfi,1.f-p);*/
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
}
|
|
|
|
|
|
#if 0
|
|
void preprocess_compute_bark_gain(
|
|
SpeexPreprocessState * restrict st,
|
|
int N,
|
|
int NM
|
|
)
|
|
{
|
|
register spx_word16_t * restrict gain_floor = st->gain_floor;
|
|
register spx_word16_t * restrict gain = st->gain;
|
|
register spx_word16_t * restrict gain2 = st->gain2;
|
|
register int i;
|
|
|
|
for (i=N;i<NM;i++)
|
|
{
|
|
register spx_word16_t tmp;
|
|
register spx_word16_t p = gain2[i];
|
|
register spx_word16_t gaini;
|
|
register spx_word16_t gfi = gain_floor[i];
|
|
|
|
gaini = MAX16(gain[i], gfi);
|
|
|
|
gain[i] = gaini;
|
|
|
|
tmp = MULT16_16_P15(p,spx_sqrt(SHL32(EXTEND32(gaini),15))) +
|
|
MULT16_16_P15(SUB16(Q15_ONE,p),spx_sqrt(SHL32(EXTEND32(gfi),15)));
|
|
|
|
gain2[i]=SQR16_Q15(tmp);
|
|
}
|
|
|
|
filterbank_compute_psd16(st->bank,gain2+N, gain2);
|
|
}
|
|
#endif
|
|
|
|
void preprocess_apply_gain(
|
|
SpeexPreprocessState * restrict st,
|
|
int N
|
|
)
|
|
{
|
|
register spx_word16_t * restrict ft = st->ft;
|
|
register spx_word16_t * restrict gain2 = st->gain2;
|
|
register int j, i;
|
|
|
|
ft[0] = MULT16_16_P15(gain2[0],ft[0]);
|
|
|
|
for (i=1,j=1; i<N ; i++,j+=2)
|
|
{
|
|
register spx_word16_t gain2i = gain2[i];
|
|
register spx_word16_t ftj = ft[j];
|
|
register spx_word16_t ftjj = ft[j+1];
|
|
|
|
ft[j] = MULT16_16_P15(gain2i,ftj);
|
|
ft[j+1] = MULT16_16_P15(gain2i,ftjj);
|
|
}
|
|
|
|
ft[(N<<1)-1] = MULT16_16_P15(gain2[N-1],ft[(N<<1)-1]);
|
|
}
|
|
|
|
#ifdef FIXED_POINT
|
|
void preprocess_scale(
|
|
SpeexPreprocessState * restrict st,
|
|
int N
|
|
)
|
|
{
|
|
register spx_word16_t * restrict frame = st->frame;
|
|
register int shift = st->frame_shift;
|
|
register int i;
|
|
register int N2 = N << 1;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<N2 ;i++)
|
|
{ register spx_word16_t framei = frame[i];
|
|
|
|
frame[i] = PSHR16(framei,shift);
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
}
|
|
|
|
#else
|
|
|
|
void preprocess_apply_agc(
|
|
SpeexPreprocessState * restrict st,
|
|
int N
|
|
)
|
|
{
|
|
register spx_word16_t max_sample=0;
|
|
register spx_word16_t * restrict frame = st->frame;
|
|
register int i;
|
|
register int N2 = N << 1;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for (i=0;i<N2;i++)
|
|
{ register spx_word16_t framei = VABS(frame[i]);
|
|
|
|
max_sample = VMUX( framei > max_sample, framei, max_sample);
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
if ( max_sample > 28000.f )
|
|
{
|
|
float damp = 28000.f/max_sample;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i< N2 ; i++ )
|
|
{ frame[i] *= damp;
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
}
|
|
}
|
|
#endif
|
|
|
|
|
|
void preprocess_update(
|
|
SpeexPreprocessState * restrict st,
|
|
spx_int16_t * restrict x,
|
|
int N
|
|
)
|
|
{
|
|
register spx_word16_t * restrict frame = st->frame;
|
|
register spx_word16_t * restrict window = st->window;
|
|
register spx_word16_t * restrict outbuf = st->outbuf;
|
|
register int framesize = st->frame_size;
|
|
register int N2 = N << 1;
|
|
register int N3 = N2 - framesize;
|
|
register int N4 = (framesize) - N3;
|
|
register int i;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<N2 ; i++)
|
|
{ register spx_word16_t fi = frame[i];
|
|
register spx_word16_t wi = window[i];
|
|
|
|
frame[i] = MULT16_16_Q15(fi, wi);
|
|
}
|
|
for (i=0;i<N3;i++)
|
|
{ x[i] = outbuf[i] + frame[i];
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
|
|
for ( i=0;i<N4;i++)
|
|
{ x[N3+i] = frame[N3+i];
|
|
}
|
|
|
|
memcpy(outbuf, frame+framesize, (N3) * sizeof(spx_word16_t));
|
|
}
|
|
|
|
#define OVERRIDE_SPEEX_PREPROCESS_RUN
|
|
int speex_preprocess_run(SpeexPreprocessState * restrict st, spx_int16_t * restrict x)
|
|
{
|
|
register int i, N, M, NM;
|
|
register spx_word32_t * restrict ps=st->ps;
|
|
register spx_word32_t Zframe;
|
|
register spx_word16_t Pframe;
|
|
|
|
st->nb_adapt++;
|
|
st->min_count++;
|
|
N = st->ps_size;
|
|
M = st->nbands;
|
|
NM = N + M;
|
|
|
|
preprocess_residue_echo(st, N, NM);
|
|
preprocess_analysis(st, x);
|
|
update_noise_prob(st);
|
|
preprocess_update_noise(st, ps, N);
|
|
|
|
if ( st->nb_adapt == 1 )
|
|
{ memcpy(st->old_ps, ps, (NM) * sizeof(spx_word32_t));
|
|
}
|
|
|
|
preprocess_compute_SNR(st, ps, NM);
|
|
Zframe = preprocess_smooth_SNR(st, N, NM);
|
|
|
|
|
|
{
|
|
register spx_word16_t effective_echo_suppress;
|
|
|
|
Pframe = QCONST16(.1f,15)+MULT16_16_Q15(QCONST16(.899f,15),qcurve(DIV32_16(Zframe,M)));
|
|
effective_echo_suppress = EXTRACT16(PSHR32(ADD32(MULT16_16(SUB16(Q15_ONE,Pframe), st->echo_suppress),
|
|
MULT16_16(Pframe, st->echo_suppress_active)),15));
|
|
compute_gain_floor(st->noise_suppress, effective_echo_suppress, st->noise+N, st->echo_noise+N, st->gain_floor+N, M);
|
|
|
|
}
|
|
|
|
preprocess_compute_emgain(st, ps, Pframe, NM);
|
|
preprocess_compute_linear_gain(st, ps, N);
|
|
|
|
|
|
if (!st->denoise_enabled)
|
|
{
|
|
register spx_word16_t * restrict gain2 = st->gain2;
|
|
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unroll=4
|
|
#pragma TCS_unrollexact=1
|
|
#endif
|
|
for ( i=0 ; i<NM ; i++ )
|
|
{ gain2[i] = Q15_ONE;
|
|
}
|
|
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
|
|
#pragma TCS_unrollexact=0
|
|
#pragma TCS_unroll=0
|
|
#endif
|
|
}
|
|
|
|
preprocess_apply_gain(st, N);
|
|
|
|
#ifndef FIXED_POINT
|
|
if (st->agc_enabled)
|
|
{ speex_compute_agc(st, Pframe, st->ft);
|
|
}
|
|
#endif
|
|
|
|
|
|
spx_ifft(st->fft_lookup, st->ft, st->frame);
|
|
|
|
#ifdef FIXED_POINT
|
|
preprocess_scale(st, N);
|
|
#endif
|
|
|
|
#ifndef FIXED_POINT
|
|
if ( st->agc_enabled )
|
|
{ preprocess_apply_agc(st, N);
|
|
}
|
|
#endif
|
|
|
|
preprocess_update(st, x, N);
|
|
|
|
if ( st->vad_enabled )
|
|
{
|
|
if (Pframe > st->speech_prob_start || (st->was_speech && Pframe > st->speech_prob_continue))
|
|
{ st->was_speech=1;
|
|
return 1;
|
|
|
|
} else
|
|
{ st->was_speech=0;
|
|
return 0;
|
|
}
|
|
} else
|
|
{ return 1;
|
|
}
|
|
}
|