1@***********************************************************
2@ Function:    WT_VoiceFilter
3@ Processor:   ARM
4@ Description:
5@ Implements a 2-pole low-pass filter with resonanance
6@
7@ Usage:
8@	void WT_VoiceFilter(
9@		S_FILTER CONTROL *pFilter,
10@		S_WT_FRAME *pWTFrame);
11@
12@ Copyright 2005 Sonic Network, Inc.
13@****************************************************************
14@ Revision Control:
15@   $Revision: 496 $
16@   $Date: 2006-12-11 14:33:26 -0800 (Mon, 11 Dec 2006) $
17@****************************************************************
18@
19@   where:
20@	S_FILTER_CONTROL *pFilter
21@	PASSED IN: r0
22@
23@	S_WT_FRAME *pWTFrame
24@	PASSED IN: r1
25@****************************************************************
26
27	.include	"ARM_synth_constants_gnu.inc"
28
29	.arm
30	.text
31
32
33	.global	WT_VoiceFilter
34
35
36@ Register usage
37@ --------------
38pFilter	.req	r0
39pWTFrame	.req	r1
40pBuffer	.req	r2
41numSamples	.req	r3
42
43z1	.req	r4
44z2	.req	r5
45b1	.req	r6
46b2	.req	r7
47K	.req	r8
48
49tmp0	.req	r1	@ reuse register
50tmp1	.req	r9
51tmp2	.req	r10
52
53
54@SaveRegs	RLIST	{r4-r10, lr}
55@RestoreRegs	RLIST	{r4-r10, pc}
56
57
58	.func	WT_VoiceFilter
59WT_VoiceFilter:
60
61	STMFD	sp!, {r4-r10, lr}
62
63@
64@ Setup passed parameters in their destination registers
65@----------------------------------------------------------------
66
67	LDR		pBuffer, [pWTFrame, #m_pAudioBuffer]
68	LDR		numSamples, [pWTFrame, #m_numSamples]
69
70	@load state variables from pFilter structure
71	LDRSH	z1, [pFilter, #m_z1]
72	LDRSH	z2, [pFilter, #m_z2]
73
74	@load coefficients from pWTFrame structure
75	LDR		K, [pWTFrame, #m_k]
76	LDR		b1, [pWTFrame, #m_b1]
77	LDR		b2, [pWTFrame, #m_b2]
78
79	RSB	b1, b1, #0						@ b1 = -b1
80	RSB	b2, b2, #0						@ b2 = -b2
81	MOV	b2, b2, ASR #1					@ b2 = b2 >> 1
82	MOV	K, K, ASR #1					@ K = K >> 1
83
84@
85@ Start processing
86@----------------------------------------------------------------
87
88	LDRSH	tmp0, [pBuffer]					@ fetch sample
89
90FilterLoop:
91	SMULBB	tmp2, z1, b1					@ tmp2 = z1 * -b1
92	SMLABB	tmp2, z2, b2, tmp2				@ tmp2 = (-b1 * z1) + (-b2 * z2)
93
94	MOV		z2, z1							@ delay line
95
96	SMLABB	tmp0, tmp0, K, tmp2				@ tmp1 = (K * x[n]) + (-b1 * z1) + (-b2 * z2)
97
98	LDRSH	tmp1, [pBuffer, #NEXT_OUTPUT_PCM]	@ fetch next sample
99
100	MOV		z1, tmp0, ASR #14				@ shift result to low word
101	STRH	z1, [pBuffer], #NEXT_OUTPUT_PCM	@ write back to buffer
102
103	SMULBB	tmp2, z1, b1					@ tmp2 = z1 * -b1
104
105	SUBS	numSamples, numSamples, #2		@ unroll loop once
106
107	SMLABB	tmp2, z2, b2, tmp2				@ tmp2 = (-b1 * z1) + (-b2 * z2)
108
109	SMLABB	tmp1, tmp1, K, tmp2				@ tmp1 = (K * x[n]) + (-b1 * z1) + (-b2 * z2)
110
111	MOV		z2, z1							@ delay line
112
113	MOV		z1, tmp1, ASR #14				@ shift result to low word
114
115	LDRGTSH	tmp0, [pBuffer, #NEXT_OUTPUT_PCM]	@ fetch next sample
116
117	STRH	z1, [pBuffer], #NEXT_OUTPUT_PCM	@ write back to buffer
118
119	BGT		FilterLoop
120@ save z terms
121@----------------------------------------------------------------
122
123	STRH	z1, [pFilter, #m_z1]
124	STRH	z2, [pFilter, #m_z2]
125
126@ Return to calling function
127@----------------------------------------------------------------
128
129	LDMFD	sp!,{r4-r10, lr}
130	BX		lr
131
132	.endfunc
133	.end
134
135