/***************************************************************************
 *             __________               __   ___.
 *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
 *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
 *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
 *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
 *                     \/            \/     \/    \/            \/
 * $Id$
 *
 * Copyright (C) 2005 by David Bryant
 *
 * All files in this archive are subject to the GNU General Public License.
 * See the file COPYING in the source tree root for full license agreement.
 *
 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
 * KIND, either express or implied.
 *
 ****************************************************************************/

/* This is an assembly optimized version of the following WavPack function:
 *
 * void decorr_stereo_pass_cont_mcf5249 (struct decorr_pass *dpp,
 *                                       long *buffer, long sample_count);
 *
 * It performs a single pass of stereo decorrelation on the provided buffer.
 * Note that this version of the function requires that the 8 previous stereo
 * samples are visible and correct. In other words, it ignores the "samples_*"
 * fields in the decorr_pass structure and gets the history data directly
 * from the buffer. It does, however, return the appropriate history samples
 * to the decorr_pass structure before returning.
 *
 * This is written to work on a MCF5249 processor, or any processor based on
 * the ColdFire V2 core with an EMAC unit. The EMAC is perfectly suited for
 * the "apply_weight" function of WavPack decorrelation because it provides
 * the requires 40-bit product. The fractional rounding mode of the EMAC is not
 * configurable and uses "round to even" while WavPack uses "round to larger",
 * so the rounding has to be done manually.
 */

        .text
        .align  2
        .global decorr_stereo_pass_cont_mcf5249

decorr_stereo_pass_cont_mcf5249:

        lea     (-44, %sp), %sp
        movem.l %d2-%d7/%a2-%a6, (%sp)
        move.l  44+4(%sp), %a2          | a2 = dpp->
        move.l  44+8(%sp), %a1          | a1 = bptr
        move.w  2(%a2), %a3             | a3 = dpp->delta
        move.w  4(%a2), %d3             | d3 = dpp->weight_A (sign extended)
        ext.l   %d3
        move.w  6(%a2), %d4             | d4 = dpp->weight_B (sign extended)
        ext.l   %d4
        move.l 44+12(%sp), %d0          | d0 = sample_count
        jbeq    return_only             | if zero, nothing to do

        lsl.l   #3, %d0                 | d5 = bptr + (sample_count * 8)
        move.l  %d0, %d5
        add.l   %a1, %d5

        moveq.l #17, %d0                | left shift weights & delta 17 places
        asl.l   %d0, %d3
        asl.l   %d0, %d4
        move.l  %a3, %d1
        asl.l   %d0, %d1
        move.l  %d1, %a3

        moveq.l #0x20, %d6
        move.l  %d6, %macsr             | set fractional mode for MAC
        move.l  #0x800000, %accext01    | acc1 = 0x00 0000 80 (for rounding)
        
        move.l  #1024<<17, %d6          | d6 & d7 are weight clipping limits
        move.l  #-1024<<17, %d7         | (only used by negative terms)

        move.w  (%a2), %d0              | d0 = term
        ext.l   %d0
        cmp.l   #17, %d0
        jbeq    term_17                 | term = 17
        cmp.l   #18, %d0
        jbeq    term_18                 | term = 18
        addq.l  #1, %d0
        jbeq    term_minus_1            | term = -1
        addq.l  #1, %d0
        jbeq    term_minus_2            | term = -2
        addq.l  #1, %d0
        jbeq    term_minus_3            | term = -3
        jbra    term_default            | default term = 1 - 8

|------------------------------------------------------------------------------
| Loop to handle term = 17 condition
|
| a0 =                          d0 = (2 * bptr [-1]) - bptr [-2]
| a1 = bptr                     d1 = initial bptr [0]
| a2 = dpp->                    d2 = updated bptr [0]
| a3 = dpp->delta << 17         d3 = dpp->weight_A << 17
| a4 =                          d4 = dpp->weight_B << 17
| a5 =                          d5 = eptr
| macsr = 0x20                  acc1 = 0x00 0000 80
|------------------------------------------------------------------------------

