| blob | f924569bc58fd83cdb45c21de0114377601482fd |
1 /***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006-2007 Thom Johansen
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21 #include "config.h"
23 /****************************************************************************
24 * void channels_process_sound_chan_mono(int count, int32_t *buf[])
25 *
26 * NOTE: The following code processes two samples at once. When count is odd,
27 * there is an additional obsolete sample processed, which will not be
28 * used by the calling functions.
29 */
30 .section .icode, "ax", %progbits
31 .align 2
32 .global channels_process_sound_chan_mono
33 .type channels_process_sound_chan_mono, %function
34 channels_process_sound_chan_mono:
35 @ input: r0 = count, r1 = buf
36 stmfd sp!, {r4-r5, lr}
37 ldmia r1, {r2-r3} @ r4 = buf[0], r5 = buf[1]
39 .monoloop:
40 ldmia r2, {r4-r5}
41 ldmia r3, {r12,lr}
42 mov r4, r4, asr #1 @ r4 = r4/2
43 add r4, r4, r12, asr #1 @ r4 = r4 + r12/2 = (buf[0]+buf[1])/2
44 mov r5, r5, asr #1 @ r5 = r5/2
45 add r5, r5, lr, asr #1 @ r5 = r5 + lr/2 = (buf[0]+buf[1])/2
46 stmia r2!, {r4-r5}
47 stmia r3!, {r4-r5}
48 subs r0, r0, #2
49 bgt .monoloop
51 ldmfd sp!, {r4-r5, pc}
52 .monoend:
53 .size channels_process_sound_chan_mono,.monoend-channels_process_sound_chan_mono
55 /****************************************************************************
56 * void channels_process_sound_chan_karaoke(int count, int32_t *buf[])
57 * NOTE: The following code processes two samples at once. When count is odd,
58 * there is an additional obsolete sample processed, which will not be
59 * used by the calling functions.
60 */
61 .section .icode, "ax", %progbits
62 .align 2
63 .global channels_process_sound_chan_karaoke
64 .type channels_process_sound_chan_karaoke, %function
65 channels_process_sound_chan_karaoke:
66 @ input: r0 = count, r1 = buf
67 stmfd sp!, {r4-r5, lr}
68 ldmia r1, {r2-r3} @ r4 = buf[0], r5 = buf[1]
70 .karaokeloop:
71 ldmia r2, {r4-r5}
72 ldmia r3, {r12,lr}
73 mov r12, r12, asr #1 @ r12 = r12/2
74 rsb r4, r12, r4, asr #1 @ r4 = -r12 + r4/2 = (buf[0]-buf[1])/2
75 rsb r12, r4, #0 @ r12 = -r4
76 mov lr, lr, asr #1 @ lr = lr/2
77 rsb r5, lr, r5, asr #1 @ r5 = -lr + r5/2 = (buf[0]-buf[1])/2
78 rsb lr, r5, #0 @ lr = -r5
79 stmia r2!, {r4-r5}
80 stmia r3!, {r12,lr}
81 subs r0, r0, #2
82 bgt .karaokeloop
84 ldmfd sp!, {r4-r5, pc}
85 .karaokeend:
86 .size channels_process_sound_chan_karaoke,.karaokeend-channels_process_sound_chan_karaoke
88 #if ARM_ARCH < 6
89 /****************************************************************************
90 * void sample_output_mono(int count, struct dsp_data *data,
91 * const int32_t *src[], int16_t *dst)
92 * NOTE: The following code processes two samples at once. When count is odd,
93 * there is an additional obsolete sample processed, which will not be
94 * used by the calling functions.
95 */
96 .section .icode, "ax", %progbits
97 .align 2
98 .global sample_output_mono
99 .type sample_output_mono, %function
100 sample_output_mono:
101 @ input: r0 = count, r1 = data, r2 = src, r3 = dst
102 stmfd sp!, {r4-r7, lr}
104 ldr r4, [r2] @ r4 = src[0]
105 ldr r5, [r1] @ lr = data->output_scale
106 sub r1, r5, #1 @ r1 = r5-1
107 mov r2, #1
108 mov r2, r2, asl r1 @ r2 = 1<<r1 = 1 << (scale-1)
109 mvn r1, #0x8000 @ r1 needed for clipping
110 mov r12, #0xff00
111 orr r12, r12, #0xff @ r12 needed for masking
113 .somloop:
114 ldmia r4!, {r6-r7}
115 add r6, r6, r2
116 mov r6, r6, asr r5 @ r6 = (r6 + 1<<(scale-1)) >> scale
117 mov lr, r6, asr #15
118 teq lr, lr, asr #31
119 eorne r6, r1, lr, asr #31 @ Clip (-32768...+32767)
120 add r7, r7, r2
121 mov r7, r7, asr r5 @ r7 = (r7 + 1<<(scale-1)) >> scale
122 mov lr, r7, asr #15
123 teq lr, lr, asr #31
124 eorne r7, r1, lr, asr #31 @ Clip (-32768...+32767)
126 and r6, r6, r12
127 orr r6, r6, r6, asl #16 @ pack first 2 halfwords into 1 word
128 and r7, r7, r12
129 orr r7, r7, r7, asl #16 @ pack last 2 halfwords into 1 word
130 stmia r3!, {r6-r7}
132 subs r0, r0, #2
133 bgt .somloop
135 ldmfd sp!, {r4-r7, pc}
136 .somend:
137 .size sample_output_mono,.somend-sample_output_mono
139 /****************************************************************************
140 * void sample_output_stereo(int count, struct dsp_data *data,
141 * const int32_t *src[], int16_t *dst)
142 * NOTE: The following code processes two samples at once. When count is odd,
143 * there is an additional obsolete sample processed, which will not be
144 * used by the calling functions.
145 */
146 .section .icode, "ax", %progbits
147 .align 2
148 .global sample_output_stereo
149 .type sample_output_stereo, %function
150 sample_output_stereo:
151 @ input: r0 = count, r1 = data, r2 = src, r3 = dst
152 stmfd sp!, {r4-r10, lr}
154 ldmia r2, {r4-r5} @ r4 = src[0], r5 = src[1]
155 ldr r6, [r1] @ r6 = data->output_scale
156 sub r1, r6, #1 @ r1 = r6-1
157 mov r2, #1
158 mov r2, r2, asl r1 @ r2 = 1<<r1 = 1 << (scale-1)
159 mvn r1, #0x8000 @ r1 needed for clipping
160 mov r12, #0xff00
161 orr r12, r12, #0xff @ r12 needed for masking
163 .sosloop:
164 ldmia r4!, {r7-r8}
165 add r7, r7, r2
166 mov r7, r7, asr r6 @ r7 = (r7 + 1<<(scale-1)) >> scale
167 mov lr, r7, asr #15
168 teq lr, lr, asr #31
169 eorne r7, r1, lr, asr #31 @ Clip (-32768...+32767)
170 add r8, r8, r2
171 mov r8, r8, asr r6 @ r8 = (r8 + 1<<(scale-1)) >> scale
172 mov lr, r8, asr #15
173 teq lr, lr, asr #31
174 eorne r8, r1, lr, asr #31 @ Clip (-32768...+32767)
176 ldmia r5!, {r9-r10}
177 add r9, r9, r2
178 mov r9, r9, asr r6 @ r9 = (r9 + 1<<(scale-1)) >> scale
179 mov lr, r9, asr #15
180 teq lr, lr, asr #31
181 eorne r9, r1, lr, asr #31 @ Clip (-32768...+32767)
182 add r10, r10, r2
183 mov r10, r10, asr r6 @ r10 = (r10 + 1<<(scale-1)) >> scale
184 mov lr, r10, asr #15
185 teq lr, lr, asr #31
186 eorne r10, r1, lr, asr #31 @ Clip (-32768...+32767)
188 and r7, r7, r12
189 orr r9, r7, r9, asl #16 @ pack first 2 halfwords into 1 word
190 and r8, r8, r12
191 orr r10, r8, r10, asl #16 @ pack last 2 halfwords into 1 word
192 stmia r3!, {r9-r10}
194 subs r0, r0, #2
195 bgt .sosloop
197 ldmfd sp!, {r4-r10, pc}
198 .sosend:
199 .size sample_output_stereo,.sosend-sample_output_stereo
200 #endif /* ARM_ARCH < 6 */
202 /****************************************************************************
203 * void apply_crossfeed(int count, int32_t* src[])
204 */
205 .section .text
206 .global apply_crossfeed
207 apply_crossfeed:
208 @ unfortunately, we ended up in a bit of a register squeeze here, and need
209 @ to keep the count on the stack :/
210 stmdb sp!, { r4-r11, lr } @ stack modified regs
211 ldmia r1, { r2-r3 } @ r2 = src[0], r3 = src[1]
213 ldr r1, =crossfeed_data
214 ldmia r1!, { r4-r11 } @ load direct gain and filter data
215 add r12, r1, #13*4*2 @ calculate end of delay
216 stmdb sp!, { r0, r12 } @ stack count and end of delay adr
217 ldr r0, [r1, #13*4*2] @ fetch current delay line address
219 /* Register usage in loop:
220 * r0 = &delay[index][0], r1 = accumulator high, r2 = src[0], r3 = src[1],
221 * r4 = direct gain, r5-r7 = b0, b1, a1 (filter coefs),
222 * r8-r11 = filter history, r12 = temp, r14 = accumulator low
223 */
224 .cfloop:
225 smull r14, r1, r6, r8 @ acc = b1*dr[n - 1]
226 smlal r14, r1, r7, r9 @ acc += a1*y_l[n - 1]
227 ldr r8, [r0, #4] @ r8 = dr[n]
228 smlal r14, r1, r5, r8 @ acc += b0*dr[n]
229 mov r9, r1, lsl #1 @ fix format for filter history
230 ldr r12, [r2] @ load left input
231 smlal r14, r1, r4, r12 @ acc += gain*x_l[n]
232 mov r1, r1, lsl #1 @ fix format
233 str r1, [r2], #4 @ save result
235 smull r14, r1, r6, r10 @ acc = b1*dl[n - 1]
236 smlal r14, r1, r7, r11 @ acc += a1*y_r[n - 1]
237 ldr r10, [r0] @ r10 = dl[n]
238 str r12, [r0], #4 @ save left input to delay line
239 smlal r14, r1, r5, r10 @ acc += b0*dl[n]
240 mov r11, r1, lsl #1 @ fix format for filter history
241 ldr r12, [r3] @ load right input
242 smlal r14, r1, r4, r12 @ acc += gain*x_r[n]
243 str r12, [r0], #4 @ save right input to delay line
244 mov r1, r1, lsl #1 @ fix format
245 str r1, [r3], #4 @ save result
247 ldr r12, [sp, #4] @ fetch delay line end addr from stack
248 cmp r0, r12 @ need to wrap to start of delay?
249 subeq r0, r0, #13*4*2 @ wrap back delay line ptr to start
251 ldr r1, [sp] @ fetch count from stack
252 subs r1, r1, #1 @ are we finished?
253 strne r1, [sp] @ nope, save count back to stack
254 bne .cfloop
256 @ save data back to struct
257 ldr r12, =crossfeed_data + 4*4
258 stmia r12, { r8-r11 } @ save filter history
259 str r0, [r12, #30*4] @ save delay line index
260 add sp, sp, #8 @ remove temp variables from stack
261 ldmia sp!, { r4-r11, pc }
262 .cfend:
263 .size apply_crossfeed,.cfend-apply_crossfeed
265 /****************************************************************************
266 * int dsp_downsample(int count, struct dsp_data *data,
267 * in32_t *src[], int32_t *dst[])
268 */
269 .section .text
270 .global dsp_downsample
271 dsp_downsample:
272 stmdb sp!, { r4-r11, lr } @ stack modified regs
273 ldmib r1, { r5-r6 } @ r5 = num_channels,r6 = resample_data.delta
274 sub r5, r5, #1 @ pre-decrement num_channels for use
275 add r4, r1, #12 @ r4 = &resample_data.phase
276 mov r12, #0xff
277 orr r12, r12, #0xff00 @ r12 = 0xffff
278 .dschannel_loop:
279 ldr r1, [r4] @ r1 = resample_data.phase
280 ldr r7, [r2, r5, lsl #2] @ r7 = s = src[ch - 1]
281 ldr r8, [r3, r5, lsl #2] @ r8 = d = dst[ch - 1]
282 add r9, r4, #4 @ r9 = &last_sample[0]
283 ldr r10, [r9, r5, lsl #2] @ r10 = last_sample[ch - 1]
284 sub r11, r0, #1
285 ldr r14, [r7, r11, lsl #2] @ load last sample in s[] ...
286 str r14, [r9, r5, lsl #2] @ and write as next frame's last_sample
287 movs r9, r1, lsr #16 @ r9 = pos = phase >> 16
288 ldreq r11, [r7] @ if pos = 0, load src[0] and jump into loop
289 beq .dsuse_last_start
290 cmp r9, r0 @ if pos >= count, we're already done
291 bge .dsloop_skip
293 @ Register usage in loop:
294 @ r0 = count, r1 = phase, r4 = &resample_data.phase, r5 = cur_channel,
295 @ r6 = delta, r7 = s, r8 = d, r9 = pos, r10 = s[pos - 1], r11 = s[pos]
296 .dsloop:
297 add r9, r7, r9, lsl #2 @ r9 = &s[pos]
298 ldmda r9, { r10, r11 } @ r10 = s[pos - 1], r11 = s[pos]
299 .dsuse_last_start:
300 sub r11, r11, r10 @ r11 = diff = s[pos] - s[pos - 1]
301 @ keep frac in lower bits to take advantage of multiplier early termination
302 and r9, r1, r12 @ frac = phase & 0xffff
303 smull r9, r14, r11, r9
304 add r10, r10, r14, lsl #16
305 add r10, r10, r9, lsr #16 @ r10 = out = s[pos - 1] + frac*diff
306 str r10, [r8], #4 @ *d++ = out
307 add r1, r1, r6 @ phase += delta
308 mov r9, r1, lsr #16 @ pos = phase >> 16
309 cmp r9, r0 @ pos < count?
310 blt .dsloop @ yup, do more samples
311 .dsloop_skip:
312 subs r5, r5, #1
313 bpl .dschannel_loop @ if (--ch) >= 0, do another channel
314 sub r1, r1, r0, lsl #16 @ wrap phase back to start
315 str r1, [r4] @ store back
316 ldr r1, [r3] @ r1 = &dst[0]
317 sub r8, r8, r1 @ dst - &dst[0]
318 mov r0, r8, lsr #2 @ convert bytes->samples
319 ldmia sp!, { r4-r11, pc } @ ... and we're out
320 .dsend:
321 .size dsp_downsample,.dsend-dsp_downsample
323 /****************************************************************************
324 * int dsp_upsample(int count, struct dsp_data *dsp,
325 * in32_t *src[], int32_t *dst[])
326 */
327 .section .text
328 .global dsp_upsample
329 dsp_upsample:
330 stmdb sp!, { r4-r11, lr } @ stack modified regs
331 ldmib r1, { r5-r6 } @ r5 = num_channels,r6 = resample_data.delta
332 sub r5, r5, #1 @ pre-decrement num_channels for use
333 add r4, r1, #12 @ r4 = &resample_data.phase
334 stmdb sp!, { r0, r4 } @ stack count and &resample_data.phase
335 .uschannel_loop:
336 ldr r12, [r4] @ r12 = resample_data.phase
337 mov r1, r12, ror #16 @ swap halfword positions, we'll use carry
338 @ to detect pos increments
339 ldr r7, [r2, r5, lsl #2] @ r7 = s = src[ch - 1]
340 ldr r8, [r3, r5, lsl #2] @ r8 = d = dst[ch - 1]
341 add r9, r4, #4 @ r9 = &last_sample[0]
342 ldr r10, [r9, r5, lsl #2] @ r10 = last_sample[ch - 1]
343 sub r11, r0, #1
344 ldr r14, [r7, r11, lsl #2] @ load last sample in s[] ...
345 str r14, [r9, r5, lsl #2] @ and write as next frame's last_sample
346 add r9, r7, r0, lsl #2 @ r9 = src_end = &src[count]
347 movs r14, r12, lsr #16 @ pos = resample_data.phase >> 16
348 beq .usstart_0 @ pos = 0
349 cmp r14, r0 @ if pos >= count, we're already done
350 bge .usloop_skip
351 add r7, r7, r14, lsl #2 @ r7 = &s[pos]
352 ldr r10, [r7, #-4] @ r11 = s[pos - 1]
353 b .usstart_0
355 @ Register usage in loop:
356 @ r0 = count, r1 = phase, r4 = &resample_data.phase, r5 = cur_channel,
357 @ r6 = delta, r7 = s, r8 = d, r9 = src_end, r10 = s[pos - 1], r11 = s[pos]
358 .usloop_1:
359 mov r10, r11 @ r10 = previous sample
360 .usstart_0:
361 ldr r11, [r7], #4 @ r11 = next sample
362 sub r0, r11, r10 @ r0 = s[pos] - s[pos - 1]
363 .usloop_0:
364 mov r4, r1, lsr #16 @ r4 = frac = phase >> 16
365 smull r12, r14, r4, r0
366 add r14, r10, r14, lsl #16
367 add r14, r14, r12, lsr #16 @ r14 = out = s[pos - 1] + frac*diff
368 str r14, [r8], #4 @ *d++ = out
369 adds r1, r1, r6, lsl #16 @ phase += delta << 16
370 bcc .usloop_0 @ if carry is set, pos is incremented
371 cmp r7, r9 @ if s < src_end, do another sample
372 blo .usloop_1
373 .usloop_skip:
374 subs r5, r5, #1
375 ldmia sp, { r0, r4 } @ reload count and &resample_data.phase
376 bpl .uschannel_loop @ if (--ch) >= 0, do another channel
377 mov r1, r1, ror #16 @ wrap phase back to start of next frame
378 str r1, [r4] @ store back
379 ldr r1, [r3] @ r1 = &dst[0]
380 sub r8, r8, r1 @ dst - &dst[0]
381 mov r0, r8, lsr #2 @ convert bytes->samples
382 add sp, sp, #8 @ adjust stack for temp variables
383 ldmia sp!, { r4-r11, pc } @ ... and we're out
384 .usend:
385 .size dsp_upsample,.usend-dsp_upsample
387 /****************************************************************************
388 * void dsp_apply_gain(int count, struct dsp_data *data, int32_t *buf[])
389 * NOTE: The following code processes two samples at once. When count is odd,
390 * there is an additional obsolete sample processed, which will not be
391 * used by the calling functions.
392 */
393 .section .icode, "ax", %progbits
394 .align 2
395 .global dsp_apply_gain
396 .type dsp_apply_gain, %function
397 dsp_apply_gain:
398 @ input: r0 = count, r1 = data, r2 = buf[]
399 stmfd sp!, {r4-r7, lr}
401 ldr r3, [r1, #4] @ r3 = data->num_channels
402 ldr r4, [r1, #32] @ r5 = data->gain
404 .dag_outerloop:
405 ldr r1, [r2], #4 @ r1 = buf[0] and increment index of buf[]
406 mov r12, r0 @ r12 = r0 = count
408 .dag_innerloop:
409 ldmia r1, {r5, r6} @ load r5, r6 from r1
410 smull r7, lr, r5, r4 @ r5 = FRACMUL_SHL(r5, r4, 8)
411 mov lr, lr, asl #9
412 orr r5, lr, r7, lsr #23
413 smull r7, lr, r6, r4 @ r6 = FRACMUL_SHL(r6, r4, 8)
414 mov lr, lr, asl #9
415 orr r6, lr, r7, lsr #23
416 stmia r1!, {r5, r6} @ save r5, r6 to r1 and increment r1
417 subs r12, r12, #2
418 bgt .dag_innerloop @ end of inner loop
420 subs r3, r3, #1
421 bgt .dag_outerloop @ end of outer loop
423 ldmfd sp!, {r4-r7, pc}
424 .dagend:
425 .size dsp_apply_gain,.dagend-dsp_apply_gain