REQUIRE8
PRESERVE8

SECTION .text:CODE:NOROOT(2)
PUBLIC MULSHIFT32
THUMB
MULSHIFT32
smull r2, r0, r1, r0
//mov pc, lr // KJ changed to BX for Cortex
BX lr



SECTION .text:CODE:NOROOT(2)
PUBLIC FASTABS
THUMB
FASTABS
mov r1, r0
mov r0, #0x0
eor r0, r1, r1, asr # 31
sub r0, r0, r1, asr # 31
bx lr

END

#define PCM r0
#define VB1 r1
#define COEF r2
; must push PCM ptr to stack during inner looop
#define VLO r0
; temp variable
#define VHI r3

#define SUM1LL r4
#define SUM1LH r5
#define SUM2LL r6
#define SUM2LH r7
#define SUM1RL r8
#define SUM1RH r9
#define SUM2RL r10
#define SUM2RH r11

#define CF1 r12
#define CF2 r14

; used for clipping - after discarding CF1
#define SIGN r12
; used for clipping - after discarding CF2
#define MAXPOS r14
; overlay loop counter with CF1, SIGN
#define I r12



#define RNDVAL (1 << ((32 - 12) + (6 - 1)))

/*; C64TOS - clip 64-bit accumulator to short (no rounding)
; xl, xh = value (lo 32, hi 32)
; input assumed to have 6 fraction bits
; sign = temp variable to use for sign
; maxPos = 0x00007fff (takes 2 instr. to generate - calculating
; once and using repeatedly saves if you do several CTOS in a row) */
C64TOS MACRO _xl, _xh, _sign, _maxPos
mov _xl, _xl, lsr #(20+6)
orr _xl, _xl, _xh, lsl #(12-6)
mov _sign, _xl, ASR #31
cmp _sign, _xl, ASR #15
it ne
eorne _xl, _sign, _maxPos
ENDM ; C64TOS