term_17:
        move.l  -8(%a1), %d0            | d0 = 2 * bptr [-1] - bptr [-2]
        add.l   %d0, %d0
        sub.l   -16(%a1), %d0
        beq     .L251                   | if zero, skip calculation
        move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + (d0 << 4) * weight_A
        mac.l   %d0, %d3, %acc0
        move.l  (%a1), %d1
        beq     .L255
        eor.l   %d1, %d0                | else compare signs
        bge     .L256                   | if same, add delta to weight
        sub.l   %a3, %d3                | else subtract delta from weight
        sub.l   %a3, %d3                | subtract again instead of branch
.L256:  add.l   %a3, %d3                | add delta to weight

.L255:  move.l  %acc0, %d2              | d2 = rounded product
        add.l   %d1, %d2                | update bptr [0] and store
        move.l  %d2, (%a1)+

.L253:  move.l  -8(%a1), %d0            | d0 = 2 * bptr [-1] - bptr [-2]
        add.l   %d0, %d0
        sub.l   -16(%a1), %d0
        beq     .L257                   | if zero, skip calculations
        move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + (d0 << 4) * weight_B
        mac.l   %d0, %d4, %acc0
        move.l  (%a1), %d1
        beq     .L254
        eor.l   %d1, %d0                | else compare signs
        bge     .L259                   | if same, add delta to weight
        sub.l   %a3, %d4                | else subtract delta from weight
        sub.l   %a3, %d4                | subtract again instead of branch
.L259:  add.l   %a3, %d4                | add delta to weight

.L254:  move.l  %acc0, %d2              | d2 = rounded product
        add.l   %d1, %d2                | update bptr [0] and store
        move.l  %d2, (%a1)+

.L252:  cmp.l   %a1, %d5                | loop if bptr < eptr
        jbhi    term_17
        bra     term_17_18_finish       | exit through common path

.L251:  addq.l  #4, %a1                 | update point and jump back into loop
        bra     .L253

.L257:  addq.l  #4, %a1                 | update point and jump back into loop
        bra     .L252

|------------------------------------------------------------------------------
| Loop to handle term = 18 condition
|
| a0 =                          d0 = ((3 * bptr [-1]) - bptr [-2]) >> 1
| a1 = bptr                     d1 = initial bptr [0]
| a2 = dpp->                    d2 = updated bptr [0]
| a3 = dpp->delta << 17         d3 = dpp->weight_A << 17
| a4 =                          d4 = dpp->weight_B << 17
| a5 =                          d5 = eptr
| macsr = 0x20                  acc1 = 0x00 0000 80
|------------------------------------------------------------------------------

term_18:
        move.l  -8(%a1), %a0            | d0 = (3 * bptr [-1] - bptr [-2]) >> 1
        lea     (%a0,%a0.l*2), %a0
        move.l  %a0, %d0
        sub.l   -16(%a1), %d0
        asr.l   #1, %d0
        beq     .L260
        move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + (d0 << 4) * weight_A
        mac.l   %d0, %d3, %acc0
        move.l  (%a1), %d1
        beq     .L266
        eor.l   %d1, %d0                | else compare signs
        bge     .L267                   | if same, add delta to weight
        sub.l   %a3, %d3                | else subtract delta from weight
        sub.l   %a3, %d3                | subtract again instead of branch
.L267:  add.l   %a3, %d3                | add delta to weight

.L266:  move.l  %acc0, %d2              | d2 = rounded product
        add.l   %d1, %d2                | add applied weight to bptr [0], store
        move.l  %d2, (%a1)+

.L268:  move.l  -8(%a1), %a0            | d0 = (3 * bptr [-1] - bptr [-2]) >> 1
        lea     (%a0,%a0.l*2), %a0
        move.l  %a0, %d0
        sub.l   -16(%a1), %d0
        asr.l   #1, %d0
        beq     .L261
        move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + (d0 << 4) * weight_B
        mac.l   %d0, %d4, %acc0
        move.l  (%a1), %d1
        beq     .L265
        eor.l   %d1, %d0                | else compare signs
        bge     .L270                   | if same, add delta to weight
        sub.l   %a3, %d4                | else subtract delta from weight
        sub.l   %a3, %d4                | subtract again instead of branch
.L270:  add.l   %a3, %d4                | add delta to weight

.L265:  move.l  %acc0, %d2              | d2 = rounded product
        add.l   %d1, %d2                | add applied weight to bptr [0], store
        move.l  %d2, (%a1)+

