iir.s54

c5x中关于IIR滤波器的实验

;******************************************************************************
;               		SJTU DSP Tech. Center
;        Copyright (c) 2003 SJTU DSP Tech. Center. All Rights Reserved.
;
;  Description:
;  		TMS320C54x Program for Students Experiment
;
;  History:
;		Date		Authors			Changes
;		2003/17/07	Fu Xuan			Created.
;******************************************************************************
STACK_ADDR	.set	0x0500

	;align (2^N > circular buffer length)
	.bss	IN_BUF,	16	;iir0--x(n), x(n-1), x(n-2)...x(n-5)
						;iir1--d(n), d(n-1), d(n-2)...d(n-8)
	.bss	OUT_BUF, 8	;y(n), y(n-1), y(n-2)...y(n-5)			
	.bss	INPUT, 2000
	.bss	OUTPUT, 2000

	.mmregs
	.global	main		
	.text
			
main:
	stm		#STACK_ADDR, SP
	stm		#0x00a8, PMST	;IPTR = 0x0080
	stm		#0x0000, SWWSR	;reset software wait-state register
	ssbx	FRCT
	ssbx	SXM
		
;Clear Buffer
	ld		#0, A
	stm		#IN_BUF, AR1
	rpt		#9-1
	stl		A, *AR1+

	stm		#OUT_BUF, AR1
	rpt		#6-1
	stl		A, *AR1+

;add probe point to get data
	nop
;choose one to use
	;call	iir0	
	call	iir1

dead_loop:
	nop
	nop
	nop
	nop
	b	dead_loop
	
;IIR Filtering
;********************************************************************************
;Direct I
iir0:
	pshm	AR0
	pshm	AR1
	pshm	AR2
	pshm	AR3
	pshm	AR4
	pshm	AR5
	pshm	AR6
	pshm	AR7
	
	stm		#INPUT, AR1
	stm		#OUTPUT, AR6
	stm		#IN_BUF, AR2	;AR2 -> x(n)
	stm		#OUT_BUF+1, AR3	;AR3 -> y(n-1)
	stm		#COEF_A, AR4
	stm		#COEF_B, AR5
	stm		#6, BK			;circular buffer length = 6
	stm		#1, AR0
	stm		#2000-1, AR7	;to filter 2000 input data

iir0_loop:
	ld		*AR1+, -1, A	;avoid overflow, x(n)/2->A
	stl		A, *AR2			;save the newest one input
;ARx+0% can only use {AR2,AR3,AR4,AR5}
	;Forward Path
	mpy		*AR5+0%, *AR2+0%, A	;A = b0 * x(n)
	rpt		#4-1				;A = b0 * x(n) + b1 * x(n-1) + ... + b4 * x(n-4)
	mac		*AR5+0%, *AR2+0%, A
	mac		*AR5+0%, *AR2, A	;here, AR2 point to oldest data in x[]
								;which may save the newset one next iteration
	;Backward Path
	stm		#3-1, BRC			;because a1, a2, a3 divide 4, result*4 then add
	rptb	next - 1
	mpy		*AR4+0%, *AR3+0%, B
	add		B, 2, A				;B * 4 = a1 * y(n-1)
next:
	mpy		*AR4+0%, *AR3+0%, B
	add		B, 1, A	
	mac		*AR4+0%, *AR3+0%, A
	mar		*AR4+0%			;AR4->COEF_A( only 5 coeffcients for COEF_A while BK=6)
							;one more pointer adjust
	banzd	iir0_loop, *AR7-
	sth		A, *AR6+		;svae y(n) to OUTPUT[]
	sth		A, *AR3			;save y(n) to OUT_BUF, circular buffer for next
	
	popm	AR7
	popm	AR6
	popm	AR5
	popm	AR4
	popm	AR3
	popm	AR2
	popm	AR1
	popm	AR0
	
	ret
;********************************************************************************
;Cascade II
iir1:
	pshm	AR1
	pshm	AR2
	pshm	AR3
	pshm	AR6
	pshm	AR7
	
	stm		#INPUT, AR1
	stm		#OUTPUT, AR6
	stm		#2000-1, BRC	;2000 input data
	rptb	iir1_loop - 1

	ld		*AR1+, 16-1, A	;ld x(n)/2->AH, avoid overflow
iir_filter:
	stm		#IN_BUF+8, AR2	;AR2->&d(n-2), {d(n), d(n-1), d(n-2)}
	stm		#CAS_COEF, AR3	;AR3->CAS_COEF, {A2, A1, B2, B1, B0}
	stm		#3-1, AR7		;3 stages
	
backward_path:
	mac		*AR3+, *AR2-, A	;A = x(n)+d(n-2)*A2
	mac		*AR3+, *AR2-, A	;A = x(n)+d(n-2)*A2+d(n-1)*A1
	sth		A, *AR2+		;d(n)=A
	mar		*AR2+			;AR2->d(n-2)
	
forward_path:
	mpy		*AR3+, *AR2-, A	;A = d(n-2)*B2
	mac		*AR3+, *AR2, A	;A = d(n-2)*B2+d(n-1)*B1
	delay 	*AR2-			;d(n-2)=d(n-1), AR2->d(n)
	banzd	backward_path, *AR7-
	mac		*AR3+, *AR2, A	;A = d(n-2)*B2+d(n-1)*B1+d(n)*B0
	delay 	*AR2-			;d(n-1)=d(n), AR2->&d(n-2) in next stage
	
	sth		A, *AR6+		;store result to OUTPUT
iir1_loop:

	popm	AR7
	popm	AR6
	popm	AR3
	popm	AR2
	popm	AR1
	ret

;********************************************************************************	
;Filter Coefficient

	.data		;Q15 format
;y(n)=sum(b[k]*x[n-k])-sum(a[j]*y[n-j])
	.align	8
COEF_B:
	.word	243		;b0 = 0.0074171
	.word   1215	;b1 = 0.037085
	.word   2430	;b2 = 0.074171
	.word   2430 	;b3 = 0.074171
	.word   1215	;b4 = 0.037085
	.word   243		;b5 = 0.0074171
			
	.align 	8
COEF_A:
	;make a[j]==>-a[j]
	.word   18499	;a1/4 = -2.2582/4
	.word   -24462	;a2/4 = 2.9861/4
	.word   19241	;a3/4 = -2.3487/4
	.word   -18227	;a4/2 = 1.1125/2
	.word   8333	;a5 = -0.2543
					
CAS_COEF:
	.word	-26686	;1st stage A2 = 0.8144
	.word	21863	;1st stage A1 = -0.6672
	.word	7758	;1st stage B2 = 0.2 * 1.1837
	.word	14210	;1st stage B1 = 0.2 * 2.1682
	.word	6554	;1st stage B0 = 0.2
	.word	-16767	;2nd stage A2 = 0.5117
	.word	32139	;2nd stage A1 = -0.9808
	.word	5537	;2nd stage B2 = 0.2 * 0.8449
	.word	12006	;2nd stage B1 = 0.2 * 1.8318
	.word	6554	;2nd stage B0 = 0.2
	.word	0		;3rd stage A2
	.word	19995	;3rd stage A1 = -0.6102
	.word	0		;3rd stage B2
	.word	6075	;3rd stage B1 = 0.0074171*32768/(6554/32768)^2
	.word	6075	;3rd stage B0 = 0.0074171*32768/(6554/32768)^2
	
;****************************************************************
	.sect	"vectors"
int_RESET:
	b	main
	nop
	nop
		
	.space 124*16
		
	.end
;end of IIR.s54