ec.asm

blackfin中实现的AEC算法

/*********************************************************************************
Module Name     : .asm
Label Name      : 


   Version     Date          Author        Comments
    
   1.0         01/02/2007     HuBo      	Original 

Description     : 	This function performs FIR filter operation on given input.

Prototype       :	void	_ec(ec_handle svptr, short *pfar_end_sig, short *pnear_end_sig)

return 			:  	None                  
          
Performance		:       
*******************************************************************************/
/*Define the global data*/

.extern _updatefilt;
.section L1_code;
.global _ec;
.align 8;


_ec:
	LINK 0x30;
	[--SP] = (R7:4);        		// Save registers R4-R7 
	
	P0 = R0;						/*Address of context*/
	I0 = P0;						/*Address of s_register[0]*/
	P4 = R0;						/*Address of context*/
	M0 = 512;						/*the modification offset*/
	I0 += M0;						/*Address of s_register[256]*/
	P4 = I0;						/*Address of s_register[256]*/
	I2 = R1;						/*Address of far-end signal*/
	I3 = R2;						/*Address of near-end signal*/
	P1 = 80;						/*the frame length = 80samples*/
	P2 = 256;						/*the number of the taps*/
	R4 = 1000;						/*the threshlod of power*/
	R5 = 1600;						/*update filter coefficient per 200ms*/
	R7 = 0x7fff;
	M1 = 672;						/*the modification offset*/
	M2 = 160;
	M3 = 2;
	M0 = 4;
	
	/*update the filter coefficient*/
	LSETUP(tap_beg,tap_end) LC0 = P1>>1;
tap_beg: R0 = [I2++]; 

tap_end: [I0++] = R0;
	
	I2 -= M2;
	
	LSETUP(aec_beg,aec_end) LC1 = P1;
aec_beg:	
		R1.L = W[I2++];
		R2.L = W[I3];					/*increase I3 after this loop*/
	
		R0 = [P0+1184];					/*value of power_e*/ 
		R3 = R0 >>> 8;
		R0 = R0 - R3; 					/*Store R0*63/64 into R0*/
		R3 = R2.L*R2.L(IS);
		R3 >>>= 8;						/*Arithmetic right shift*/
		R0 = R0 + R3;
		//R0 = (A0 += R2.L*R2.L)(IS);	/*Store the power_e into R0*/
		[P0+1184] = R0;					/*Update the power_e*/
	
		R0 = [P0+1188];					/*value of power_s*/
		R3 = R0 >>> 8;
		R0 = R0 - R3;					/*Store R0*63/64 into R0*/
		R3 = R1.L*R1.L(IS);
		R3 >>>= 8;						/*Arithmetic right shift*/
		R0 = R0 + R3;
		//R0 = (A0 += R1.L*R1.L)(IS);	/*Store the power_s into R0*/
		[P0+1188] = R0;					/*Update the power_s*/
		
		CC = R0 <= R4;					/*power_s*/
		if CC JUMP modify_idx;			/*if the power of far-end signal is too low,jump to modify_idx*/
		R0 = [P0+1184];					/*load power_e*/
		CC = R0 <= R4;					
		if CC JUMP modify_idx;			/*if the power of near-end signal is too low,jump to modify_idx*/
		
		/*filter*/
		A0 = 0;
		A1 = 0;	
		I0 = P0;		
		I0 += M1;						/*Address of h[0]*/
								
		I1 = P4;						/*Address of s_register[256+i]*/	
			
		R1 = I1;						/*store address of s_register[256+i]*/
		R3 = 0x00000003;
		R3 = R1 & R3;
		CC = R3 == 0;
		if !CC JUMP	filter_1;

filter_0:			/*the address of s_register[256+i] is aligned by 4-byte */
		MNOP || R0 = [I0++] || R1.L = W[I1--];
		I1 -= M3;
		LSETUP(filter_beg_0,filter_end_0) LC0 = P2>>1;
					/*a0+=h[i++]*s[n--] */
