d37bf24d90
Replaces the NATIVE_FREQUENCY constant with a configurable frequency. The user may select 48000Hz if the hardware supports it. The default is still 44100Hz and the minimum is 44100Hz. The setting is located in the playback settings, under "Frequency". "Frequency" was duplicated in english.lang for now to avoid having to fix every .lang file for the moment and throwing everything out of sync because of the new play_frequency feature in features.txt. The next cleanup should combine it with the one included for recording and generalize the ID label. If the hardware doesn't support 48000Hz, no setting will be available. On particular hardware where very high rates are practical and desireable, the upper bound can be extended by patching. The PCM mixer can be configured to play at the full hardware frequency range. The DSP core can configure to the hardware minimum up to the maximum playback setting (some buffers must be reserved according to the maximum rate). If only 44100Hz is supported or possible on a given target for playback, using the DSP and mixer at other samperates is possible if the hardware offers them. Change-Id: I6023cf0c0baa8bc6292b6919b4dd3618a6a25622 Reviewed-on: http://gerrit.rockbox.org/479 Reviewed-by: Michael Sevakis <jethead71@rockbox.org> Tested-by: Michael Sevakis <jethead71@rockbox.org>
777 lines
33 KiB
ArmAsm
777 lines
33 KiB
ArmAsm
/***************************************************************************
|
|
* __________ __ ___.
|
|
* Open \______ \ ____ ____ | | _\_ |__ _______ ___
|
|
* Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
|
|
* Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
|
|
* Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
|
|
* \/ \/ \/ \/ \/
|
|
* $Id$
|
|
*
|
|
* Copyright (C) 2006-2007 Thom Johansen
|
|
* Copyright (C) 2010 Bertrik Sikken
|
|
* Copyright (C) 2012 Michael Sevakis
|
|
*
|
|
* This program is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU General Public License
|
|
* as published by the Free Software Foundation; either version 2
|
|
* of the License, or (at your option) any later version.
|
|
*
|
|
* This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
|
|
* KIND, either express or implied.
|
|
*
|
|
****************************************************************************/
|
|
#include "rbcodecconfig.h"
|
|
|
|
/****************************************************************************
|
|
* void channel_mode_proc_mono(struct dsp_proc_entry *this,
|
|
* struct dsp_buffer **buf_p)
|
|
*/
|
|
.section .text, "ax",%progbits
|
|
.global channel_mode_proc_mono
|
|
.type channel_mode_proc_mono, %function
|
|
channel_mode_proc_mono:
|
|
@ input: r0 = this, r1 = buf_p
|
|
ldr r1, [r1] @ r1 = buf = *buf_p;
|
|
stmfd sp!, { r4, lr } @
|
|
@
|
|
ldmia r1, { r0-r2 } @ r0 = buf->remcount, r1 = buf->p32[0],
|
|
@ r2 = buf->p32[1]
|
|
subs r0, r0, #1 @ odd: end at 0; even: end at -1
|
|
beq .mono_singlesample @ Zero? Only one sample!
|
|
@
|
|
.monoloop: @
|
|
ldmia r1, { r3, r4 } @ r3, r4 = Li0, Li1
|
|
ldmia r2, { r12, r14 } @ r12, r14 = Ri0, Ri1
|
|
mov r3, r3, asr #1 @ Mo0 = Li0 / 2 + Ri0 / 2
|
|
mov r4, r4, asr #1 @ Mo1 = Li1 / 2 + Ri1 / 2
|
|
add r12, r3, r12, asr #1 @
|
|
add r14, r4, r14, asr #1 @
|
|
subs r0, r0, #2 @
|
|
stmia r1!, { r12, r14 } @ store Mo0, Mo1
|
|
stmia r2!, { r12, r14 } @ store Mo0, Mo1
|
|
bgt .monoloop @
|
|
@
|
|
ldmpc cond=lt, regs=r4 @ if count was even, we're done
|
|
@
|
|
.mono_singlesample: @
|
|
ldr r3, [r1] @ r3 = Ls
|
|
ldr r12, [r2] @ r12 = Rs
|
|
mov r3, r3, asr #1 @ Mo = Ls / 2 + Rs / 2
|
|
add r12, r3, r12, asr #1 @
|
|
str r12, [r1] @ store Mo
|
|
str r12, [r2] @ store Mo
|
|
@
|
|
ldmpc regs=r4 @
|
|
.size channel_mode_proc_mono, .-channel_mode_proc_mono
|
|
|
|
/****************************************************************************
|
|
* void channel_mode_proc_custom(struct dsp_proc_entry *this,
|
|
* struct dsp_buffer **buf_p)
|
|
*/
|
|
.section .text, "ax",%progbits
|
|
.global channel_mode_proc_custom
|
|
.type channel_mode_proc_custom, %function
|
|
channel_mode_proc_custom:
|
|
@ input: r0 = this, r1 = buf_p
|
|
ldr r2, [r0] @ r2 = &channel_mode_data = this->data
|
|
ldr r1, [r1] @ r1 = buf = *buf_p;
|
|
|
|
stmfd sp!, { r4-r10, lr }
|
|
|
|
ldmia r2, { r3, r4 } @ r3 = sw_gain, r4 = sw_cross
|
|
|
|
ldmia r1, { r0-r2 } @ r0 = buf->remcount, r1 = buf->p32[0],
|
|
@ r2 = buf->p32[1]
|
|
|
|
subs r0, r0, #1
|
|
beq .custom_single_sample @ Zero? Only one sample!
|
|
|
|
.custom_loop:
|
|
ldmia r1, { r5, r6 } @ r5 = Li0, r6 = Li1
|
|
ldmia r2, { r7, r8 } @ r7 = Ri0, r8 = Ri1
|
|
|
|
subs r0, r0, #2
|
|
|
|
smull r9, r10, r5, r3 @ Lc0 = Li0*gain
|
|
smull r12, r14, r7, r3 @ Rc0 = Ri0*gain
|
|
smlal r9, r10, r7, r4 @ Lc0 += Ri0*cross
|
|
smlal r12, r14, r5, r4 @ Rc0 += Li0*cross
|
|
|
|
mov r9, r9, lsr #31 @ Convert to s0.31
|
|
mov r12, r12, lsr #31
|
|
orr r5, r9, r10, asl #1
|
|
orr r7, r12, r14, asl #1
|
|
|
|
smull r9, r10, r6, r3 @ Lc1 = Li1*gain
|
|
smull r12, r14, r8, r3 @ Rc1 = Ri1*gain
|
|
smlal r9, r10, r8, r4 @ Lc1 += Ri1*cross
|
|
smlal r12, r14, r6, r4 @ Rc1 += Li1*cross
|
|
|
|
mov r9, r9, lsr #31 @ Convert to s0.31
|
|
mov r12, r12, lsr #31
|
|
orr r6, r9, r10, asl #1
|
|
orr r8, r12, r14, asl #1
|
|
|
|
stmia r1!, { r5, r6 } @ Store Lc0, Lc1
|
|
stmia r2!, { r7, r8 } @ Store Rc0, Rc1
|
|
|
|
bgt .custom_loop
|
|
|
|
ldmpc cond=lt, regs=r4-r10 @ < 0? even count
|
|
|
|
.custom_single_sample:
|
|
ldr r5, [r1] @ handle odd sample
|
|
ldr r7, [r2]
|
|
|
|
smull r9, r10, r5, r3 @ Lc0 = Li0*gain
|
|
smull r12, r14, r7, r3 @ Rc0 = Ri0*gain
|
|
smlal r9, r10, r7, r4 @ Lc0 += Ri0*cross
|
|
smlal r12, r14, r5, r4 @ Rc0 += Li0*cross
|
|
|
|
mov r9, r9, lsr #31 @ Convert to s0.31
|
|
mov r12, r12, lsr #31
|
|
orr r5, r9, r10, asl #1
|
|
orr r7, r12, r14, asl #1
|
|
|
|
str r5, [r1] @ Store Lc0
|
|
str r7, [r2] @ Store Rc0
|
|
|
|
ldmpc regs=r4-r10
|
|
.size channel_mode_proc_custom, .-channel_mode_proc_custom
|
|
|
|
/****************************************************************************
|
|
* void channel_mode_proc_karaoke(struct dsp_proc_entry *this,
|
|
* struct dsp_buffer **buf_p)
|
|
*/
|
|
.section .text, "ax",%progbits
|
|
.global channel_mode_proc_karaoke
|
|
.type channel_mode_proc_karaoke, %function
|
|
channel_mode_proc_karaoke:
|
|
@ input: r0 = this, r1 = buf_p
|
|
ldr r1, [r1] @ r1 = buf = *buf_p;
|
|
stmfd sp!, { r4, lr } @
|
|
@
|
|
ldmia r1, { r0-r2 } @ r0 = buf->remcount, r1 = buf->p32[0],
|
|
@ r2 = buf->p32[1]
|
|
subs r0, r0, #1 @ odd: end at 0; even: end at -1
|
|
beq .karaoke_singlesample @ Zero? Only one sample!
|
|
@
|
|
.karaokeloop: @
|
|
ldmia r1, { r3, r4 } @ r3, r4 = Li0, Li1
|
|
ldmia r2, { r12, r14 } @ r12, r14 = Ri0, Ri1
|
|
mov r3, r3, asr #1 @ Lo0 = Li0 / 2 - Ri0 / 2
|
|
mov r4, r4, asr #1 @ Lo1 = Li1 / 2 - Ri1 / 2
|
|
sub r3, r3, r12, asr #1 @
|
|
sub r4, r4, r14, asr #1 @
|
|
rsb r12, r3, #0 @ Ro0 = -Lk0 = Rs0 / 2 - Ls0 / 2
|
|
rsb r14, r4, #0 @ Ro1 = -Lk1 = Ri1 / 2 - Li1 / 2
|
|
subs r0, r0, #2 @
|
|
stmia r1!, { r3, r4 } @ store Lo0, Lo1
|
|
stmia r2!, { r12, r14 } @ store Ro0, Ro1
|
|
bgt .karaokeloop @
|
|
@
|
|
ldmpc cond=lt, regs=r4 @ if count was even, we're done
|
|
@
|
|
.karaoke_singlesample: @
|
|
ldr r3, [r1] @ r3 = Li
|
|
ldr r12, [r2] @ r12 = Ri
|
|
mov r3, r3, asr #1 @ Lk = Li / 2 - Ri /2
|
|
sub r3, r3, r12, asr #1 @
|
|
rsb r12, r3, #0 @ Rk = -Lo = Ri / 2 - Li / 2
|
|
str r3, [r1] @ store Lo
|
|
str r12, [r2] @ store Ro
|
|
@
|
|
ldmpc regs=r4 @
|
|
.size channel_mode_proc_karaoke, .-channel_mode_proc_karaoke
|
|
|
|
/****************************************************************************
|
|
* void crossfeed_process(struct dsp_proc_entry *this,
|
|
* struct dsp_buffer **buf_p)
|
|
*/
|
|
.section .text, "ax",%progbits
|
|
.global crossfeed_process
|
|
crossfeed_process:
|
|
@ input: r0 = this, r1 = buf_p
|
|
@ unfortunately, we ended up in a bit of a register squeeze here, and need
|
|
@ to keep the count on the stack :/
|
|
ldr r1, [r1] @ r1 = buf = *buf_p;
|
|
stmfd sp!, { r4-r11, lr } @ stack modified regs
|
|
ldr r0, [r0] @ r0 = this->data = &crossfeed_state
|
|
ldmia r1, { r1-r3 } @ r1 = buf->remcount, r2 = buf->p32[0],
|
|
@ r3 = buf->p32[1]
|
|
ldmia r0, { r4-r12, r14 } @ r4 = gain, r5-r7 = coeffs,
|
|
@ r8-r11 = history, r12 = index,
|
|
@ r14 = index_max
|
|
add r0, r0, #0x28 @ r0 = state->delay
|
|
stmfd sp!, { r0-r1, r14 } @ stack state->delay, count, index_max
|
|
|
|
/* Register usage in loop:
|
|
* r0 = acc low/count, r1 = acc high, r2 = buf->p32[0],
|
|
* r3 = buf->p32[1], r4 = direct gain, r5-r7 = b0, b1, a1 (filter coefs),
|
|
* r8 = dr[n-1], r9 = y_r[n-1], r10 = dl[n-1], r11 = y_l[n-1],
|
|
* r12 = index, r14 = scratch/index_max
|
|
*/
|
|
.cfloop:
|
|
smull r0, r1, r6, r8 @ acc = b1*dr[n - 1]
|
|
ldr r8, [r12, #4] @ r8 = dr[n]
|
|
smlal r0, r1, r7, r9 @ acc += a1*y_r[n - 1]
|
|
smlal r0, r1, r5, r8 @ acc += b0*dr[n]
|
|
ldr r14, [r2] @ load left input: x_l[n]
|
|
mov r9, r1, asl #1 @ fix format for filter history
|
|
smlal r0, r1, r4, r14 @ acc += gain*x_l[n]
|
|
mov r1, r1, asl #1 @ fix format
|
|
str r1, [r2], #4 @ save result
|
|
smull r0, r1, r6, r10 @ acc = b1*dl[n - 1]
|
|
ldr r10, [r12] @ r10 = dl[n]
|
|
smlal r0, r1, r7, r11 @ acc += a1*y_l[n - 1]
|
|
smlal r0, r1, r5, r10 @ acc += b0*dl[n]
|
|
str r14, [r12], #4 @ save left input to delay line
|
|
ldr r14, [r3] @ load right input: x_r[n]
|
|
mov r11, r1, asl #1 @ fix format for filter history
|
|
smlal r0, r1, r4, r14 @ acc += gain*x_r[n]
|
|
str r14, [r12], #4 @ save right input to delay line
|
|
ldmib sp, { r0, r14 } @ fetch count and delay end
|
|
mov r1, r1, asl #1 @ fix format
|
|
str r1, [r3], #4 @ save result
|
|
|
|
cmp r12, r14 @ need to wrap to start of delay?
|
|
ldrhs r12, [sp] @ wrap delay index
|
|
|
|
subs r0, r0, #1 @ are we finished?
|
|
strgt r0, [sp, #4] @ save count to stack
|
|
bgt .cfloop
|
|
|
|
@ save data back to struct
|
|
ldr r0, [sp] @ fetch state->delay
|
|
sub r0, r0, #0x18 @ save filter history and delay index
|
|
stmia r0, { r8-r12 } @
|
|
add sp, sp, #12 @ remove temp variables from stack
|
|
ldmpc regs=r4-r11
|
|
.size crossfeed_process, .-crossfeed_process
|
|
|
|
/****************************************************************************
|
|
* void crossfeed_meier_process(struct dsp_proc_entry *this,
|
|
* struct dsp_buffer **buf_p)
|
|
*/
|
|
.section .text
|
|
.global crossfeed_meier_process
|
|
crossfeed_meier_process:
|
|
@ input: r0 = this, r1 = buf_p
|
|
ldr r1, [r1] @ r1 = buf = *buf_p;
|
|
ldr r0, [r0] @ r0 = this->data = &crossfeed_state
|
|
stmfd sp!, { r4-r10, lr } @ stack non-volatile context
|
|
ldmia r1, { r1-r3 } @ r1 = buf->remcout, r2=p32[0], r3=p32[1]
|
|
ldmib r0, { r4-r8 } @ r4 = vcl, r5 = vcr, r6 = vdiff
|
|
@ r7 = coef1, r8 = coef2
|
|
.cfm_loop:
|
|
ldr r12, [r2] @ r12 = lout
|
|
ldr r14, [r3] @ r14 = rout
|
|
smull r9, r10, r8, r6 @ r9, r10 = common = coef2*vdiff
|
|
add r12, r12, r4 @ lout += vcl
|
|
add r14, r14, r5 @ rout += vcr
|
|
sub r6, r12, r14 @ r6 = vdiff = lout - rout
|
|
str r12, [r2], #4 @ store left channel
|
|
str r14, [r3], #4 @ store right channel
|
|
rsbs r12, r9, #0 @ r12 = -common (lo)
|
|
rsc r14, r10, #0 @ r14 = -common (hi)
|
|
smlal r9, r10, r7, r4 @ r9, r10 = res1 = coef1*vcl + common
|
|
smlal r12, r14, r7, r5 @ r12, r14 = res2 = coef1*vcr - common
|
|
subs r1, r1, #1 @ count--
|
|
mov r9, r9, lsr #31 @ r9 = convert res1 to s0.31
|
|
orr r9, r9, r10, asl #1 @ .
|
|
mov r12, r12, lsr #31 @ r12 = convert res2 to s0.31
|
|
orr r12, r12, r14, asl #1 @ .
|
|
sub r4, r4, r9 @ r4 = vcl -= res1
|
|
sub r5, r5, r12 @ r5 = vcr -= res2
|
|
bgt .cfm_loop @ more samples?
|
|
|
|
stmib r0, { r4-r6 } @ save vcl, vcr, vdiff
|
|
ldmpc regs=r4-r10 @ restore non-volatile context, return
|
|
.size crossfeed_meier_process, .-crossfeed_meier_process
|
|
|
|
/****************************************************************************
|
|
* int resample_hermite(struct resample_data *data, struct dsp_buffer *src,
|
|
* struct dsp_buffer *dst)
|
|
*/
|
|
.section .text, "ax",%progbits
|
|
.global resample_hermite
|
|
resample_hermite:
|
|
@input: r0 = data, r1 = src, r2 = dst
|
|
stmfd sp!, { r0-r2, r4-r11, lr } @ stack parms, modified regs
|
|
ldr r9, [r1] @ r9 = srcrem = src->remcount
|
|
ldrb r10, [r1, #17] @ r10 = ch = num_channels
|
|
ldr r14, [r0] @ r14 = data->delta, r0 = data
|
|
|
|
cmp r9, #0x8000 @ srcrem = MIN(srcrem, 0x8000)
|
|
movgt r9, #0x8000 @
|
|
|
|
@ Channels are processed high to low while history is saved low to high
|
|
@ It's really noone's business how we do this
|
|
add r12, r0, #8 @ r12 = h = data->history
|
|
|
|
.hrs_channel_loop:
|
|
stmfd sp!, { r10, r12 } @ push ch, h
|
|
ldr r5, [r0, #4] @ r5 = data->phase
|
|
ldr r6, [r1, r10, lsl #2] @ r6 = src->p32[ch]
|
|
ldr r7, [r2, r10, lsl #2] @ r7 = dst->p32[ch]
|
|
ldr r8, [r2, #12] @ r8 = dstrem = dst->bufcount
|
|
|
|
mov r0, r5, lsr #16 @ r0 = pos = phase >> 16
|
|
cmp r0, r9 @ r0 = pos = MIN(pos, srcrem)
|
|
movgt r0, r9 @
|
|
|
|
add r6, r6, r0, lsl #2 @ r6 = &s[pos]
|
|
|
|
cmp r0, #3 @ pos >= 3? history not needed
|
|
ldmgedb r6, { r1-r3 } @ x3..x1 = s[pos-3]..s[pos-1]
|
|
bge .hrs_loadhist_done @
|
|
add r10, r0, r0, lsl #1 @ branch pc + pos*12
|
|
add pc, pc, r10, lsl #2 @
|
|
nop @
|
|
|
|
ldmia r12, { r1-r3 } @ x3..x1 = h[0]..h[2]
|
|
b .hrs_loadhist_done @
|
|
nop @
|
|
|
|
ldmib r12, { r1-r2 } @ x3..x2 = h[1]..h[2]
|
|
ldr r3, [r6, #-4] @ x1 = s[0]
|
|
b .hrs_loadhist_done @
|
|
|
|
ldr r1, [r12, #8] @ x3 = h[2]
|
|
ldmdb r6, { r2-r3 } @ x2..x1 = s[0]..s[1]
|
|
.hrs_loadhist_done:
|
|
|
|
cmp r0, r9 @ pos past end?
|
|
bge .hrs_channel_done
|
|
|
|
cmp r14, #0x10000 @ delta >= 1.0?
|
|
bhs .hrs_dsstart @ yes? is downsampling
|
|
|
|
/** Upsampling **/
|
|
str r9, [sp, #-4]! @ push srcrem
|
|
mov r5, r5, lsl #16 @ r5 = phase << 16
|
|
sub r0, r9, r0 @ r0 = dte = srcrem - pos
|
|
mov r14, r14, lsl #16 @ r14 = delta << 16
|
|
|
|
@ Register usage in loop:
|
|
@ r0 = dte
|
|
@ r1 = x3, r2 = x2, r3 = x1, r4 = x0
|
|
@ r5 = phase << 16/frac, r6 = &s[pos], r7 = d, r8 = dstrem
|
|
@ r9 = scratch/acclo, r10 = scratch/acchi
|
|
@ r11 = c2, r12 = c3, c1 calculated in frac loop
|
|
@ r14 = delta << 16
|
|
@
|
|
@ Try to avoid overflow as much as possible and at the same time preserve
|
|
@ accuracy. Same formulas apply to downsampling but registers and
|
|
@ instruction order differ due to specific constraints.
|
|
@ c1 = -0.5*x3 + 0.5*x1
|
|
@ = 0.5*(x1 - x3) <--
|
|
@
|
|
@ v = x1 - x2, -v = x2 - x1
|
|
@ c2 = x3 - 2.5*x2 + 2*x1 - 0.5*x0
|
|
@ = x3 + 2*(x1 - x2) - 0.5*(x0 + x2)
|
|
@ = x3 + 2*v - 0.5*(x0 + x2) <--
|
|
@
|
|
@ c3 = -0.5*x3 + 1.5*x2 - 1.5*x1 + 0.5*x0
|
|
@ = 0.5*(x0 - x3 + (x2 - x1)) + (x2 - x1)
|
|
@ = 0.5*(x0 - x3 - v) - v <--
|
|
.hrs_usloop_carry:
|
|
ldr r4, [r6], #4 @ x0 = s[pos]
|
|
sub r9, r3, r2 @ r9 = v, r11 = c2, r12 = c3
|
|
add r11, r1, r9, asl #1 @
|
|
add r10, r4, r2 @
|
|
sub r12, r4, r1 @
|
|
sub r12, r12, r9 @
|
|
sub r11, r11, r10, asr #1 @
|
|
rsb r12, r9, r12, asr #1 @
|
|
.hrs_usloop_frac:
|
|
mov r5, r5, lsr #16 @ r5 = phase -> frac
|
|
smull r9, r10, r12, r5 @ acc = frac * c3 + c2
|
|
add r9, r11, r9, lsr #16 @
|
|
add r9, r9, r10, asl #16 @
|
|
smull r9, r10, r5, r9 @ acc = frac * acc + c1
|
|
mov r9, r9, lsr #16 @
|
|
orr r9, r9, r10, asl #16 @
|
|
sub r10, r3, r1 @
|
|
add r9, r9, r10, asr #1 @
|
|
smull r9, r10, r5, r9 @ acc = frac * acc + x2
|
|
subs r8, r8, #1 @ destination full?
|
|
add r9, r2, r9, lsr #16 @
|
|
add r9, r9, r10, asl #16 @
|
|
str r9, [r7], #4 @ *d++ = acc
|
|
bls .hrs_usfull @ yes? channel is done
|
|
adds r5, r14, r5, lsl #16 @ frac += delta
|
|
bcc .hrs_usloop_frac @ if carry is set, pos is incremented
|
|
|
|
subs r0, r0, #1 @ if dte > 0, do another sample
|
|
mov r1, r2 @ x3 = x2
|
|
mov r2, r3 @ x2 = x1
|
|
mov r3, r4 @ x1 = x0
|
|
bgt .hrs_usloop_carry
|
|
b .hrs_usdone
|
|
|
|
.hrs_usfull:
|
|
adds r5, r14, r5, lsl #16 @ do missed phase increment
|
|
bcc .hrs_usdone @
|
|
sub r0, r0, #1 @ do missed dte decrement
|
|
mov r1, r2 @ do missed history update
|
|
mov r2, r3 @
|
|
mov r3, r4 @
|
|
|
|
.hrs_usdone:
|
|
ldr r9, [sp], #4 @ r9 = pop srcrem
|
|
mov r14, r14, lsr #16 @ restore delta for next round
|
|
sub r0, r9, r0 @ r0 = pos = srcrem - dte
|
|
orr r5, r5, r0 @ reconstruct swapped phase
|
|
mov r5, r5, ror #16 @ swap pos and frac for phase
|
|
b .hrs_channel_done
|
|
|
|
/** Downsampling **/
|
|
@ Register usage in loop:
|
|
@ r0 = pos/frac
|
|
@ r1 = x3, r2 = x2, r3 = x1, r4 = x0
|
|
@ r5 = phase, r6 = &s[pos], r7 = d, r8 = dstrem
|
|
@ r9 = srcrem, r10 = scratch/acclo
|
|
@ r11 = c2/scratch, r12 = c3/acchi
|
|
@ r14 = delta
|
|
.hrs_dsloop_4:
|
|
ldmdb r6, { r1-r3 } @ x3..x0 = s[pos-3]..s[pos-1]
|
|
b .hrs_dsloop
|
|
.hrs_dsloop_3:
|
|
ldmdb r6, { r2-r3 } @ x2..x0 = s[pos-2]..s[pos-1]
|
|
mov r1, r4 @ x3 = x0
|
|
b .hrs_dsloop
|
|
.hrs_dsloop_2:
|
|
mov r1, r3 @ x3 = x1
|
|
ldr r3, [r6, #-4] @ x1 = s[pos-1]
|
|
mov r2, r4 @ x2 = x0
|
|
b .hrs_dsloop
|
|
.hrs_dsloop_1: @ expected loop destination
|
|
mov r1, r2 @ x3 = x2
|
|
mov r2, r3 @ x2 = x1
|
|
mov r3, r4 @ x1 = x0
|
|
.hrs_dsloop:
|
|
subs r8, r8, #1 @ destination full?
|
|
cmpgt r9, r0 @ ... || pos >= srcrem?
|
|
ble .hrs_channel_done
|
|
.hrs_dsstart:
|
|
ldr r4, [r6] @ x0 = s[pos]
|
|
sub r10, r3, r2 @ r10 = v, r11 = c2, r12 = c3
|
|
add r11, r4, r2 @
|
|
bic r0, r5, r0, lsl #16 @ r0 = frac = phase & 0xffff
|
|
sub r11, r1, r11, asr #1 @
|
|
add r11, r11, r10, asl #1 @
|
|
sub r12, r4, r1 @
|
|
sub r12, r12, r10 @
|
|
rsb r12, r10, r12, asr #1 @
|
|
smull r10, r12, r0, r12 @ acc = frac * c3 + c2
|
|
add r10, r11, r10, lsr #16 @
|
|
add r10, r10, r12, asl #16 @
|
|
sub r11, r3, r1 @
|
|
smull r10, r12, r0, r10 @ acc = frac * acc + c1
|
|
mov r11, r11, asr #1 @
|
|
add r10, r11, r10, lsr #16 @
|
|
add r10, r10, r12, asl #16 @
|
|
smull r10, r12, r0, r10 @ acc = frac * acc + x2
|
|
mov r11, r5, lsr #16 @ r11 = last_pos
|
|
add r5, r5, r14 @ phase += delta
|
|
mov r0, r5, lsr #16 @ r0 = pos = phase >> 16
|
|
add r10, r2, r10, lsr #16 @
|
|
add r10, r10, r12, asl #16 @
|
|
str r10, [r7], #4 @ *d++ = acc
|
|
|
|
cmp r0, r9 @ r0 = pos = MIN(pos, srcrem)
|
|
movgt r0, r9 @
|
|
sub r11, r0, r11 @ shift = pos - last_pos
|
|
cmp r11, #4 @
|
|
add r6, r6, r11, lsl #2 @ r6 += shift * 4
|
|
bge .hrs_dsloop_4 @
|
|
ldr pc, [pc, r11, lsl #2] @ branch to corresponding loop address
|
|
.word 0, 0
|
|
.word .hrs_dsloop_1
|
|
.word .hrs_dsloop_2
|
|
.word .hrs_dsloop_3
|
|
|
|
.hrs_channel_done:
|
|
ldmfd sp!, { r10, r12 } @ recover ch, h
|
|
subs r10, r10, #1 @ --ch
|
|
stmia r12!, { r1-r3 } @ h[0..2] = x3..x1
|
|
ldmgtia sp, { r0-r2 } @ load data, src, dst
|
|
bgt .hrs_channel_loop
|
|
|
|
ldmfd sp!, { r1-r3 } @ pop data, src, dst
|
|
sub r5, r5, r0, lsl #16 @ r5 = phase - (pos << 16)
|
|
ldr r2, [r3, #12] @ r2 = dst->bufcount
|
|
str r5, [r1, #4] @ data->phase = r5
|
|
sub r2, r2, r8 @ r2 = dst->bufcount - dstrem
|
|
str r2, [r3] @ dst->remcount = r2
|
|
ldmpc regs=r4-r11 @ ... and we're out
|
|
.size resample_hermite, .-resample_hermite
|
|
|
|
/****************************************************************************
|
|
* void pga_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p)
|
|
*/
|
|
.section .text, "ax",%progbits
|
|
.global pga_process
|
|
.type pga_process, %function
|
|
pga_process:
|
|
@ input: r0 = this, r1 = buf_p
|
|
ldr r0, [r0] @ r0 = data = this->data (&pga_data)
|
|
ldr r1, [r1] @ r1 = buf = *buf_p;
|
|
stmfd sp!, { r4-r8, lr }
|
|
|
|
ldr r4, [r0] @ r4 = data->gain
|
|
ldr r0, [r1], #4 @ r0 = buf->remcount, r1 = buf->p32
|
|
ldrb r3, [r1, #13] @ r3 = buf->format.num_channels
|
|
|
|
.pga_channelloop:
|
|
ldr r2, [r1], #4 @ r2 = buf->p32[ch] and inc index of p32
|
|
subs r12, r0, #1 @ r12 = count - 1
|
|
beq .pga_singlesample @ Zero? Only one sample!
|
|
|
|
.pga_loop:
|
|
ldmia r2, { r5, r6 } @ load r5, r6 from r2 (*p32[ch])
|
|
smull r7, r8, r5, r4 @ r7 = FRACMUL_SHL(r5, r4, 8)
|
|
smull r14, r5, r6, r4 @ r14 = FRACMUL_SHL(r6, r4, 8)
|
|
subs r12, r12, #2
|
|
mov r7, r7, lsr #23
|
|
mov r14, r14, lsr #23
|
|
orr r7, r7, r8, asl #9
|
|
orr r14, r14, r5, asl #9
|
|
stmia r2!, { r7, r14 } @ save r7, r14 to *p32[ch] and increment
|
|
bgt .pga_loop @ end of pga loop
|
|
|
|
blt .pga_evencount @ < 0? even count
|
|
|
|
.pga_singlesample:
|
|
ldr r5, [r2] @ handle odd sample
|
|
smull r7, r8, r5, r4 @ r7 = FRACMUL_SHL(r5, r4, 8)
|
|
mov r7, r7, lsr #23
|
|
orr r7, r7, r8, asl #9
|
|
str r7, [r2]
|
|
|
|
.pga_evencount:
|
|
subs r3, r3, #1
|
|
bgt .pga_channelloop @ end of channel loop
|
|
|
|
ldmpc regs=r4-r8
|
|
.size pga_process, .-pga_process
|
|
|
|
/****************************************************************************
|
|
* void filter_process(struct dsp_filter *f, int32_t *buf[], int count,
|
|
* unsigned int channels)
|
|
*
|
|
* define HIGH_PRECISION as '1' to make filtering calculate lower bits after
|
|
* shifting. without this, "shift" - 1 of the lower bits will be lost here.
|
|
*/
|
|
#define HIGH_PRECISION 0
|
|
|
|
#if CONFIG_CPU == PP5002
|
|
.section .icode,"ax",%progbits
|
|
#else
|
|
.section .text, "ax",%progbits
|
|
#endif
|
|
.global filter_process
|
|
filter_process:
|
|
@input: r0 = f, r1 = buf, r2 = count, r3 = channels
|
|
stmfd sp!, { r4-r11, lr } @ save all clobbered regs
|
|
ldmia r0!, { r4-r8 } @ load coefs, r0 = f->history
|
|
sub r3, r3, #1 @ r3 = ch = channels - 1
|
|
stmfd sp!, { r0-r3 } @ save adjusted params
|
|
ldrb r14, [r0, #32] @ r14 = shift
|
|
|
|
@ Channels are processed high to low while history is saved low to high
|
|
@ It's really noone's business how we do this
|
|
.fp_channelloop:
|
|
ldmia r0, { r9-r12 } @ load history, r0 = history[channels-ch-1]
|
|
ldr r3, [r1, r3, lsl #2] @ r3 = buf[ch]
|
|
|
|
@ r9-r12 = history, r4-r8 = coefs, r0..r1 = accumulator,
|
|
@ r2 = number of samples, r3 = buf[ch], r14 = shift amount
|
|
.fp_loop:
|
|
@ Direct form 1 filtering code.
|
|
@ y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
|
|
@ where y[] is output and x[] is input. This is performed out of order to
|
|
@ reuse registers, we're pretty short on regs.
|
|
smull r0, r1, r5, r9 @ acc = b1*x[i - 1]
|
|
smlal r0, r1, r6, r10 @ acc += b2*x[i - 2]
|
|
mov r10, r9 @ fix input history
|
|
ldr r9, [r3] @ load input and fix history
|
|
smlal r0, r1, r7, r11 @ acc += a1*y[i - 1]
|
|
smlal r0, r1, r8, r12 @ acc += a2*y[i - 2]
|
|
smlal r0, r1, r4, r9 @ acc += b0*x[i] /* avoid stall on arm9 */
|
|
mov r12, r11 @ fix output history
|
|
mov r11, r1, asl r14 @ get upper part of result and shift left
|
|
#if HIGH_PRECISION
|
|
rsb r1, r14, #32 @ get shift amount for lower part
|
|
orr r11, r11, r0, lsr r1 @ then mix in correctly shifted lower part
|
|
#endif
|
|
str r11, [r3], #4 @ save result
|
|
subs r2, r2, #1 @ are we done with this channel?
|
|
bgt .fp_loop @
|
|
|
|
ldr r3, [sp, #12] @ r3 = ch
|
|
ldr r0, [sp] @ r0 = history[channels-ch-1]
|
|
subs r3, r3, #1 @ all channels processed?
|
|
stmia r0!, { r9-r12 } @ save back history, history++
|
|
ldmhsib sp, { r1-r2 } @ r1 = buf, r2 = count
|
|
strhs r3, [sp, #12] @ store ch
|
|
strhs r0, [sp] @ store history[channels-ch-1]
|
|
bhs .fp_channelloop
|
|
|
|
add sp, sp, #16 @ compensate for temp storage
|
|
ldmpc regs=r4-r11
|
|
.size filter_process, .-filter_process
|
|
|
|
#if ARM_ARCH < 6
|
|
/****************************************************************************
|
|
* void sample_output_mono(struct sample_io_data *this,
|
|
* struct dsp_buffer *src,
|
|
* struct dsp_buffer *dst)
|
|
*/
|
|
.section .icode,"ax",%progbits
|
|
.global sample_output_mono
|
|
.type sample_output_mono, %function
|
|
sample_output_mono:
|
|
@ input: r0 = this, r1 = src, r2 = dst
|
|
stmfd sp!, { r4-r6, lr }
|
|
|
|
ldr r0, [r0] @ r0 = this->outcount
|
|
ldr r3, [r2, #4] @ r2 = dst->p16out
|
|
ldr r2, [r1, #4] @ r1 = src->p32[0]
|
|
ldrb r1, [r1, #19] @ r2 = src->format.output_scale
|
|
|
|
mov r4, #1
|
|
mov r4, r4, lsl r1 @ r4 = 1 << (scale-1)
|
|
mov r4, r4, lsr #1
|
|
mvn r14, #0x8000 @ r14 = 0xffff7fff, needed for
|
|
@ clipping and masking
|
|
subs r0, r0, #1 @
|
|
beq .som_singlesample @ Zero? Only one sample!
|
|
|
|
.somloop:
|
|
ldmia r2!, { r5, r6 }
|
|
add r5, r5, r4 @ r6 = (r6 + 1<<(scale-1)) >> scale
|
|
mov r5, r5, asr r1
|
|
mov r12, r5, asr #15
|
|
teq r12, r12, asr #31
|
|
eorne r5, r14, r5, asr #31 @ Clip (-32768...+32767)
|
|
add r6, r6, r4
|
|
mov r6, r6, asr r1 @ r7 = (r7 + 1<<(scale-1)) >> scale
|
|
mov r12, r6, asr #15
|
|
teq r12, r12, asr #31
|
|
eorne r6, r14, r6, asr #31 @ Clip (-32768...+32767)
|
|
|
|
and r5, r5, r14, lsr #16
|
|
and r6, r6, r14, lsr #16
|
|
orr r5, r5, r5, lsl #16 @ pack first 2 halfwords into 1 word
|
|
orr r6, r6, r6, lsl #16 @ pack last 2 halfwords into 1 word
|
|
stmia r3!, { r5, r6 }
|
|
|
|
subs r0, r0, #2
|
|
bgt .somloop
|
|
|
|
ldmpc cond=lt, regs=r4-r6 @ even 'count'? return
|
|
|
|
.som_singlesample:
|
|
ldr r5, [r2] @ do odd sample
|
|
add r5, r5, r4
|
|
mov r5, r5, asr r1
|
|
mov r12, r5, asr #15
|
|
teq r12, r12, asr #31
|
|
eorne r5, r14, r5, asr #31
|
|
|
|
and r5, r5, r14, lsr #16 @ pack 2 halfwords into 1 word
|
|
orr r5, r5, r5, lsl #16
|
|
str r5, [r3]
|
|
|
|
ldmpc regs=r4-r6
|
|
.size sample_output_mono, .-sample_output_mono
|
|
|
|
/****************************************************************************
|
|
* void sample_output_stereo(struct sample_io_data *this,
|
|
* struct dsp_buffer *src,
|
|
* struct dsp_buffer *dst)
|
|
*/
|
|
.section .icode,"ax",%progbits
|
|
.global sample_output_stereo
|
|
.type sample_output_stereo, %function
|
|
sample_output_stereo:
|
|
@ input: r0 = this, r1 = src, r2 = dst
|
|
stmfd sp!, { r4-r9, lr }
|
|
|
|
ldr r0, [r0] @ r0 = this->outcount
|
|
ldr r3, [r2, #4] @ r3 = dsp->p16out
|
|
ldmib r1, { r2, r5 } @ r2 = src->p32[0], r5 = src->p32[1]
|
|
ldrb r1, [r1, #19] @ r1 = src->format.output_scale
|
|
|
|
mov r4, #1
|
|
mov r4, r4, lsl r1 @ r4 = 1 << (scale-1)
|
|
mov r4, r4, lsr #1 @
|
|
|
|
mvn r14, #0x8000 @ r14 = 0xffff7fff, needed for
|
|
@ clipping and masking
|
|
subs r0, r0, #1 @
|
|
beq .sos_singlesample @ Zero? Only one sample!
|
|
|
|
.sosloop:
|
|
ldmia r2!, { r6, r7 } @ 2 left
|
|
ldmia r5!, { r8, r9 } @ 2 right
|
|
|
|
add r6, r6, r4 @ r6 = (r6 + 1<<(scale-1)) >> scale
|
|
mov r6, r6, asr r1
|
|
mov r12, r6, asr #15
|
|
teq r12, r12, asr #31
|
|
eorne r6, r14, r6, asr #31 @ Clip (-32768...+32767)
|
|
add r7, r7, r4
|
|
mov r7, r7, asr r1 @ r7 = (r7 + 1<<(scale-1)) >> scale
|
|
mov r12, r7, asr #15
|
|
teq r12, r12, asr #31
|
|
eorne r7, r14, r7, asr #31 @ Clip (-32768...+32767)
|
|
|
|
add r8, r8, r4 @ r8 = (r8 + 1<<(scale-1)) >> scale
|
|
mov r8, r8, asr r1
|
|
mov r12, r8, asr #15
|
|
teq r12, r12, asr #31
|
|
eorne r8, r14, r8, asr #31 @ Clip (-32768...+32767)
|
|
add r9, r9, r4 @ r9 = (r9 + 1<<(scale-1)) >> scale
|
|
mov r9, r9, asr r1
|
|
mov r12, r9, asr #15
|
|
teq r12, r12, asr #31
|
|
eorne r9, r14, r9, asr #31 @ Clip (-32768...+32767)
|
|
|
|
and r6, r6, r14, lsr #16 @ pack first 2 halfwords into 1 word
|
|
orr r8, r6, r8, asl #16
|
|
and r7, r7, r14, lsr #16 @ pack last 2 halfwords into 1 word
|
|
orr r9, r7, r9, asl #16
|
|
|
|
stmia r3!, { r8, r9 }
|
|
|
|
subs r0, r0, #2
|
|
bgt .sosloop
|
|
|
|
ldmpc cond=lt, regs=r4-r9 @ even 'count'? return
|
|
|
|
.sos_singlesample:
|
|
ldr r6, [r2] @ left odd sample
|
|
ldr r8, [r5] @ right odd sample
|
|
|
|
add r6, r6, r4 @ r6 = (r7 + 1<<(scale-1)) >> scale
|
|
mov r6, r6, asr r1
|
|
mov r12, r6, asr #15
|
|
teq r12, r12, asr #31
|
|
eorne r6, r14, r6, asr #31 @ Clip (-32768...+32767)
|
|
add r8, r8, r4 @ r8 = (r8 + 1<<(scale-1)) >> scale
|
|
mov r8, r8, asr r1
|
|
mov r12, r8, asr #15
|
|
teq r12, r12, asr #31
|
|
eorne r8, r14, r8, asr #31 @ Clip (-32768...+32767)
|
|
|
|
and r6, r6, r14, lsr #16 @ pack 2 halfwords into 1 word
|
|
orr r8, r6, r8, asl #16
|
|
|
|
str r8, [r3]
|
|
|
|
ldmpc regs=r4-r9
|
|
.size sample_output_stereo, .-sample_output_stereo
|
|
#endif /* ARM_ARCH < 6 */
|