/*************************************************************************** * __________ __ ___. * Open \______ \ ____ ____ | | _\_ |__ _______ ___ * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ * \/ \/ \/ \/ \/ * * 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" .syntax unified /**************************************************************************** * 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 #ifndef CPU_ARM_MICRO /**************************************************************************** * 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 ldmdbge 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 ldmiagt 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 #endif /* CPU_ARM_MICRO */ /**************************************************************************** * 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 #ifndef CPU_ARM_MICRO /**************************************************************************** * 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 defined(CPU_PP) .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++ ldmibhs 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 #endif /* CPU_ARM_MICRO */ #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 */