echo2.s

关于DVD的MPEG2用的DSP代码,在DSP的实现MPEG的压缩,解压算法.

		dmawr	r0
		WaitDma
#else	//EFFECT
		movi	ByteLocal, local_DL2+0x80
		movi	ByteSize, 0x12
		dmabw	DL2_START
#endif	//EFFECT

;********* il faut sauver la premiere sortie avant de faire *******
;********* la seconde partie du reverberator **********************



;*******************************************************************
;********* SECOND PART OF REVERBERATOR *****************************

;********* OAP2 HISTORY (V(n-T5))***********************************
;********* load 64 samples of the history of OAP2 (T5=396) *********
//		movhf	r0, %OAP2_ptr	; base of the block		tchou
		movh	r0, %OAP2_ptr	; base of the block
		addi	r1, r0, 0x80	; add 64 samples 
		tsti	r1, OAP2_END	; test if wrapping is required or not
		blte	no_OAP2_wrapp
; wrapping required, split the block in two pieces....
; bytes required for the second transfert=r2
#ifdef	EFFECT
		subi	r2,r1,OAP2_SIZE+OAP2_START+4 

; bytes required for the first transfert=r3
		movi	r3,0x78
		sub	r3, r2
		addi	r4, r3,local_OAP2+4
// do in words
		shr	r0,2
		shr	r2,2
		shr	r3,2
		shr	r4,2
//first DMA in words
		mov	DMASize, r3					
		movi	LocalAddr, (local_OAP2)>>2
		dmarr	r0
		WaitDma

//second DMA in words
		movi	r0,(OAP2_START)>>2
		mov	DMASize, r2			
		mov	LocalAddr, r4
		dmarr	r0
		WaitDma
#else	//EFFECT
		subi	r2,r1,OAP2_SIZE+OAP2_START+2 

; bytes required for the first transfert=r3
		movi	r3,0x7c
		sub		r3, r2			

; first transfert
		mov		ByteSize, r3
		movi	ByteLocal, local_OAP2
		dmabrr	r0
; second transfert
		addi	r4, r3,local_OAP2+2
		mov		ByteSize, r2
		mov		ByteLocal, r4
		dmabr	OAP2_START
#endif	//EFFECT
		j		OAP2_load_done
no_OAP2_wrapp:
; no need to wrap....
#ifdef	EFFECT
		shr	r0,2			//in words
		movi	DMASize, 31		//32 words (0x80 bytes)			
		movi	LocalAddr, (local_OAP2)>>2
		dmarr	r0
		WaitDma
#else	//EFFECT
		movi	ByteSize, 0x7e
		movi	ByteLocal,local_OAP2 
		dmabrr	r0	 
#endif	//EFFECT		
OAP2_load_done:

;;; sauver le resultat du premier filtrage pour l'echo avant d'ecraser tout
;;; avec le traitement du deuxieme.

;*******************************************************************
;******** OAP2 *****************************************************
		movi	AGRAdr2, local_DL2	; points on X/U 
		movi	AGRAdr3, local_OAP2	; points on V/Y
		movi	AGRAdr0, local_micro_in	; points on echo output 
		movi	AGRInc0, 0x2
		movi	AGRSiz0, 0xffff

		movi	AGRAdr1, local_OAP1	; points on output of first AP filter block
		movi	AGRInc1, 0x2
		movi	AGRSiz1, 0xffff


		movhf	r3, %g5
//		movi	r0, 0x7fff		
//		movi	r1, 0x4000		; 0.5 for the output ! tchou
		li		r0,0x7fffff
		li		r1,0x7fffff		;tchou 0x400000

;********** deseable interrupt **************
		movi	TrapReg, 0x0
		nop
		nop

		loop	64, OAP2_loop
;********** update Yn **********************************************
		mulhf	r6, r0, a3(0)	; r6=V(n-M2)*32767	
		msubhf	r6, r3, a2(0)	; r6=V(n-M2)*32767-g5*X(n)	
		rndhf	a3(0), r6		; Y(n)

;********** update Un **********************************************
		mulhf	r6, r0, a2(0)	; r6=X(n)*32767
		maddhf	r6, r3, a3(0)	; r6=X(n)*32767+g5*Y(n), increments to get out
		rndhf	i2, r6			; U(n)