.L269:  cmp.l   %a1, %d5                | loop if bptr < eptr
        jbhi    term_18
        bra     term_17_18_finish       | exit through common path

.L260:  addq.l  #4, %a1                 | bump pointer and jump back into loop
        bra     .L268

.L261:  addq.l  #4, %a1                 | bump pointer and jump back into loop
        bra     .L269

term_17_18_finish:
        move.l  -4(%a1), 40(%a2)        | restore dpp->samples_A [0-1], B [0-1]
        move.l  -8(%a1), 8(%a2)
        move.l  -12(%a1), 44(%a2)
        move.l  -16(%a1), 12(%a2)
        jbra    finish_up

|------------------------------------------------------------------------------
| Loop to handle default terms (i.e. 1 - 8)
|
| a0 = tptr                     d0 = tptr [0]
| a1 = bptr                     d1 = initial bptr [0]
| a2 = dpp->                    d2 = updated bptr [0]
| a3 = dpp->delta << 17         d3 = dpp->weight_A << 17
| a4 =                          d4 = dpp->weight_B << 17
| a5 =                          d5 = eptr
| macsr = 0x20                  acc1 = 0x00 0000 80
|------------------------------------------------------------------------------

term_default:
        move.w  (%a2), %d0              | a0 = a1 - (dpp->term * 8)
        ext.l   %d0
        lsl.l   #3, %d0
        move.l  %a1, %a0
        sub.l   %d0, %a0

term_default_loop:
        move.l  (%a0)+, %d0             | d0 = tptr [0], skip ahead if zero
        beq     .L271
        move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + (d0 << 4) * weight_A
        mac.l   %d0, %d3, %acc0
        move.l  (%a1), %d1
        beq     .L277
        eor.l   %d1, %d0                | else compare signs
        bge     .L278                   | if same, add delta to weight
        sub.l   %a3, %d3                | else subtract delta from weight
        sub.l   %a3, %d3                | subtract again instead of branch
.L278:  add.l   %a3, %d3                | add delta to weight

.L277:  move.l  %acc0, %d2              | d2 = rounded product
        add.l   %d1, %d2                | add applied weight to bptr [0], store
        move.l  %d2, (%a1)+

.L275:  move.l  (%a0)+, %d0             | d0 = tptr [0], skip ahead if zero
        beq     .L272
        move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + (d0 << 4) * weight_B
        mac.l   %d0, %d4, %acc0
        move.l  (%a1), %d1
        beq     .L276
        eor.l   %d1, %d0                | else compare signs
        bge     .L281                   | if same, add delta to weight
        sub.l   %a3, %d4                | else subtract delta from weight
        sub.l   %a3, %d4                | subtract again instead of branch
.L281:  add.l   %a3, %d4                | add delta to weight

.L276:  move.l  %acc0, %d2              | d2 = rounded product
        add.l   %d1, %d2                | add applied weight to bptr [0], store
        move.l  %d2, (%a1)+

.L274:  cmp.l   %a1, %d5                | loop back if bptr < eptr
        jbhi    term_default_loop
        move.w  (%a2), %d0              | d0 = term - 1
        moveq.l #8, %d1                 | d1 = loop counter

.L323:  subq.l  #1, %d0                 | back up & mask index
        and.l   #7, %d0
        move.l  -(%a1), 40(%a2,%d0.l*4) | store dpp->samples_B [d0]
        move.l  -(%a1), 8(%a2,%d0.l*4)  | store dpp->samples_A [d0]
        subq.l  #1, %d1                 | loop on count
        jbne    .L323
        jbra    finish_up

.L271:  addq.l  #4, %a1                 | bump pointer and jump back into loop
        bra     .L275

.L272:  addq.l  #4, %a1                 | bump pointer and jump back into loop
        bra     .L274


|------------------------------------------------------------------------------
| Loop to handle term = -1 condition
|
| a0 =                          d0 = decorrelation sample
| a1 = bptr                     d1 = initial bptr [0]
| a2 = dpp->                    d2 = updated bptr [0]
| a3 = dpp->delta << 17         d3 = dpp->weight_A << 17
| a4 =                          d4 = dpp->weight_B << 17
| a5 =                          d5 = eptr
| a6 =                          d6 = 1024 << 17
| a7 =                          d7 = -1024 << 17
| macsr = 0x20                  acc1 = 0x00 0000 80
|------------------------------------------------------------------------------

