filters_tm.h

一个开源的sip源代码

		tmp2 = ld32x(x,i+2);
		tmp3 = ld32x(x,i+3);

		tmp2 = iabs(tmp2);
		tmp3 = iabs(tmp3);

		tmp0 = imax(tmp0, tmp1);
		max_val = imax(max_val, tmp0); 
		tmp2 = imax(tmp2, tmp3);		
		max_val = imax(max_val, tmp2);
	}

	sig_shift=0;
	while (max_val>max_scale)
	{	sig_shift++;
		max_val >>= 1;
	}

	if ( sig_shift != 0 )
	{
		for ( i=0 ; i<len ; i+=8, y+=8 )
		{
			register int x0, x1, x2, x3, x4, x5, x6, x7;
			register int y10, y32, y54, y76;

			x0 = ld32x(x,i);   x1 = ld32x(x,i+1); x2 = ld32x(x,i+2); x3 = ld32x(x,i+3); 
			x4 = ld32x(x,i+4); x5 = ld32x(x,i+5); x6 = ld32x(x,i+6); x7 = ld32x(x,i+7);

			y10 = pack16lsb(x1 >> sig_shift, x0 >> sig_shift);
			y32 = pack16lsb(x3 >> sig_shift, x2 >> sig_shift);
			y54 = pack16lsb(x5 >> sig_shift, x4 >> sig_shift);
			y76 = pack16lsb(x7 >> sig_shift, x6 >> sig_shift);

			st32(y,y10);
			st32d(4,y,y32);
			st32d(8,y,y54);
			st32d(12,y,y76);
		}
	}
	return sig_shift;
}


#define OVERRIDE_NORMALIZE16
int normalize16(const Int32 *x, Int16 *y, Int32 max_scale, int len)
{
	TMDEBUG_ALIGNMEM(x);
	TMDEBUG_ALIGNMEM(y);

	NORMALIZE16_START();

	if ( len == 9 )
	{	NORMALIZE16_STOP();
		return normalize16_9(x,y,max_scale);
	} else
	{	NORMALIZE16_STOP();
		return normalize16_mod8(x,y,max_scale,len);
	}
}


void filter_mem16_10(const Int16 *x, const Int16 *num, const Int16 *den, Int16 *y, int N, Int32 *mem)
{
	register int i;
	register int c9, c8, c7, c6, c5;
	register int c4, c3, c2, c1, c0;
	register int input;
	register int output_0, output_1, output_2, output_3, output_4;
	register int output_5, output_6, output_7, output_8, output_9;
    register Int16 xi, yi;

	c9 = pack16lsb(-den[9],num[9]);
	c8 = pack16lsb(-den[8],num[8]);
	c7 = pack16lsb(-den[7],num[7]);
	c6 = pack16lsb(-den[6],num[6]);
	c5 = pack16lsb(-den[5],num[5]);
	c4 = pack16lsb(-den[4],num[4]);
	c3 = pack16lsb(-den[3],num[3]);
	c2 = pack16lsb(-den[2],num[2]);
	c1 = pack16lsb(-den[1],num[1]);
	c0 = pack16lsb(-den[0],num[0]);

	output_0 = mem[0];
	output_1 = mem[1];
	output_2 = mem[2];
	output_3 = mem[3];
	output_4 = mem[4];
	output_5 = mem[5];
	output_6 = mem[6];
	output_7 = mem[7];
	output_8 = mem[8];
	output_9 = mem[9];

#if (TM_UNROLL && TM_UNROLL_FILTER)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif

	for ( i=0 ; i<N ; i++ )
    {
		xi = (int)(x[i]);
		yi = iclipi(iadd(xi,PSHR32(output_0,LPC_SHIFT)),32767);

		input	= pack16lsb(yi,xi);
		output_0= iadd(ifir16(c0,input),output_1);
		output_1= iadd(ifir16(c1,input),output_2);
		output_2= iadd(ifir16(c2,input),output_3);
		output_3= iadd(ifir16(c3,input),output_4);
		output_4= iadd(ifir16(c4,input),output_5);
		output_5= iadd(ifir16(c5,input),output_6);
		output_6= iadd(ifir16(c6,input),output_7);
		output_7= iadd(ifir16(c7,input),output_8);
		output_8= iadd(ifir16(c8,input),output_9);
		output_9= ifir16(c9,input);

		y[i] = yi;
   }

#if (TM_UNROLL && TM_UNROLL_FILTER)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	mem[0] = output_0;
	mem[1] = output_1;
	mem[2] = output_2;
	mem[3] = output_3;
	mem[4] = output_4;
	mem[5] = output_5;
	mem[6] = output_6;
	mem[7] = output_7;
	mem[8] = output_8;
	mem[9] = output_9;
}