;********** generate the output of the echo ************************
		mulhf	r6, r1, a3(2)	; r6=Y(n)*16384
		maddhf	r6, r1, i1		; r6=(Y(n)+X(n))*16384=echo(n)
		rndhf	i0, r6			; echo(n), ready for the rate conversion 
	
OAP2_loop:

;********* enable interrupt *******************
		movi	TrapReg, 1<<2

;********* NAP3 HISTORY (T(n-M4))***********************************
;********* load 64 samples of the history of NAP3 (M4=330) *********
//		movhf	r0, %NAP3_ptr	; base of the block	tchou
		movh	r0, %NAP3_ptr	; base of the block
		addi	r1, r0, 0x80	; add 64 samples 
		tsti	r1, NAP3_END	; test if wrapping is required or not
		blte	no_NAP3_wrapp
; wrapping required, split the block in two pieces....
; bytes required for the second transfert=r2
#ifdef	EFFECT
		subi	r2,r1,NAP3_SIZE+NAP3_START+4 

; bytes required for the first transfert=r3
		movi	r3,0x78
		sub	r3, r2
		addi	r4, r3,local_NAP3+4
// do in words
		shr	r0,2
		shr	r2,2
		shr	r3,2
		shr	r4,2
//first DMA in words
		mov	DMASize, r3					
		movi	LocalAddr, (local_NAP3)>>2
		dmarr	r0
		WaitDma

//second DMA in words
		movi	r0,(NAP3_START)>>2
		mov	DMASize, r2			
		mov	LocalAddr, r4
		dmarr	r0
		WaitDma
#else	//EFFECT
		subi	r2,r1,NAP3_SIZE+NAP3_START+2 

; bytes required for the first transfert=r3
		movi	r3,0x7c
		sub		r3, r2			

; first transfert
		mov		ByteSize, r3
		movi	ByteLocal, local_NAP3
		dmabrr	r0
; second transfert
		addi	r4, r3,local_NAP3+2
		mov		ByteSize, r2
		mov		ByteLocal, r4
		dmabr	NAP3_START
#endif	//EFFECT
		j		NAP3_load_done
no_NAP3_wrapp:
; no need to wrap....
#ifdef	EFFECT
		shr	r0,2			//in words
		movi	DMASize, 31		//32 words (0x80 bytes)			
		movi	LocalAddr, (local_NAP3)>>2
		dmarr	r0
		WaitDma
#else	//EFFECT
		movi	ByteSize, 0x7e
		movi	ByteLocal,local_NAP3 
		dmabrr	r0	 
#endif	//EFFECT		
NAP3_load_done:


;*******************************************************************
;******** NAP3 *****************************************************
		movi	AGRAdr2, local_DL2	; points on U/T 
		movi	AGRAdr3, local_NAP3	; points on T/V

		movhf	r3, %g4
//		movi	r0, 0x7fff	
		li		r0, 0x7fffff
;********** deseable interrupt ************
		movi	TrapReg, 0x0
		nop
		nop

		loop	64, NAP3_loop
;********** update Vn **********************************************
		mulhf	r6, r0, a3(0)	; r6=T(n-M4)*32767	
		msubhf	r6, r3, a2(0)	; r6=T(n-M4)*32767-g4*U(n)	
		rndhf	a3(0), r6		; V(n)

;********** update Tn **********************************************
		mulhf	r6, r0, a2(0)	; r6=U(n)*32767
		maddhf	r6, r3, i3		; r6=U(n)*32767+g4*V(n), increments to get out
		rndhf	i2, r6			; T(n)

NAP3_loop:

;********* enable interrupt ****************
		movi	TrapReg, 1<<2

;********* UPDATE OAP2 and NAP3 HISTORY ****************************
;********* update the history of OAP2 ******************************
//		movhf	r0, %OAP2_ptr	; base of the block	tchou
		movh	r0, %OAP2_ptr	; base of the block
		addi	r1, r0, 0x80	; add 64 samples 
		tsti	r1, OAP2_END	; test if wrapping is required or not
		blte	no_OAP2_up_wrapp
; wrapping required, split the block in two pieces....
; bytes required for the second transfert=r2
#ifdef	EFFECT
		subi	r2,r1,OAP2_SIZE+OAP2_START+4 

; bytes required for the first transfert=r3
		movi	r3,0x78
		sub	r3, r2
		addi	r4, r3,local_NAP3+4
// do in words
		shr	r0,2
		shr	r2,2
		shr	r3,2
		shr	r4,2
