apps/dsp_arm.S

   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  ****************************************************************************/
  19
  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]
  30
  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
  36
  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
  52
  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
  64
  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
  68
  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
  73
  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
  82
  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
 110
 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
 140
 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
 172
 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
 204