void filter_mem16_8(const Int16 *x, const Int16 *num, const Int16 *den, Int16 *y, int N, Int32 *mem)
{
	register int i;
	register int c7, c6, c5, c4, c3, c2, c1, c0;
	register int output_0, output_1, output_2, output_3, output_4, output_5, output_6, output_7;
	register int input;
    register Int16 xi, yi;

	c7 = pack16lsb(-den[7],num[7]);
	c6 = pack16lsb(-den[6],num[6]);
	c5 = pack16lsb(-den[5],num[5]);
	c4 = pack16lsb(-den[4],num[4]);
	c3 = pack16lsb(-den[3],num[3]);
	c2 = pack16lsb(-den[2],num[2]);
	c1 = pack16lsb(-den[1],num[1]);
	c0 = pack16lsb(-den[0],num[0]);

	output_0 = mem[0];
	output_1 = mem[1];
	output_2 = mem[2];
	output_3 = mem[3];
	output_4 = mem[4];
	output_5 = mem[5];
	output_6 = mem[6];
	output_7 = mem[7];

#if (TM_UNROLL && TM_UNROLL_FILTER)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif

	for ( i=0 ; i<N ; i++ )
    {
		xi = x[i];
		yi = iclipi(iadd((int)(xi),PSHR32(output_0,LPC_SHIFT)),32767);

		input	= pack16lsb(yi,xi);
		output_0= iadd(ifir16(c0,input),output_1);
		output_1= iadd(ifir16(c1,input),output_2);
		output_2= iadd(ifir16(c2,input),output_3);
		output_3= iadd(ifir16(c3,input),output_4);
		output_4= iadd(ifir16(c4,input),output_5);
		output_5= iadd(ifir16(c5,input),output_6);
		output_6= iadd(ifir16(c6,input),output_7);
		output_7= ifir16(c7,input);

		y[i] = yi;
   }

#if (TM_UNROLL && TM_UNROLL_FILTER)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif


	mem[0] = output_0;
	mem[1] = output_1;
	mem[2] = output_2;
	mem[3] = output_3;
	mem[4] = output_4;
	mem[5] = output_5;
	mem[6] = output_6;
	mem[7] = output_7;
}

#define OVERRIDE_FILTER_MEM16
void filter_mem16(const Int16 *x, const Int16 *num, const Int16 *den, Int16 *y, int N, int ord, Int32 *mem, char *stack)
{
	TMDEBUG_ALIGNMEM(x);
	TMDEBUG_ALIGNMEM(y);
	TMDEBUG_ALIGNMEM(num);
	TMDEBUG_ALIGNMEM(den);

	FILTERMEM16_START();

	if(ord==10)
		filter_mem16_10(x, num, den, y, N, mem);
	else if (ord==8)
		filter_mem16_8(x, num, den, y, N, mem);

#ifndef REMARK_ON
	(void)stack;
#endif

	FILTERMEM16_STOP();
}

void iir_mem16_8(const Int16 *x, const Int16 *den, Int16 *y, int N, Int32 *mem)
{
	register int i;
	register int c67, c45, c23, c01;
	register int r1, r2, r3;
	register int y10, y32, y54, y76, yi;
	
	c67 = pack16lsb(-den[6],-den[7]);
	c45 = pack16lsb(-den[4],-den[5]);
	c23 = pack16lsb(-den[2],-den[3]);
	c01 = pack16lsb(-den[0],-den[1]);

	y10 = mem[0];
	y32 = mem[1];
	y54 = mem[2];
	y76 = mem[3];

#if (TM_UNROLL && TM_UNROLL_IIR)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif

	for ( i=0 ; i < N ; ++i )
	{		
		r2 = iadd(ifir16(y10,c67),ifir16(y32,c45));
		r3 = iadd(ifir16(y54,c23),ifir16(y76,c01));
		r1 = iadd(r2,r3);

		y10 = funshift2(y32, y10);
		y32 = funshift2(y54, y32);
		y54 = funshift2(y76, y54);
		
		yi	= iclipi(iadd((int)(x[i]),PSHR32(r1,LPC_SHIFT)),32767);
		y[i]= yi;
		y76 = funshift2(yi, y76);
	}

#if (TM_UNROLL && TM_UNROLL_IIR)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	mem[0] = y10;
	mem[1] = y32;
	mem[2] = y54;
	mem[3] = y76;

}