//first DMA in words
		mov	DMASize, r3					
		movi	LocalAddr, (local_NAP3)>>2
		dmawr	r0
		WaitDma

//second DMA in words
		movi	r0,(OAP2_START)>>2
		mov	DMASize, r2			
		mov	LocalAddr, r4
		dmawr	r0
		WaitDma
#else	//EFFECT
		subi	r2,r1,OAP2_SIZE+OAP2_START+2 

; bytes required for the first transfert=r3
		movi	r3,0x7c
		sub		r3, r2			

; first transfert
		mov		ByteSize, r3
		movi	ByteLocal, local_NAP3
		dmabwr	r0
; second transfert
		addi	r4, r3,local_NAP3+2
		mov		ByteSize, r2
		mov		ByteLocal, r4
		dmabw	OAP2_START
#endif	//EFFECT
		j		OAP2_up_done
no_OAP2_up_wrapp:
; no need to wrap....
#ifdef	EFFECT
		shr	r0,2			//in words
		movi	DMASize, 31		//32 words (0x80 bytes)			
		movi	LocalAddr, (local_NAP3)>>2
		dmawr	r0
		WaitDma
#else	//EFFECT
		movi	ByteSize, 0x7e
		movi	ByteLocal,local_NAP3 
		dmabwr	r0	 
#endif	//EFFECT		
OAP2_up_done:

;********* update pointer on OAP2 in DRAM **************************

//		tstsi	r1, OAP2_END		tchou
		tsti	r1, OAP2_END
		blt		no_OAP2_ptr_wrap
		subi	r1, OAP2_SIZE	; wrap if necessary
no_OAP2_ptr_wrap:
		nop
//		movhf	%OAP2_ptr, r1	; update the pointer	tchou
		movh	%OAP2_ptr, r1	; update the pointer 

;********* update the history of NAP3 ******************************
//		movhf	r0, %NAP3_ptr	; base of the block		tchou
		movh	r0, %NAP3_ptr	; base of the block
		addi	r1, r0, 0x80	; add 64 samples 
		tsti	r1, NAP3_END	; test if wrapping is required or not
		blte	no_NAP3_up_wrapp
; wrapping required, split the block in two pieces....
; bytes required for the second transfert=r2
#ifdef	EFFECT
		subi	r2,r1,NAP3_SIZE+NAP3_START+4 

; bytes required for the first transfert=r3
		movi	r3,0x78
		sub	r3, r2
		addi	r4, r3,local_DL2+4
// do in words
		shr	r0,2
		shr	r2,2
		shr	r3,2
		shr	r4,2
//first DMA in words
		mov	DMASize, r3					
		movi	LocalAddr, (local_DL2)>>2
		dmawr	r0
		WaitDma

//second DMA in words
		movi	r0,(NAP3_START)>>2
		mov	DMASize, r2			
		mov	LocalAddr, r4
		dmawr	r0
		WaitDma

#else	//EFFECT
		subi	r2,r1,NAP3_SIZE+NAP3_START+2 

; bytes required for the first transfert=r3
		movi	r3,0x7c
		sub		r3, r2			
; first transfert
		mov		ByteSize, r3
		movi	ByteLocal, local_DL2
		dmabwr	r0
; second transfert
		addi	r4, r3,local_DL2+2
		mov		ByteSize, r2
		mov		ByteLocal, r4
		dmabw	NAP3_START
#endif	//EFFECT
		j		NAP3_up_done
no_NAP3_up_wrapp:
; no need to wrap....
#ifdef	EFFECT
		shr	r0,2			//in words
		movi	DMASize, 31		//32 words (0x80 bytes)			
		movi	LocalAddr, (local_DL2)>>2
		dmawr	r0
		WaitDma
#else	//EFFECT
		movi	ByteSize, 0x7e
		movi	ByteLocal,local_DL2 
		dmabwr	r0	 
#endif	//EFFECT		
NAP3_up_done:

;********* update pointer on NAP3 in DRAM **************************

//		tstsi	r1, NAP3_END	tchou
		tsti	r1, NAP3_END
		blt		no_NAP3_ptr_wrap
		subi	r1, NAP3_SIZE	; wrap if necessary
no_NAP3_ptr_wrap:
		nop
//		movhf	%NAP3_ptr, r1	; update the pointer 
		movh	%NAP3_ptr, r1	; update the pointer 