filter_beg_0:	
		A0 += R0.L * R1.L (IS) || R1 = [I1--] || NOP;
filter_end_0:	
		A1 += R0.H * R1.H (IS) || R0 = [I0++] || NOP;		
		JUMP filter_end;
		
filter_1:		/*the address of s_register[256+i] isn't aligned by 4-byte */
		MNOP || R0 = [I0++] || I1 -= M3;
		//MNOP || R0 = [I0++] || R1.H = [I1--];
		R1 = [I1--];
		LSETUP(filter_beg_1,filter_beg_1) LC0 = P2>>1;
					/*a0+=h[i++]*s[n--];*/	
filter_beg_1: A0 += R0.L * R1.H , A1 += R0.H * R1.L(IS) || R0 = [I0++] || R1 = [I1--];

filter_end:

		A0 = A0 >>> 15;				//h(k) is Q15;
		A1 = A1 >>> 15;
		R0 = A0, R1 = A1;
		R3 = R0 + R1;				/*The output of filter*/
		//R2.L = W[I3];				/*Load signal value into R2*/
		R2 = R2.L(X);				/*sign extended*/
		R2 = R2 - R3;				/*Subtract the echo signal*/
		//R2 = R2.L(X);				/*R2 is the signal which has been taken out the echo*/
		W[I3] = R2.L;				/*store the R2*/
		
		R2 = [P0+1184];				/*the power of echo*/
		R0 = R2 >>> 2;				/*R0 is one fourth of R2*/ 
		R3 = [P0+1188];				/*The power of signal*/
		CC = R3 <= R0;				/*Double talk*/
		if CC JUMP double_talk;
									/*single talk*/
		R3 = W[P0+1194];			/*talk_status*/
		CC = R3 == 0; 
		if CC JUMP talk_status;		/*if talk_status == 1 ,set talk_status=0,st_count=1*/
		R3 = 0;
		W[P0+1194] = R3;
		R3 = 1;
		W[P0+1192] = R3;
		
talk_status:		
		R3 = W[P0+1192];			/*st_count*/
		CC = R5 <= R3;				/*if R3 == 1600; update filter coefficient*/
		if CC JUMP update_filter;	
		R3 += 1;
		W[P0+1192] = R3;			/*increase the st_count*/
		JUMP modify_idx;			/*end this loop*/

double_talk:
		R3 = 1;
		W[P0+1194] = R3;	
		JUMP modify_idx;			/*End this loop*/
			
update_filter:	
		R1.L = 0;
		R1.H = 0x8;					/*R1 is the positive threshold*/
		R0.L = 0x0;
		R0.H = 0xFFF8;				/*R0 is the negative threshold*/
		R3 = [P0+1188];				/*load power_s*/ 
		CC = R1 <= R3;
		IF CC JUMP shift_1; 
		CC = R3 <= R0;
		IF CC JUMP shift_1;
		R3 = R3 >>> 5;				/*Arithmetic right shift*/
		R2.L = W[I3];				/*Load R2*/
		R2 = R2.L(X);
		R2 = R2 << 15(S);			/*Arithmetic left shift*/
		P3 = 16;                	/*Loop counter for division */
		DIVS(R2, R3);            	/*To reset the AQ flag */

    	LSETUP(DIV_ST0,DIV_ST0) LC0 = P3;
DIV_ST0: 
		DIVQ(R2,R3);        		/*Do divq 16 times */
		R2 = R2.L(X);				/*sign extended*/	
		JUMP div_end;