void iir_mem16_10(const Int16 *x, const Int16 *den, Int16 *y, int N, Int32 *mem)
{
	register int i;
	register int c89, c67, c45, c23, c01;
	register int r1, r2, r3, r4, r5;
	register int y10, y32, y54, y76, y98, yi;
	
	c89 = pack16lsb(-den[8],-den[9]);
	c67 = pack16lsb(-den[6],-den[7]);
	c45 = pack16lsb(-den[4],-den[5]);
	c23 = pack16lsb(-den[2],-den[3]);
	c01 = pack16lsb(-den[0],-den[1]);

	y10 = mem[0];
	y32 = mem[1];
	y54 = mem[2];
	y76 = mem[3];
	y98 = mem[4];

#if (TM_UNROLL && TM_UNROLL_IIR)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif

	for ( i=0 ; i < N ; ++i )
	{		
		r2 = iadd(ifir16(y10,c89),ifir16(y32,c67));
		r3 = iadd(ifir16(y54,c45),ifir16(y76,c23));
		r4 = ifir16(y98,c01);
		r5 = iadd(r2,r3);
		r1 = iadd(r4,r5);

		y10 = funshift2(y32, y10);
		y32 = funshift2(y54, y32);
		y54 = funshift2(y76, y54);
		y76 = funshift2(y98, y76);
		
		yi	= iclipi(iadd((int)(x[i]),PSHR32(r1,LPC_SHIFT)),32767);
		y[i]= yi;
		y98 = funshift2(yi, y98);		
	}

#if (TM_UNROLL && TM_UNROLL_IIR)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	mem[0] = y10;
	mem[1] = y32;
	mem[2] = y54;
	mem[3] = y76;
	mem[4] = y98;
}

#define OVERRIDE_IIR_MEM16
void iir_mem16(const Int16 *x, const Int16 *den, Int16 *y, int N, int ord, Int32 *mem, char *stack)
{
	TMDEBUG_ALIGNMEM(den);

	IIRMEM16_START();

	if(ord==10)
		iir_mem16_10(x, den, y, N, mem);
	else if (ord==8)
		iir_mem16_8(x, den, y, N, mem);

#ifndef REMARK_ON
	(void)stack;
#endif

	IIRMEM16_STOP();
}

void fir_mem16_8(const Int16 *x, const Int16 *num, Int16 *y, int N, Int32 *mem)
{
	register int i, N_2;
	register int c67, c45, c23, c01;
	register int b0, b1, b2, b3;
	register int r1, r2, r3;
	register int x10, x32, x54, x76, xi;
	register Int16 *a; 

	N_2 = N >> 1;

	c67 = ld32x(num,3);
	c45 = ld32x(num,2);
	c23 = ld32x(num,1);
	c01 = ld32x(num,0);

	c67 = funshift2(c67,c67);
	c45 = funshift2(c45,c45);
	c23 = funshift2(c23,c23);
	c01 = funshift2(c01,c01);

	b3 = x76 = ld32x(x,N_2-1);
	b2 = x54 = ld32x(x,N_2-2);
	b1 = x32 = ld32x(x,N_2-3);
	b0 = x10 = ld32x(x,N_2-4);

#if (TM_UNROLL && TM_UNROLL_FILTER > 0)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif

	for ( i=N-1 ; i >= 8 ; --i )
	{
		xi  = asri(16,x76);
		x76 = funshift2(x76, x54);
		x54 = funshift2(x54, x32);
		x32 = funshift2(x32, x10);
		x10 = pack16lsb(x10, (int)x[i-8]);

		r2 = iadd(ifir16(x10,c67),ifir16(x32,c45));
		r3 = iadd(ifir16(x54,c23),ifir16(x76,c01));
		r1 = iadd(r2,r3);
		
		y[i] = iclipi(iadd(xi,PSHR32(r1,LPC_SHIFT)),32767);
	}
	for ( i=7, a=(Int16*)mem ; i>=0 ; --i )
	{
		xi  = asri(16,x76);
		x76 = funshift2(x76, x54);
		x54 = funshift2(x54, x32);
		x32 = funshift2(x32, x10);
		x10 = pack16lsb(x10, (int)a[i]);
	
		r2 = iadd(ifir16(x10,c67),ifir16(x32,c45));
		r3 = iadd(ifir16(x54,c23),ifir16(x76,c01));
		r1 = iadd(r2,r3);
		
		y[i] = iclipi(iadd(xi,PSHR32(r1,LPC_SHIFT)),32767);
	}

#if (TM_UNROLL && TM_UNROLL_FILTER > 0)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	mem[0] = b0;
	mem[1] = b1;
	mem[2] = b2;
	mem[3] = b3;
}