term_minus_1:
        move.l  -4(%a1), %d0            | d0 = bptr [-1]
        beq     .L402
        move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + ((d0 << 4) * weight_A)
        mac.l   %d0, %d3, %acc0
        move.l  (%a1), %d1
        beq     .L405
        eor.l   %d1, %d0                | else compare signs
        bge     .L404                   | if same, add delta to weight
        sub.l   %a3, %d3                | else subtract delta from weight
        cmp.l   %d7, %d3                | check for negative clip limit
        bge     .L405
        move.l  %d7, %d3
        bra     .L405

.L404:  add.l   %a3, %d3                | add delta to weight
        cmp.l   %d6, %d3                | check for positive clip limit
        ble     .L405
        move.l  %d6, %d3

.L405:  move.l  %acc0, %d0              | d2 = rounded product
        add.l   %d1, %d0                | add applied weight to bptr [0], store
        move.l  %d0, (%a1)+
        beq     .L401

.L410:  move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + ((d0 << 4) * weight_B)
        mac.l   %d0, %d4, %acc0
        move.l  (%a1), %d1
        beq     .L403
        eor.l   %d1, %d0                | else compare signs
        bge     .L407                   | if same, add delta to weight
        sub.l   %a3, %d4                | else subtract delta from weight
        cmp.l   %d7, %d4                | check for negative clip limit
        bge     .L403
        move.l  %d7, %d4
        bra     .L403

.L407:  add.l   %a3, %d4                | add delta to weight
        cmp.l   %d6, %d4                | check for positive clip limit
        ble     .L403
        move.l  %d6, %d4

.L403:  move.l  %acc0, %d2              | d2 = rounded product
        add.l   %d1, %d2                | add applied weight to bptr [1], store
        move.l  %d2, (%a1)+

.L411:  cmp.l   %a1, %d5                | loop back if bptr < eptr
        jbhi    term_minus_1
        move.l  -4(%a1), 8(%a2)         | dpp->samples_A [0] = bptr [-1]
        jbra    finish_up

.L402:  move.l  (%a1)+, %d0
        bne     .L410

.L401:  addq.l  #4, %a1
        bra     .L411


|------------------------------------------------------------------------------
| Loop to handle term = -2 condition
|
| a0 =                          d0 = decorrelation sample
| a1 = bptr                     d1 = initial bptr [0]
| a2 = dpp->                    d2 = updated bptr [0]
| a3 = dpp->delta << 17         d3 = dpp->weight_A << 17
| a4 =                          d4 = dpp->weight_B << 17
| a5 =                          d5 = eptr
| a6 =                          d6 = 1024 << 17
| a7 =                          d7 = -1024 << 17
| macsr = 0x20                  acc1 = 0x00 0000 80
|------------------------------------------------------------------------------

term_minus_2:
        move.l  -8(%a1), %d0            | d0 = bptr [-2]
        beq     .L511
        move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + ((d0 << 4) * weight_B)
        mac.l   %d0, %d4, %acc0
        move.l  4(%a1), %d1
        beq     .L505
        eor.l   %d1, %d0                | else compare signs
        bge     .L504                   | if same, add delta to weight
        sub.l   %a3, %d4                | else subtract delta from weight
        cmp.l   %d7, %d4                | ckeck for negative clip limit
        bge     .L505
        move.l  %d7, %d4
        bra     .L505

.L504:  add.l   %a3, %d4                | add delta to weight
        cmp.l   %d6, %d4                | check for positive clip limit
        ble     .L505
        move.l  %d6, %d4

.L505:  move.l  %acc0, %d0              | d2 = rounded product
        add.l   %d1, %d0                | add applied weight to bptr [0], store
        move.l  %d0, 4(%a1)
        beq     .L512

.L510:  move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + ((d0 << 4) * weight_A)
        mac.l   %d0, %d3, %acc0
        move.l  (%a1), %d1
        beq     .L503
        eor.l   %d1, %d0                | else compare signs
        bge     .L507                   | if same, add delta to weight
        sub.l   %a3, %d3                | else subtract delta from weight
        cmp.l   %d7, %d3                | check for negative clip limit
        bge     .L503
        move.l  %d7, %d3
        bra     .L503