shift_1:
		R1.L = 0;
		R1.H = 0x80;				/*R1 is the positive threshold*/
		R0.L = 0;
		R0.H = 0xFF80;				/*R0 is the negative threshold*/
		R3 = [P0+1188];				/*Load power_s*/
		
		CC = R1 <= R3;
		IF CC JUMP shift_2; 
		CC = R3 <= R0;
		IF CC JUMP shift_2;
		
		R3 = R3 >>> 9;				/*Arithmetic right shift*/
		R2.L = W[I3];				/*Load R2*/
		R2 = R2.L(X);
		R2 = R2 << 11(S);			/*Arithmetic left shift*/
		P3 = 16;                	/*Loop counter for division */
		
		DIVS(R2, R3);            	/*To reset the AQ flag */
    	LSETUP(DIV_ST1,DIV_ST1) LC0 = P3;
DIV_ST1: 
		DIVQ(R2,R3);        		/*Do divq 16 times */
		R2 = R2.L(X);				/*sign extended*/	
		JUMP div_end;

shift_2:
		R3 = [P0+1188];				/*load power_s*/
		R3 = R3 >>> 13;				/*Arithmetic right shift*/
		R2.L = W[I3];				/*Load R2*/
		R2 = R2.L(X);
		R2 = R2 << 7(S);			/*Arithmetic left shift*/
		P3 = 16;                	/*Loop counter for division */
		
		DIVS(R2, R3);            	/*To reset the AQ flag */
    	LSETUP(DIV_ST2,DIV_ST2) LC0 = P3;
DIV_ST2: 
		DIVQ(R2,R3);        		/*Do divq 16 times */
		R2 = R2.L(X);				/*sign extended*/	
		
div_end:
		B2 = I2;					//store I2
		I0 = P0;		
		I0 += M1;					/*Address of h[0]*/
		I2 = I0;
		R0 = [I0++];
		R1 = P4;					/*Load R1, address of s_register[256+i]*/
		I1 = R1;
		R3 = 0x00000003;
		R3 = R1 & R3;
		CC = R3 == 0;
		if !CC JUMP	update_1;

update_0:	

	/*initiate a0, a1 and R3, a0 and a1 left ashift 15*/
	R1.L = W[I1--]; 
	LSETUP(update_coef_beg_0, update_coef_end_0) LC0 = P2>>1;	
update_coef_beg_0:	A0 = R0.L * R7.L, A1 = R0.H * R7.L || R1.H = W[I1--] ;	/*a0 and a1 left shift 16 bit*/

/*the high half of the result is extracted and stored in the 16-bit destination registers */
					R0.L = (A0 += R2.L * R1.L) , R0.H = (A1 += R2.L * R1.H) || R1.L = W[I1--];
update_coef_end_0:	R3 = R2 + R1(S) ||[I2++M0] = R0 || R0 = [I0++M0];//store R0 and increase I0;
					//NOP;
	jump update_end;

update_1:

/*the address of s_register[256+i] isn't aligned by 4-byte */	
	I1 -= M3;
	LSETUP(update_coef_beg_1, update_coef_end_1) LC0 = P2>>1;
update_coef_beg_1:	A0 = R0.L * R7.L, A1 = R0.H * R7.L || R1 = [I1--] ;/*a0 and a1 left shift 16 bit*/

/*the high half of the result is extracted and stored in the 16-bit destination registers */
					
					R0.L = (A0 += R2.L * R1.H), R0.H = (A1 += R2.L * R1.L);// || R1.L = W[I1--];
update_coef_end_1:	MNOP || [I2++M0]=R0 || R0 = [I0++M0];//store R0 and increase I0;

update_end:	I2 = B2;	//load I2;
modify_idx:	
			I3 += M3;
aec_end: 
			P4 +=2 ;//|| I3 += M3 ;					/*increase P4 each loop*/

	/*reserve the s_register for next time*/
	I0 = P0;
	I1 = P0;
	I1 += M2;
	
	LSETUP(tap_update_beg,tap_update_end) LC0 = P2>>1;
tap_update_beg: R0 = [I1++]; 

tap_update_end: [I0++] = R0;

	(R7:4) = [SP++];        		// Load registers R4-R7 

	UNLINK;
	RTS;

_ec.end: