-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathIIRT_Filter.s
202 lines (160 loc) · 8.14 KB
/
IIRT_Filter.s
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;
; FILE: IIRT_Filter.s
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; offsets into IIRTransposeFilter structure
; (entries are word wide -- hence +=2)
.equ oNumSectionsLess1, 0 ; number of second order sections - 1
.equ oCoefs, 2 ; pointer to coefficients
; number of coefficients is
; 5 * number of second order sections
.equ oPSVpage, 4 ; page number in program memory if
; coefficients are in program memory
; 0xFF00 if not
.equ oStates1, 6 ; pointer to state variable 1
; one word for every section
.equ oStates2, 8 ; pointer to state variable 1
; one word for every section
.equ oFinalShift, 10 ; final left shift count
; restores filter gain to 0 dB
; shift count may be zero
; if not zero, it is the number of bits
; to shift the output to the left
; ..............................................................................
; ..............................................................................
; -----------------------------------------------------------------------------
; -----------------------------------------------------------------------------
; Block Cascaded Transpose IIR Implementation (cascade form I)
; This file should be assembled and linked against filter coefficients
; generated by dsPicFD -- filter design software by Momentum Data Systems.
;
; Module Re-entrancy:
; Module re-entrancy is not supported
;
; Input: for routine _BlockIIRTransposeFilter
;
; w0 = pointer to filter structure
; w1 = pointer to input sample buffer
; w2 = pointer to output sample buffer
; w3 = number of output samples to generate
;
;
; System Resource usage
; W0 used but value on exit is same as entry
; W1, W2, W3 used not restored
; W4, W5, W6, W7 used not restored
; w8, W9, w10,W11 used, saved and restored
; accumulators a,b used not restored
; CORCON used, saved and restored
; PSVPAG used, saved and restored
;
;
; Input: for _IIRTransposeFilterInit
; w0 = pointer to filter structure
;
; System Resource usage
; w0, w1, W2 used not restored
; -----------------------------------------------------------------------------
; -----------------------------------------------------------------------------
;
; DO and REPEAT instruction usage
; 2 level DO instruction
; REPEAT instruction is not used
;
;
; Module Program Memory Size
; _BlockIIRTransposeFilter: 49
; _IIRTransposeFilterInit: 8
;
;
; Module Cycle Count
; _BlockIIRTransposeFilter: 27 + N*(11 + S*11) (+2 if PSV)
; _IIRTransposeFilterInit: 8 + S*2
; with N: number of samples per block, S: number of sections in filter.
;
; -----------------------------------------------------------------------------
; -----------------------------------------------------------------------------
.text
.global _BlockIIRTransposeFilter
_BlockIIRTransposeFilter:
PUSH w8 ; save context of w8
PUSH w9 ; save context of w9
PUSH w10 ; save context of W10
PUSH w11 ; save context of w11
PUSH CORCON ; save context of CORCON
PUSH PSVPAG ; save context of PSVPAG
; Check if filter coefficients are stored in Program Space or Data Space and enable
; PSV and set up PSVPAG accordingly
MOV [w0+oPSVpage], W10
MOV #0xFF00, W8
CP W8, W10 ; perform w8-w10 (if w10 = FF00 then do not enable PSV)
BRA Z, no_psv ; branch if compare true (coefficients are in data space)
MOV #0x00F4,W8 ; Enable enable Program Space Visibility,
; Accumulator A Saturation and Data Space write Saturation
; as bits 2, 5 and 7 are set in CORCON
; Also note bits 2, 5 and 7 are in low order byte which is
; the first byte of a 2 byte word in a little endian
; processor such as the dsPIC30
; also, enable unbiased (convergent) rounding mode,
; 1.39 saturation enabled, fractional mult.
MOV w10, PSVPAG ; PSVPAG = Program Space page containing filter taps
BRA SetupPointers
no_psv:
MOV #0x00F0,w8 ; Enable Accumulator A Saturation and
; Data Space write Saturation
; as bits 5 and 7 are set in CORCON
; ..............................................................................;
; Setup pointers
SetupPointers:
MOV W8, CORCON ; set PSV and saturation options
MOV [w0+oNumSectionsLess1], w4 ; w4 = number of sections - 1
MOV [w0+oFinalShift], w9 ; w9 = final shift count
DEC w3, w3 ; w3 = number of output samples -1
DO w3, transposeBlockLoop ; loop on the number of input samples
MOV [w0+oCoefs], w8 ; w8 = base address of coefs
MOV [w1++], w6 ; w6 = next input sample
MOV [w0+oStates1], w10 ; w10 = base address of states1 buffer
MOV [w0+oStates2], w11 ; w11 = base address of states2 buffer
MOV [w8++], w5 ; fetch first coefficient
LAC [w10], #1, a ; fetch filter state
DO w4, transposeSectionLoop ; loop on the number of second order sections
MAC w5*w6, a, [w8]+=2, w5
LAC [w11], #1, b
SAC.R a, #-1, w7
MAC w5*w6, b, [w8]+=2, w5
MAC w5*w7, b, [w8]+=2, w5
SAC.R b, #-1, [w10++]
MPY w5*w6, b, [w8]+=2, w5
SAC.R a, #-1, w6
LAC [w10], #1, a
MAC w5*w7, b, [w8]+=2, w5
transposeSectionLoop:
SAC.R b, #-1, [w11++]
LAC w6, a
SFTAC a, w9 ; perform arithmetic shift
transposeBlockLoop:
SAC.R a, [w2++] ; round and store result in output buffer
POP PSVPAG ; restore context of PSVPAG
POP CORCON ; restore context of CORCON
POP w11 ; restore context of w11
POP w10 ; restore context of W10
POP w9 ; restore context of w9
POP w8 ; restore context of w8
RETURN ; exit from _BlockIIRTransposeFilter:
; -----------------------------------------------------------------------------
; Input:
; w0 = pointer to filter structure
.text
.global _IIRTransposeFilterInit
_IIRTransposeFilterInit:
MOV [w0+oStates1], w1 ; w1 = base address of states1 buffer
MOV [w0+oStates2], w2 ; w2 = base address of states2 buffer
MOV [w0+oNumSectionsLess1], w0 ; w0 = number of sections - 1
; initialize state buffers (i.e. fill with zeros)
DO w0, transposeInitLoop
CLR [w1++]
transposeInitLoop:
CLR [w2++]
RETURN ; exit from _IIRTransposeFilterInit:
; -----------------------------------------------------------------------------