/* ; MC0S - process 2 taps, 1 sample per channel (sample 0) */
/* ; x = vb1 offset */
MC0S MACRO val_x
ldr CF1, [COEF], #4
ldr CF2, [COEF], #4
ldr VLO, [VB1, #(4*(val_x))]
ldr VHI, [VB1, #(4*(23 - val_x))]

smlal SUM1LL, SUM1LH, VLO, CF1
ldr VLO, [VB1, #(4*(32 + val_x))]
rsb CF2, CF2, #0
smlal SUM1LL, SUM1LH, VHI, CF2
ldr VHI, [VB1, #(4*(32 + 23 - val_x))]

smlal SUM1RL, SUM1RH, VLO, CF1
smlal SUM1RL, SUM1RH, VHI, CF2
ENDM ;MC0S

/* ; MC1S - process 2 taps, 1 sample per channel (sample 16)
; x = vb1 offset */
MC1S MACRO val_x
ldr CF1, [COEF], #4
ldr VLO, [VB1, #(4*(val_x))]
ldr VHI, [VB1, #(4*(32 + val_x))]
smlal SUM1LL, SUM1LH, VLO, CF1
smlal SUM1RL, SUM1RH, VHI, CF1
ENDM ;MC1S

/* ; MC2S - process 2 taps, 2 samples per channel
; x = vb1 offset */
MC2S MACRO val_x
/* ; load data as far as possible in advance of using it */
ldr CF1, [COEF], #4
ldr CF2, [COEF], #4
ldr VLO, [VB1, #(4*(val_x))]
ldr VHI, [VB1, #(4*(23 - val_x))]

smlal SUM1LL, SUM1LH, VLO, CF1
smlal SUM2LL, SUM2LH, VLO, CF2
rsb CF2, CF2, #0
smlal SUM2LL, SUM2LH, VHI, CF1
smlal SUM1LL, SUM1LH, VHI, CF2

ldr VHI, [VB1, #(4*(32 + 23 - val_x))]
ldr VLO, [VB1, #(4*(32 + val_x))]

smlal SUM1RL, SUM1RH, VHI, CF2
smlal SUM2RL, SUM2RH, VHI, CF1
rsb CF2, CF2, #0
smlal SUM1RL, SUM1RH, VLO, CF1
smlal SUM2RL, SUM2RH, VLO, CF2
ENDM ;MC2S








/* ; void PolyphaseStereo(short *pcm, int *vbuf, const int *coefBase) */
SECTION .text:CODE:NOROOT(2)
PUBLIC xmp3_PolyphaseStereo
THUMB
xmp3_PolyphaseStereo:

stmfd sp!, {r4-r11, r14}

/* ; clear out stack space for 2 local variables (4 bytes each) */
sub sp, sp, #8
str PCM , [sp, #4] /*; sp[1] = pcm pointer */

/* ; special case, output sample 0 */
mov SUM1LL, #RNDVAL /* ; load rndVal (low 32) */
mov SUM1RL, #RNDVAL /* ; load rndVal (low 32) */
mov SUM1LH, #0
mov SUM1RH, #0

MC0S 0
MC0S 1
MC0S 2
MC0S 3
MC0S 4
MC0S 5
MC0S 6
MC0S 7

ldr PCM, [sp, #4] /* ; load pcm pointer */
mov MAXPOS, #0x7f00
orr MAXPOS, MAXPOS, #0xff

C64TOS SUM1LL, SUM1LH, SIGN, MAXPOS
C64TOS SUM1RL, SUM1RH, SIGN, MAXPOS

strh SUM1LL, [PCM, #(2*0)]
strh SUM1RL, [PCM, #(2*1)]

/* ; special case, output sample 16 */
add COEF, COEF, #(4*(256-16)) /* ; coef = coefBase + 256 (was coefBase + 16 after MC0S block) */
add VB1, VB1, #(4*1024) /* ; vb1 = vbuf + 64*16 */

mov SUM1LL, #RNDVAL /* ; load rndVal (low 32) */
mov SUM1RL, #RNDVAL /* ; load rndVal (low 32) */
mov SUM1LH, #0
mov SUM1RH, #0

MC1S 0
MC1S 1
MC1S 2
MC1S 3
MC1S 4
MC1S 5
MC1S 6
MC1S 7

ldr PCM, [sp, #4] /* load pcm pointer */
mov MAXPOS, #0x7f00
orr MAXPOS, MAXPOS, #0xff

C64TOS SUM1LL, SUM1LH, SIGN, MAXPOS
C64TOS SUM1RL, SUM1RH, SIGN, MAXPOS

strh SUM1LL, [PCM, #(2*(2*16+0))]
strh SUM1RL, [PCM, #(2*(2*16+1))]

/*; main convolution loop: sum1L = samples 1, 2, 3, ... 15 sum2L = samples 31, 30, ... 17 */
sub COEF, COEF, #(4*(264-16)) /* ; coef = coefBase + 16 (was coefBase + 264 after MC1S block) */
sub VB1, VB1, #(4*(1024-64)) /* ; vb1 = vbuf + 64 (was vbuf + 64*16 after MC1S block) */
mov I, #15 /* ; loop counter, count down */
add PCM, PCM, #(2*2) /* ; pcm+=2 */

LoopPS:
str I, [sp, #0] /* ; sp[0] = i (loop counter) */
str PCM, [sp, #4] /* ; sp[1] = pcm (pointer to pcm buffer) */

mov SUM1LL, #RNDVAL /* ; load rndVal (low 32) */
mov SUM1RL, #RNDVAL /* ; load rndVal (low 32) */
mov SUM2LL, #RNDVAL /* ; load rndVal (low 32) */
mov SUM2RL, #RNDVAL /* ; load rndVal (low 32) */

mov SUM1LH, #0
mov SUM1RH, #0
mov SUM2LH, #0
mov SUM2RH, #0

MC2S 0
MC2S 1
MC2S 2
MC2S 3
MC2S 4
MC2S 5
MC2S 6
MC2S 7

add VB1, VB1, #(4*64) /*; vb1 += 64*/

ldr PCM, [sp, #4] /*; load pcm pointer*/
mov MAXPOS, #0x7f00
orr MAXPOS, MAXPOS, #0xff

C64TOS SUM1LL, SUM1LH, SIGN, MAXPOS
C64TOS SUM1RL, SUM1RH, SIGN, MAXPOS
C64TOS SUM2LL, SUM2LH, SIGN, MAXPOS
C64TOS SUM2RL, SUM2RH, SIGN, MAXPOS

ldr I, [sp, #0] /*; load loop counter*/
add CF2, PCM, I, lsl #3 /*; CF2 = PCM + 4*i (short offset)*/
strh SUM2LL, [CF2], #2 /*; *(pcm + 2*2*i + 0)*/
strh SUM2RL, [CF2], #2 /*; *(pcm + 2*2*i + 1)*/

strh SUM1LL, [PCM], #2 /*; *(pcm + 0)*/
strh SUM1RL, [PCM], #2 /*; *(pcm + 1)*/

subs I, I, #1
bne LoopPS

/* ; restore stack pointer*/
add sp, sp, #8

ldmfd sp!, {r4-r11, pc}
/* ENDFUNC*/















/*;; MONO PROCESSING*/

/*; MC0M - process 2 taps, 1 sample (sample 0)*/
/*; x = vb1 offset*/
MC0M MACRO val_x
ldr CF1, [COEF], #4
ldr CF2, [COEF], #4
ldr VLO, [VB1, #(4*(val_x))]
ldr VHI, [VB1, #(4*(23 - val_x))]

rsb CF2, CF2, #0
smlal SUM1LL, SUM1LH, VLO, CF1
smlal SUM1LL, SUM1LH, VHI, CF2
ENDM ;MC0M


/*; MC1M - process 2 taps, 1 sample (sample 16)*/
/*; x = vb1 offset*/
MC1M MACRO val_x
ldr CF1, [COEF], #4
ldr VLO, [VB1, #(4*(val_x))]
smlal SUM1LL, SUM1LH, VLO, CF1
ENDM ;MC1M


/* ; MC2M - process 2 taps, 2 samples*/
/* ; x = vb1 offset*/
MC2M MACRO val_x
/* ; load data as far as possible in advance of using it*/
ldr CF1, [COEF], #4
ldr CF2, [COEF], #4
ldr VLO, [VB1, #(4*(val_x))]
ldr VHI, [VB1, #(4*(23 - val_x))]

smlal SUM1LL, SUM1LH, VLO, CF1
smlal SUM2LL, SUM2LH, VLO, CF2
rsb CF2, CF2, #0
smlal SUM1LL, SUM1LH, VHI, CF2
smlal SUM2LL, SUM2LH, VHI, CF1
ENDM ;MC2M



/*; void PolyphaseMono(short *pcm, int *vbuf, const int *coefBase)*/
SECTION .text:CODE:NOROOT(2)
PUBLIC xmp3_PolyphaseMono
THUMB
xmp3_PolyphaseMono:

stmfd sp!, {r4-r11, r14}

/*; clear out stack space for 4 local variables (4 bytes each)*/
sub sp, sp, #8
str PCM, [sp, #4] /*; sp[1] = pcm pointer*/

/* ; special case, output sample 0*/
mov SUM1LL, #RNDVAL /* ; load rndVal (low 32)*/
mov SUM1LH, #0

MC0M 0
MC0M 1
MC0M 2
MC0M 3
MC0M 4
MC0M 5
MC0M 6
MC0M 7

ldr PCM, [sp, #4] /*; load pcm pointer*/
mov MAXPOS, #0x7f00
orr MAXPOS, MAXPOS, #0xff

C64TOS SUM1LL, SUM1LH, SIGN, MAXPOS
strh SUM1LL, [PCM, #(2*0)]

/* ; special case, output sample 16*/
add COEF, COEF, #(4*(256-16)) /* ; coef = coefBase + 256 (was coefBase + 16 after MC0M block)*/
add VB1, VB1, #(4*1024) /* ; vb1 = vbuf + 64*16*/

mov SUM1LL, #RNDVAL /* ; load rndVal (low 32)*/
mov SUM1LH, #0

MC1M 0
MC1M 1
MC1M 2
MC1M 3
MC1M 4
MC1M 5
MC1M 6
MC1M 7

ldr PCM, [sp, #4] /* ; load pcm pointer*/
mov MAXPOS, #0x7f00
orr MAXPOS, MAXPOS, #0xff

C64TOS SUM1LL, SUM1LH, SIGN, MAXPOS

strh SUM1LL, [PCM, #(2*16)]

/* ; main convolution loop: sum1L = samples 1, 2, 3, ... 15 sum2L = samples 31, 30, ... 17*/
sub COEF, COEF, #(4*(264-16)) /* ; coef = coefBase + 16 (was coefBase + 264 after MC1M block)*/
sub VB1, VB1, #(4*(1024-64)) /* ; vb1 = vbuf + 64 (was vbuf + 64*16 after MC1M block)*/
mov I, #15 /* ; loop counter, count down*/
add PCM, PCM, #(2) /* ; pcm++*/

LoopPM:
str I, [sp, #0] /* ; sp[0] = i (loop counter)*/
str PCM, [sp, #4] /* ; sp[1] = pcm (pointer to pcm buffer)*/

mov SUM1LL, #RNDVAL /* ; load rndVal (low 32)*/
mov SUM2LL, #RNDVAL /* ; load rndVal (low 32)*/
mov SUM1LH, #0
mov SUM2LH, #0

MC2M 0
MC2M 1
MC2M 2
MC2M 3
MC2M 4
MC2M 5
MC2M 6
MC2M 7

add VB1, VB1, #(4*64) /*; vb1 += 64*/

ldr PCM, [sp, #4] /*; load pcm pointer*/
mov MAXPOS, #0x7f00
orr MAXPOS, MAXPOS, #0xff

C64TOS SUM1LL, SUM1LH, SIGN, MAXPOS
C64TOS SUM2LL, SUM2LH, SIGN, MAXPOS

ldr I, [sp, #0] /*; load loop counter*/
add CF2, PCM, I, lsl #2 /*; CF2 = PCM + 2*i (short offset)*/
strh SUM2LL, [CF2], #2 ; /**(pcm + 2*i + 0)*/
strh SUM1LL, [PCM], #2 ; /**(pcm + 0) ; pcm++*/

subs I, I, #1
bne LoopPM
/* ; restore stack pointer*/
add sp, sp, #8
ldmfd sp!, {r4-r11, pc}


END

for (ch = 0; ch < mp3DecInfo->nChans; ch++)
{
// unpack scale factors and compute size of scale factor block
prevBitOffset = bitOffset;
offset = UnpackScaleFactors(mp3DecInfo, mainPtr, &bitOffset, mainBits, gr, ch);

sfBlockBits = 8*offset - prevBitOffset + bitOffset;
huffBlockBits = mp3DecInfo->part23Length[gr][ch] - sfBlockBits;
mainPtr += offset;
mainBits -= sfBlockBits;

if (offset < 0 || mainBits < huffBlockBits)
{
MP3ClearBadFrame(mp3DecInfo, outbuf);
return ERR_MP3_INVALID_SCALEFACT;
}
// decode Huffman code words
prevBitOffset = bitOffset;
offset = DecodeHuffman(mp3DecInfo, mainPtr, &bitOffset, huffBlockBits, gr, ch);
if (offset < 0)
{
MP3ClearBadFrame(mp3DecInfo, outbuf);
return ERR_MP3_INVALID_HUFFCODES;
}
mainPtr += offset;
mainBits -= (8*offset - prevBitOffset + bitOffset);
}

        if (Dequantize(mp3DecInfo, gr) < 0)
        {
            MP3ClearBadFrame(mp3DecInfo, outbuf);
            return ERR_MP3_INVALID_DEQUANTIZE;            
        }

        for (ch = 0; ch < mp3DecInfo->nChans; ch++)
        {
            if (IMDCT(mp3DecInfo, gr, ch) < 0)
            {
                MP3ClearBadFrame(mp3DecInfo, outbuf);
                return ERR_MP3_INVALID_IMDCT;            
            }
        }

        if (Subband(mp3DecInfo, outbuf + gr*mp3DecInfo->nGranSamps*mp3DecInfo->nChans) < 0)
        {
            MP3ClearBadFrame(mp3DecInfo, outbuf);
            return ERR_MP3_INVALID_SUBBAND;            
        }

static inline int MULSHIFT32(int x, int y)
{
return (((long long)x)*((long long)y))>>32;
}

static inline int FASTABS(int x)
{
int sign;

sign = x >> (sizeof(int) * 8 - 1);
x ^= sign;
x -= sign;

return x;
}

static inline int CLZ(int x)
{
int numZeros;

if (!x)
return (sizeof(int) * 8);

numZeros = 0;
while (!(x & 0x80000000)) {
numZeros++;
x <<= 1;
}

return numZeros;
}

static inline Word64 MADD64(Word64 sum, int x, int y)
{
return (sum + ((Word64)x * y));
}

static inline Word64 SAR64(Word64 sum, int x)
{
return (sum >> x);
}