.L507:  add.l   %a3, %d3                | add delta to weight
        cmp.l   %d6, %d3                | check for negative clip limit
        ble     .L503
        move.l  %d6, %d3

.L503:  move.l  %acc0, %d2              | d2 = rounded product
        add.l   %d1, %d2                | add applied weight to bptr [1], store
        move.l  %d2, (%a1)

.L512:  addq.l  #8, %a1
        cmp.l   %a1, %d5                | loop if bptr < eptr
        jbhi    term_minus_2
        move.l  -8(%a1), 40(%a2)        | dpp->samples_B [0] = bptr [-4]
        jbra    finish_up

.L511:  move.l  4(%a1), %d0
        beq     .L512
        bra     .L510


|------------------------------------------------------------------------------
| Loop to handle term = -3 condition
|
| a0 =                          d0 = decorrelation sample
| a1 = bptr                     d1 = initial bptr [0]
| a2 = dpp->                    d2 = updated bptr [0]
| a3 = dpp->delta << 17         d3 = dpp->weight_A << 17
| a4 =                          d4 = dpp->weight_B << 17
| a5 =                          d5 = eptr
| a6 =                          d6 = 1024 << 17
| a7 =                          d7 = -1024 << 17
| macsr = 0x20                  acc1 = 0x00 0000 80
|------------------------------------------------------------------------------

term_minus_3:
        move.l  -4(%a1), %d0            | d0 = bptr [-1]
        beq     .L301
        move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + ((d0 << 4) * weight_A)
        mac.l   %d0, %d3, %acc0
        move.l  (%a1), %d1
        beq     .L320
        eor.l   %d1, %d0                | else compare signs
        bge     .L319                   | if same, add delta to weight
        sub.l   %a3, %d3                | else subtract delta from weight
        cmp.l   %d7, %d3                | check for negative clip limit
        bge     .L320
        move.l  %d7, %d3
        bra     .L320

.L319:  add.l   %a3, %d3                | add delta to weight
        cmp.l   %d6, %d3                | check for positive clip limit
        ble     .L320
        move.l  %d6, %d3

.L320:  move.l  %acc0, %d2              | d2 = rounded product
        add.l   %d1, %d2                | add applied weight to bptr [0], store
        move.l  %d2, (%a1)+

.L330:  move.l  -12(%a1), %d0           | d0 = bptr [-2]
        beq     .L302
        move.l  %acc1, %acc0
        asl.l   #4, %d0                 | acc0 = acc1 + ((d0 << 4) * weight_B)
        mac.l   %d0, %d4, %acc0
        move.l  (%a1), %d1
        beq     .L318
        eor.l   %d1, %d0                | else compare signs
        bge     .L322                   | if same, add delta to weight
        sub.l   %a3, %d4                | else subtract delta from weight
        cmp.l   %d7, %d4                | check for negative clip limit
        bge     .L318
        move.l  %d7, %d4
        bra     .L318

.L322:  add.l   %a3, %d4                | add delta to weight
        cmp.l   %d6, %d4                | check for positive clip limit
        ble     .L318
        move.l  %d6, %d4

.L318:  move.l  %acc0, %d2              | d2 = rounded product
        add.l   %d1, %d2                | add applied weight to bptr [1], store
        move.l  %d2, (%a1)+

.L331:  cmp.l   %a1, %d5                | bptr, eptr
        jbhi    term_minus_3
        move.l  -4(%a1), 8(%a2)         | dpp->samples_A [0] = bptr [-1]
        move.l  -8(%a1), 40(%a2)        | dpp->samples_B [0] = bptr [-2]
        jbra    finish_up

.L301:  addq.l  #4, %a1
        bra     .L330

.L302:  addq.l  #4, %a1
        bra     .L331

| finish and return

finish_up:
        moveq.l #17, %d0
        asr.l   %d0, %d3
        asr.l   %d0, %d4
        move.w  %d3, 4(%a2)     | weight_A, dpp->weight_A
        move.w  %d4, 6(%a2)     | weight_B, dpp->weight_B

        clr.l   %d0             | clear up EMAC
        move.l  %d0, %acc0
        move.l  %d0, %acc1

return_only:
        movem.l (%sp), %d2-%d7/%a2-%a6
        lea     (44,%sp), %sp
        rts
