| blob | c3e5c7cd05dd46a099e7b79590cb60a5eaff0636 |
1 /***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006-2007 Thom Johansen
11 *
12 * All files in this archive are subject to the GNU General Public License.
13 * See the file COPYING in the source tree root for full license agreement.
14 *
15 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
16 * KIND, either express or implied.
17 *
18 ****************************************************************************/
20 /****************************************************************************
21 * void apply_crossfeed(int count, int32_t* src[])
22 */
23 .section .text
24 .global apply_crossfeed
25 apply_crossfeed:
26 @ unfortunately, we ended up in a bit of a register squeeze here, and need
27 @ to keep the count on the stack :/
28 stmdb sp!, { r4-r11, lr } @ stack modified regs
29 ldmia r1, { r2-r3 } @ r2 = src[0], r3 = src[1]
31 ldr r1, =crossfeed_data
32 ldmia r1!, { r4-r11 } @ load direct gain and filter data
33 add r12, r1, #13*4*2 @ calculate end of delay
34 stmdb sp!, { r0, r12 } @ stack count and end of delay adr
35 ldr r0, [r1, #13*4*2] @ fetch current delay line address
37 /* Register usage in loop:
38 * r0 = &delay[index][0], r1 = accumulator high, r2 = src[0], r3 = src[1],
39 * r4 = direct gain, r5-r7 = b0, b1, a1 (filter coefs),
40 * r8-r11 = filter history, r12 = temp, r14 = accumulator low
41 */
42 .cfloop:
43 smull r14, r1, r6, r8 @ acc = b1*dr[n - 1]
44 smlal r14, r1, r7, r9 @ acc += a1*y_l[n - 1]
45 ldr r8, [r0, #4] @ r8 = dr[n]
46 smlal r14, r1, r5, r8 @ acc += b0*dr[n]
47 mov r9, r1, lsl #1 @ fix format for filter history
48 ldr r12, [r2] @ load left input
49 smlal r14, r1, r4, r12 @ acc += gain*x_l[n]
50 mov r1, r1, lsl #1 @ fix format
51 str r1, [r2], #4 @ save result
53 smull r14, r1, r6, r10 @ acc = b1*dl[n - 1]
54 smlal r14, r1, r7, r11 @ acc += a1*y_r[n - 1]
55 ldr r10, [r0] @ r10 = dl[n]
56 str r12, [r0], #4 @ save left input to delay line
57 smlal r14, r1, r5, r10 @ acc += b0*dl[n]
58 mov r11, r1, lsl #1 @ fix format for filter history
59 ldr r12, [r3] @ load right input
60 smlal r14, r1, r4, r12 @ acc += gain*x_r[n]
61 str r12, [r0], #4 @ save right input to delay line
62 mov r1, r1, lsl #1 @ fix format
63 str r1, [r3], #4 @ save result
65 ldr r12, [sp, #4] @ fetch delay line end addr from stack
66 cmp r0, r12 @ need to wrap to start of delay?
67 subeq r0, r0, #13*4*2 @ wrap back delay line ptr to start
69 ldr r1, [sp] @ fetch count from stack
70 subs r1, r1, #1 @ are we finished?
71 strne r1, [sp] @ nope, save count back to stack
72 bne .cfloop
74 @ save data back to struct
75 ldr r12, =crossfeed_data + 4*4
76 stmia r12, { r8-r11 } @ save filter history
77 str r0, [r12, #30*4] @ save delay line index
78 add sp, sp, #8 @ remove temp variables from stack
79 ldmia sp!, { r4-r11, pc }
80 .cfend:
81 .size apply_crossfeed,.cfend-apply_crossfeed
83 /****************************************************************************
84 * int dsp_downsample(int count, struct dsp_data *data,
85 * in32_t *src[], int32_t *dst[])
86 */
87 .section .text
88 .global dsp_downsample
89 dsp_downsample:
90 stmdb sp!, { r4-r11, lr } @ stack modified regs
91 ldmib r1, { r5-r6 } @ r5 = num_channels,r6 = resample_data.delta
92 sub r5, r5, #1 @ pre-decrement num_channels for use
93 add r4, r1, #12 @ r4 = &resample_data.phase
94 mov r12, #0xff
95 orr r12, r12, #0xff00 @ r12 = 0xffff
96 .dschannel_loop:
97 ldr r1, [r4] @ r1 = resample_data.phase
98 ldr r7, [r2, r5, lsl #2] @ r7 = s = src[ch - 1]
99 ldr r8, [r3, r5, lsl #2] @ r8 = d = dst[ch - 1]
100 add r9, r4, #4 @ r9 = &last_sample[0]
101 ldr r10, [r9, r5, lsl #2] @ r10 = last_sample[ch - 1]
102 sub r11, r0, #1
103 ldr r14, [r7, r11, lsl #2] @ load last sample in s[] ...
104 str r14, [r9, r5, lsl #2] @ and write as next frame's last_sample
105 movs r9, r1, lsr #16 @ r9 = pos = phase >> 16
106 ldreq r11, [r7] @ if pos = 0, load src[0] and jump into loop
107 beq .dsuse_last_start
108 cmp r9, r0 @ if pos >= count, we're already done
109 bge .dsloop_skip
111 @ Register usage in loop:
112 @ r0 = count, r1 = phase, r4 = &resample_data.phase, r5 = cur_channel,
113 @ r6 = delta, r7 = s, r8 = d, r9 = pos, r10 = s[pos - 1], r11 = s[pos]
114 .dsloop:
115 add r9, r7, r9, lsl #2 @ r9 = &s[pos]
116 ldmda r9, { r10, r11 } @ r10 = s[pos - 1], r11 = s[pos]
117 .dsuse_last_start:
118 sub r11, r11, r10 @ r11 = diff = s[pos] - s[pos - 1]
119 @ keep frac in lower bits to take advantage of multiplier early termination
120 and r9, r1, r12 @ frac = phase & 0xffff
121 smull r9, r14, r11, r9
122 add r10, r10, r14, lsl #16
123 add r10, r10, r9, lsr #16 @ r10 = out = s[pos - 1] + frac*diff
124 str r10, [r8], #4 @ *d++ = out
125 add r1, r1, r6 @ phase += delta
126 mov r9, r1, lsr #16 @ pos = phase >> 16
127 cmp r9, r0 @ pos < count?
128 blt .dsloop @ yup, do more samples
129 .dsloop_skip:
130 subs r5, r5, #1
131 bpl .dschannel_loop @ if (--ch) >= 0, do another channel
132 sub r1, r1, r0, lsl #16 @ wrap phase back to start
133 str r1, [r4] @ store back
134 ldr r1, [r3] @ r1 = &dst[0]
135 sub r8, r8, r1 @ dst - &dst[0]
136 mov r0, r8, lsr #2 @ convert bytes->samples
137 ldmia sp!, { r4-r11, pc } @ ... and we're out
138 .dsend:
139 .size dsp_downsample,.dsend-dsp_downsample
141 /****************************************************************************
142 * int dsp_upsample(int count, struct dsp_data *dsp,
143 * in32_t *src[], int32_t *dst[])
144 */
145 .section .text
146 .global dsp_upsample
147 dsp_upsample:
148 stmdb sp!, { r4-r11, lr } @ stack modified regs
149 ldmib r1, { r5-r6 } @ r5 = num_channels,r6 = resample_data.delta
150 sub r5, r5, #1 @ pre-decrement num_channels for use
151 add r4, r1, #12 @ r4 = &resample_data.phase
152 stmdb sp!, { r0, r4 } @ stack count and &resample_data.phase
153 .uschannel_loop:
154 ldr r12, [r4] @ r12 = resample_data.phase
155 mov r1, r12, ror #16 @ swap halfword positions, we'll use carry
156 @ to detect pos increments
157 ldr r7, [r2, r5, lsl #2] @ r7 = s = src[ch - 1]
158 ldr r8, [r3, r5, lsl #2] @ r8 = d = dst[ch - 1]
159 add r9, r4, #4 @ r9 = &last_sample[0]
160 ldr r10, [r9, r5, lsl #2] @ r10 = last_sample[ch - 1]
161 sub r11, r0, #1
162 ldr r14, [r7, r11, lsl #2] @ load last sample in s[] ...
163 str r14, [r9, r5, lsl #2] @ and write as next frame's last_sample
164 add r9, r7, r0, lsl #2 @ r9 = src_end = &src[count]
165 movs r14, r12, lsr #16 @ pos = resample_data.phase >> 16
166 beq .usstart_0 @ pos = 0
167 cmp r14, r0 @ if pos >= count, we're already done
168 bge .usloop_skip
169 add r7, r7, r14, lsl #2 @ r7 = &s[pos]
170 ldr r10, [r7, #-4] @ r11 = s[pos - 1]
171 b .usstart_0
173 @ Register usage in loop:
174 @ r0 = count, r1 = phase, r4 = &resample_data.phase, r5 = cur_channel,
175 @ r6 = delta, r7 = s, r8 = d, r9 = src_end, r10 = s[pos - 1], r11 = s[pos]
176 .usloop_1:
177 mov r10, r11 @ r10 = previous sample
178 .usstart_0:
179 ldr r11, [r7], #4 @ r11 = next sample
180 sub r0, r11, r10 @ r0 = s[pos] - s[pos - 1]
181 .usloop_0:
182 mov r4, r1, lsr #16 @ r4 = frac = phase >> 16
183 smull r12, r14, r4, r0
184 add r14, r10, r14, lsl #16
185 add r14, r14, r12, lsr #16 @ r14 = out = s[pos - 1] + frac*diff
186 str r14, [r8], #4 @ *d++ = out
187 adds r1, r1, r6, lsl #16 @ phase += delta << 16
188 bcc .usloop_0 @ if carry is set, pos is incremented
189 cmp r7, r9 @ if s < src_end, do another sample
190 blo .usloop_1
191 .usloop_skip:
192 subs r5, r5, #1
193 ldmia sp, { r0, r4 } @ reload count and &resample_data.phase
194 bpl .uschannel_loop @ if (--ch) >= 0, do another channel
195 mov r1, r1, ror #16 @ wrap phase back to start of next frame
196 str r1, [r4] @ store back
197 ldr r1, [r3] @ r1 = &dst[0]
198 sub r8, r8, r1 @ dst - &dst[0]
199 mov r0, r8, lsr #2 @ convert bytes->samples
200 add sp, sp, #8 @ adjust stack for temp variables
201 ldmia sp!, { r4-r11, pc } @ ... and we're out
202 .usend:
203 .size dsp_upsample,.usend-dsp_upsample