;;; OK jusque la
;*******************************************************************
;********* low pass filtering and feed back on the input ***********
;********* load input part required for feedback *******************
#if		0
;*********  load a new block in input buffer (DL1) *****************
		movi	ByteSize, 0x7e
		movi	StartAddrHigh, 0x9
		movi	ByteLocal, local_DL1FB
//		movhf	r0, %mic_data_ptr			tchou
		movh	r0, %mic_data_ptr	
		dmabrr	r0

;*********	increments the pointer on fake buffer ******************
;********* to be replaced by pointer on Micro Fifo *****************	
		addi	r0, 0x80
//		movhf	%mic_data_ptr, r0			tchou
		movh	%mic_data_ptr, r0			
#endif 

;********* load history of low pass filter *************************
;		movi	StartAddrHigh, 0x8
;		movi	StartAddrHigh, 0x7
#ifdef	EFFECT
		movi	r0,(LP_START)>>2		//in words
		movi	DMASize, 6		//7 words (14 coeffs)			
		movi	LocalAddr, (local_LP)>>2
		dmarr	r0
		WaitDma
#else	//EFFECT
		movi	StartAddrHigh, DataSeg	// 10-13-98
		movi	ByteSize, 0x1a	; 14 coefficients for history 
		movi	ByteLocal,local_LP
		dmabr	LP_START
#endif	//EFFECT

		movi	AGRAdr2,local_LP; points on the data to filter
		movi	AGRAdr0, LPFB	; points on the LP filter coefficients
		movi	AGRSiz0, 0x1c	; 15 coefficients

		movi	AGRAdr3,local_DL1FB		;points on input sequence for FB

//		movi	r0, 0x7fff	
		li		r0,0x7fffff	

;********** deseable interrupt ****************
		movi	TrapReg, 0x0
		nop
		nop

		loop	64, LP_filter 
		
		mulhf	r6, i0, i2
		loop	13, do_LP
		maddhf	r6, i0, i2
do_LP:
		maddhf	r6, i0, a2(-26)	; for the next sample

		rndhf	r4, r6			; rounds result of LP filtering
		mulhf	r6, r4, %FBgain	; scale the result of LP with gain
		maddhf	r6, r0, a3(0)	; add input 
		rndhf	i3, r6			; add feed back on input sequence
LP_filter:

;********* enable interrupt ********************
		movi	TrapReg, 1<<2

;********* update input part required for feedback *******************
#ifdef	EFFECT
//		movhf	r0, %DL1_ptr			tchou
		movh	r0, %DL1_ptr	
		shr	r1,r0,2			//in words
		movi	DMASize, 31		//32 words 			
		movi	LocalAddr, (local_DL1FB)>>2
		dmawr	r1
		WaitDma
#else	//EFFECT
		movi	ByteSize, 0x7e
		movi	ByteLocal, local_DL1FB
//		movhf	r0, %DL1_ptr			tchou
		movh	r0, %DL1_ptr	
		dmabwr	r0				; update input+feed back. 
#endif	//EFFECT
;********* update DL1_ptr ********************************************

		addi	r0, 0x80
		tsti	r0, DL1_END
		blt		no_DL1_wrapp
		subi	r0, DL1_SIZE
no_DL1_wrapp:
//		movhf	%DL1_ptr, r0		tchou
		movh	%DL1_ptr, r0

;********* update history of LP filter *******************************
#ifdef	EFFECT
		movi	r0,(LP_START)>>2		//in words
		movi	DMASize, 6		//7 words (14 coeffs)			
		movi	LocalAddr, ((local_LP)>>2+32)
		dmawr	r0
		WaitDma
#else	//EFFECT
		movi	ByteSize, 0x1a
		movi	ByteLocal, local_LP+0x80
		dmabw	LP_START
#endif	//EFFECT
;********* end of routine ********************************************
	
		j		r11				; go back to main routine

#ifndef	EFFECT
.rdata
LPfilter:	.align	2
		.short	0x68	;104
		.short	0xa7	;167
		.short	0xff5d	;-163
		.short	0xfb92	;-1134
		.short	0xfc4d	;-947
		.short	0xb7b	;2939
		.short	0x2401	;9217
		.short	0x306f	;12399
		.short	0x2401	;9217
		.short	0xb7b	;2939
		.short	0xfc4d	;-947
		.short	0xfb92	;-1134
		.short	0xff5d	;-163
		.short	0xa7	;167
		.short	0x68	;104
#endif	//EFFECT