apps/dsp_cf.S

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2006 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     .section .text
  21     .global apply_crossfeed
  22 apply_crossfeed:
  23     lea.l (-44, %sp), %sp
  24     movem.l %d2-%d7/%a2-%a6, (%sp)      | save all regs
  25     move.l (44+4, %sp), %a4
  26     movem.l (%a4), %a4-%a5              | a4 = src[0], a5 = src[1]
  27     move.l (44+8, %sp), %d7             | d7 = count
  28
  29     lea.l crossfeed_data, %a1
  30     lea.l (8*4, %a1), %a0               | a0 = &delay[0][0]
  31     move.l (%a1)+, %a6                  | a6 = direct gain
  32     movem.l (3*4, %a1), %d0-%d3         | fetch filter history samples
  33     move.l (33*4, %a1), %d4             | fetch delay line index
  34     movem.l (%a1), %a1-%a3              | load filter coefs
  35     move.l %d4, %d5
  36     lsl.l #3, %d5
  37     add.l %d5, %a0                      | point a0 to current delay position
  38 |    lea.l (%d4*4, %a0), %a0
  39 |    lea.l (%d4*4, %a0), %a0             | point a0 to current delay position
  40     /* Register usage in loop:
  41      * a0 = &delay[index][0], a1..a3 = b0, b1, a1 (filter coefs),
  42      * a4 = src[0], a5 = src[1], a6 = direct gain,
  43      * d0..d3 = history
  44      * d4 = delay line index,
  45      * d5,d6 = temp.
  46      * d7 = count
  47      */
  48 .cfloop:
  49     mac.l %a2, %d0, (4, %a0), %d0, %acc0 | acc = b1*dr[n - 1] d0 = dr[n]
  50     mac.l %a1, %d0, %acc0               | acc += b0*dr[n]
  51     mac.l %a3, %d1, (%a4), %d5, %acc0   | acc += a1*y_l[n - 1], load left input
  52     move.l %acc0, %d1                   | get filtered delayed sample
  53     mac.l %a6, %d5, %acc0               | acc += gain*x_l[n]
  54     movclr.l %acc0, %d6
  55     move.l %d6, (%a4)+                  | write result
  56
  57     mac.l %a2, %d2, (%a0), %d2, %acc0   | acc = b1*dl[n - 1], d2 = dl[n]
  58     move.l %d5, (%a0)+                  | save left input to delay line
  59     mac.l %a1, %d2, %acc0               | acc += b0*dl[n]
  60     mac.l %a3, %d3, (%a5), %d5, %acc0   | acc += a1*y_r[n - 1], load right input
  61     move.l %acc0, %d3                   | get filtered delayed sample
  62     mac.l %a6, %d5, %acc0               | acc += gain*x_r[n]
  63     move.l %d5, (%a0)+                  | save right input to delay line
  64     movclr.l %acc0, %d6
  65     move.l %d6, (%a5)+                  | write result
  66
  67     addq.l #1, %d4                      | index++
  68     moveq.l #13, %d6
  69     cmp.l %d6, %d4                      | wrap index to 0 if it overflows
  70     jlt .nowrap
  71     moveq.l #13*8, %d4
  72     sub.l %d4, %a0                      | wrap back delay line ptr as well
  73     clr.l %d4
  74 .nowrap:
  75     subq.l #1, %d7
  76     jne .cfloop
  77     | save data back to struct
  78     lea.l crossfeed_data + 4*4, %a1
  79     movem.l %d0-%d3, (%a1)
  80     move.l %d4, (30*4, %a1)
  81     movem.l (%sp), %d2-%d7/%a2-%a6
  82     lea.l (44, %sp), %sp
  83     rts
  84