void fir_mem16_10(const Int16  *x, const Int16 *num, Int16  *y, int N, Int32  *mem)
{
	register int N_2, i;
	register int c89, c67, c45, c23, c01;
	register int b0, b1, b2, b3, b4;
	register int r1, r2, r3, r4, r5;
	register int x10, x32, x54, x76, x98, xi;
	register Int16 *a; 

	N_2 = N >> 1;

	c89 = ld32x(num,4);
	c67 = ld32x(num,3);
	c45 = ld32x(num,2);
	c23 = ld32x(num,1);
	c01 = ld32x(num,0);

	c89 = funshift2(c89,c89);
	c67 = funshift2(c67,c67);
	c45 = funshift2(c45,c45);
	c23 = funshift2(c23,c23);
	c01 = funshift2(c01,c01);

	b4 = x98 = ld32x(x,N_2-1);
	b3 = x76 = ld32x(x,N_2-2);
	b2 = x54 = ld32x(x,N_2-3);
	b1 = x32 = ld32x(x,N_2-4);
	b0 = x10 = ld32x(x,N_2-5);

#if (TM_UNROLL && TM_UNROLL_FIR > 0)
#pragma TCS_unroll=5
#pragma TCS_unrollexact=1
#endif

	for ( i=N-1 ; i >= 10 ; --i )
	{
		xi  = asri(16,x98);
		x98 = funshift2(x98, x76);
		x76 = funshift2(x76, x54);
		x54 = funshift2(x54, x32);
		x32 = funshift2(x32, x10);
		x10 = pack16lsb(x10, (int)(x[i-10]));

		r2 = iadd(ifir16(x10,c89),ifir16(x32,c67));
		r3 = iadd(ifir16(x54,c45),ifir16(x76,c23));
		r4 = ifir16(x98,c01);
		r5 = iadd(r2,r3);
		r1 = iadd(r4,r5);
		
		y[i] = iclipi(iadd(xi,PSHR32(r1,LPC_SHIFT)),32767);
	}

	for ( i=9,a =(Int16*)mem ; i>=0 ; --i )
	{
		xi  = asri(16,x98);
		x98 = funshift2(x98, x76);
		x76 = funshift2(x76, x54);
		x54 = funshift2(x54, x32);
		x32 = funshift2(x32, x10);
		x10 = pack16lsb(x10, (int)(a[i]));

		r2 = iadd(ifir16(x10,c89),ifir16(x32,c67));
		r3 = iadd(ifir16(x54,c45),ifir16(x76,c23));
		r4 = ifir16(x98,c01);
		r5 = iadd(r2,r3);
		r1 = iadd(r4,r5);
		
		y[i] = iclipi(iadd(xi,PSHR32(r1,LPC_SHIFT)),32767);
	}

#if (TM_UNROLL && TM_UNROLL_FIR > 0)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	mem[0] = b0;
	mem[1] = b1;
	mem[2] = b2;
	mem[3] = b3;
	mem[4] = b4;


}

#define OVERRIDE_FIR_MEM16
void fir_mem16(const spx_word16_t *x, const Int16 *num, spx_word16_t *y, int N, int ord, Int32 *mem, char *stack)
{
	TMDEBUG_ALIGNMEM(x);
	TMDEBUG_ALIGNMEM(y);
	TMDEBUG_ALIGNMEM(num);

	FIRMEM16_START();