mdf_tm.h

一个开源的sip源代码

/* Copyright (C) 2007 Hong Zhiqian */
/**
   @file mdf_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"

// shifted power spectrum to fftwrap.c so that optimisation can be shared between mdf.c and preprocess.c
#define OVERRIDE_POWER_SPECTRUM					

#ifdef FIXED_POINT

#else

#define OVERRIDE_FILTER_DC_NOTCH16
void filter_dc_notch16(
	const spx_int16_t * restrict in, 
	float radius, 
	float * restrict out, 
	int len, 
	float * restrict mem
)
{
	register int i;
	register float den2, r1;
	register float mem0, mem1;

	FILTERDCNOTCH16_START();

	r1 = 1 - radius;
	den2 = (radius * radius) + (0.7 * r1 * r1);
	mem0 = mem[0];
	mem1 = mem[1];

#if (TM_UNROLL && TM_UNROLL_FILTERDCNOTCH16)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for ( i=0 ; i<len ; ++i )
	{
		register float vin  = in[i];
		register float vout = mem0 + vin;
		register float rvout = radius * vout;

		mem0 = mem1 + 2 * (-vin + rvout);
		mem1 = vin - (den2 * vout);
		
		out[i] = rvout;
	}
#if (TM_UNROLL && TM_UNROLL_FILTERDCNOTCH16)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	mem[0] = mem0;
	mem[1] = mem1;

	FILTERDCNOTCH16_STOP();
}

#define OVERRIDE_MDF_INNER_PROD
float mdf_inner_prod(
	const float * restrict x, 
	const float * restrict y, 
	int len
)
{
	register float sum = 0;
   
	MDFINNERPROD_START();

	len >>= 1;
  
#if (TM_UNROLL && TM_UNROLL_MDFINNERPRODUCT)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	while(len--)
	{
		register float acc0, acc1;

		acc0 = (*x++) * (*y++);
		acc1 = (*x++) * (*y++);

		sum	 += acc0 + acc1;
	}
#if (TM_UNROLL && TM_UNROLL_MDFINNERPRODUCT)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	MDFINNERPROD_STOP();

	return sum;
}

#define OVERRIDE_SPECTRAL_MUL_ACCUM
void spectral_mul_accum(
	const float * restrict X, 
	const float * restrict Y, 
	float * restrict acc, 
	int N, int M
)
{
	register int i, j;
	register float Xi, Yi, Xii, Yii;
	register int _N;

	SPECTRALMULACCUM_START();

	acc[0] = X[0] * Y[0];
	_N = N-1;

	for ( i=1 ; i<_N ; i+=2 )
	{
		Xi = X[i];
		Yi = Y[i];
		Xii = X[i+1];
		Yii = Y[i+1];

		acc[i]	= (Xi  * Yi - Xii * Yii);
		acc[i+1]= (Xii * Yi + Xi  * Yii);
	}
      
	acc[_N] = X[_N] * Y[_N];

	for ( j=1,X+=N,Y+=N ; j<M ; j++ )
	{	
		acc[0] += X[0] * Y[0];

		for ( i=1 ; i<N-1 ; i+=2 )
		{
			Xi = X[i];
			Yi = Y[i];
			Xii = X[i+1];
			Yii = Y[i+1];

			acc[i]	+= (Xi  * Yi - Xii * Yii);
			acc[i+1]+= (Xii * Yi + Xi  * Yii);
		}
      
		acc[_N] += X[_N] * Y[_N];
		X += N;
		Y += N;
	}

	SPECTRALMULACCUM_STOP();
}

#define OVERRIDE_WEIGHTED_SPECTRAL_MUL_CONJ
void weighted_spectral_mul_conj(
	const float * restrict w, 
	const float p, 
	const float * restrict X, 
	const float * restrict Y, 
	float * restrict prod, 
	int N
)
{
	register int i, j;
   	register int _N;

	WEIGHTEDSPECTRALMULCONJ_START();

	prod[0] = p * w[0] * X[0] * Y[0]; 
	_N = N-1;

	for (i=1,j=1;i<_N;i+=2,j++)
	{
		register float W;
		register float Xi, Yi, Xii, Yii;

		Xi = X[i];
		Yi = Y[i];
		Xii = X[i+1];
		Yii = Y[i+1];
		W = p * w[j];


		prod[i]	 = W * (Xi  * Yi + Xii * Yii);
		prod[i+1]= W * (Xi  * Yii - Xii * Yi);
	}
   
	prod[_N] = p * w[j] * X[_N] * Y[_N];

	WEIGHTEDSPECTRALMULCONJ_STOP();
}

#define OVERRIDE_MDF_ADJUST_PROP
void mdf_adjust_prop(
	const float * restrict W, 
	int N, 
	int M, 
	float * restrict prop
)
{
	register int i, j;
	register float max_sum = 1;
	register float prop_sum = 1;

	MDFADJUSTPROP_START();

	for ( i=0 ; i<M ; ++i )
	{
		register float tmp = 1;
		register int k = i * N;
		register int l = k + N;
		register float propi;

#if (TM_UNROLL && TM_UNROLL_MDFADJUSTPROP)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
		for ( j=k ; j<l ; ++j )
		{	
			register float wi = W[j];
			
			tmp += wi * wi;
		}
#if (TM_UNROLL && TM_UNROLL_MDFADJUSTPROP)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

		propi  = spx_sqrt(tmp);
		prop[i]= propi;
		max_sum= fmux(propi > max_sum, propi, max_sum);
	}

	for ( i=0 ; i<M ; ++i )
	{
		register float propi = prop[i];

		propi += .1f * max_sum;
		prop_sum += propi;
		prop[i] = propi;
	}

	prop_sum = 0.99f / prop_sum;
	for ( i=0 ; i<M ; ++i )
	{	prop[i] = prop_sum * prop[i];
	}

	MDFADJUSTPROP_STOP();
}


#define OVERRIDE_SPEEX_ECHO_GET_RESIDUAL
void speex_echo_get_residual(
	SpeexEchoState * restrict st, 
	float * restrict residual_echo, 
	int len
)
{
	register int i;
	register float leak2, leake;
	register int N;
	register float * restrict window;
	register float * restrict last_y;
	register float * restrict y;

	SPEEXECHOGETRESIDUAL_START();

	window = st->window;
	last_y = st->last_y;
	y = st->y;
	N = st->window_size;


#if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for (i=0;i<N;i++)
	{	y[i] = window[i] * last_y[i];	
	}
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
      
	spx_fft(st->fft_table, st->y, st->Y);
	power_spectrum(st->Y, residual_echo, N);
      
	leake = st->leak_estimate;
	leak2 = fmux(leake > .5, 1, 2 * leake);
	N = st->frame_size;

#if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for ( i=0 ; i<N ; ++i )
	{	residual_echo[i] *= leak2;
	}
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	residual_echo[N] *= leak2;

#ifndef NO_REMARK
   (void)len;
#endif

   SPEEXECHOGETRESIDUAL_STOP();
}
#endif


void mdf_preemph(
	SpeexEchoState * restrict st, 
	spx_word16_t * restrict x,
	const spx_int16_t * restrict far_end,
	int framesize
)
{
	register spx_word16_t preemph = st->preemph;
	register spx_word16_t memX = st->memX;
	register spx_word16_t memD = st->memD;
	register spx_word16_t * restrict input = st->input;
	register int i;
#ifdef FIXED_POINT
	register int saturated = st->saturated;
#endif

#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for ( i=0 ; i<framesize ; ++i )
	{
		register spx_int16_t far_endi = far_end[i];
		register spx_word32_t tmp32;
		register spx_word16_t inputi = input[i];

		tmp32 = SUB32(EXTEND32(far_endi), EXTEND32(MULT16_16_P15(preemph,memX)));

#ifdef FIXED_POINT
		saturated = mux(iabs(tmp32) > 32767, M+1, saturated);
		tmp32 = iclipi(tmp32,32767);
#endif

		x[i] = EXTRACT16(tmp32);
		memX = far_endi;
		tmp32 = SUB32(EXTEND32(inputi), EXTEND32(MULT16_16_P15(preemph, memD)));

#ifdef FIXED_POINT
		saturated = mux( ((tmp32 > 32767) && (saturated == 0)), 1,
					mux( ((tmp32 <-32767) && (saturated == 0)), 1, saturated ));
		tmp32 = iclipi(tmp32,32767);
#endif
		memD = inputi;
		input[i] = tmp32;
	}
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	st->memD = memD;
	st->memX = memX;

#ifdef FIXED_POINT
	st->saturated = saturated;
#endif
}