#define FUNCTION_NAME ds_stereo

/*
void ds_stereo(
    int32_t final_shr, 
    int32_t * coefs, 
    int32_t * data, 
    chanend_t c_in,
    chanend_t c_out,
    int32_t num_phase);
*/

    #define NSTACKWORDS 32

    #define S_LEFT 0
    #define S_RIGHT 1
    #define S_LOOP_COUNT 2
    #define S_COEF_BASE_POINTER 3

    .cc_top FUNCTION_NAME.function
    .type   FUNCTION_NAME,@function

    .issue_mode dual
    .align 4
#define ONE 0x40000000
zeros:
    .word 0, 0, 0, 0, 0, 0, 0, 0
left_chan:
    .word 0, 0, 0, 0, ONE, ONE, ONE, ONE
right_chan:
    .word ONE, ONE, ONE, ONE, 0, 0, 0, 0
    .align 16
    .globl FUNCTION_NAME
.globl FUNCTION_NAME.nstackwords
.set   FUNCTION_NAME.nstackwords, 32
FUNCTION_NAME:
    { dualentsp NSTACKWORDS;}

    stw r4, sp[20]
    stw r5, sp[21]
    stw r6, sp[22]
    stw r7, sp[23]
    stw r8, sp[24]
    stw r9, sp[25]
    stw r10, sp[26]

    ldw r4, sp[NSTACKWORDS+1]
    stw r4, sp[27]


    //{mov r0, r0; mov r1, r1}
    //{mov r2, r2; mov r3, r3}

    #define CHANNELS 2
    #define MACCRS_PER_PHASE 4
    #define BYTES_PER_WORD 4
    #define WORDS_PER_VECTOR 8
    #define BYTES_PER_VECTOR (WORDS_PER_VECTOR*BYTES_PER_WORD)
    
    ldc r10, (BYTES_PER_VECTOR+BYTES_PER_WORD)
    ldc r9, BYTES_PER_VECTOR
    ldc r8, (2*MACCRS_PER_PHASE-1)*BYTES_PER_VECTOR - 4 + (BYTES_PER_VECTOR+BYTES_PER_WORD) + 4
    ldc r7, MACCRS_PER_PHASE*BYTES_PER_VECTOR
    ldc r6, ((1<<(BYTES_PER_WORD*CHANNELS))-1)

    //load number of phases - 1 to r11
    ldw r11, sp[NSTACKWORDS+2]
    sub r11, r11, 1
    // jump to passthrough loop if num of phases is 1
    bf r11, passthrough_loop
    std r1, r11, sp[1]
    //store the final_shr for the VLSAT 
    std r0, r0, sp[2]
    std r0, r0, sp[3]
    std r0, r0, sp[4]
    std r0, r0, sp[5]

   
    ldc r11, (MACCRS_PER_PHASE-1)*BYTES_PER_VECTOR - 4 + 4 //the final +4 is for the padding at the origin
    {add r2, r2, r11; nop }

outer_loop:
    ldc r11, (MACCRS_PER_PHASE-1)*BYTES_PER_VECTOR
    {vclrdr ; add r11, r2, 0}         //clear accu and reset data pointer
    ldd r1, r4, sp[1]                 // set loop counter and coef base pointer

phase_loop:

    //{nop; nop}
    //{nop; nop}
    //{nop; nop}

    //MACCR 1 left
    {vldc r11[0]; add r11, r11, BYTES_PER_WORD}
    {vlmaccr r1[0]; add r1, r1, r9} 
    {vstc r11[0]; sub r11, r11, r10}

    //MACCR 2 left
    {vldc r11[0]; add r11, r11, BYTES_PER_WORD}
    {vlmaccr r1[0]; add r1, r1, r9} 
    {vstc r11[0]; sub r11, r11, r10}

    //MACCR 3 left
    {vldc r11[0]; add r11, r11, BYTES_PER_WORD}
    {vlmaccr r1[0]; add r1, r1, r9} 
    {vstc r11[0]; sub r11, r11, r10}

    {testct r0, res[r3]; nop}
    {bt r0, exit; nop}
    {in r0, res[r3] ; nop }               //fetch data from channel, then get the other one too
    {stw r0, r11[0] ; in r5, res[r3] }    //store to start of phase

    //MACCR 4 left
    {vldc r11[0]; add r11, r11, BYTES_PER_WORD}
    {vlmaccr r1[0]; add r1, r1, r9} 
    {vstc r11[0]; sub r11, r11, r10}


    //rewind r1 by MACCRS_PER_PHASE * BYTES_PER_VECTOR
    {sub r1, r1, r7; add r11, r11, r8 }

    //now do the right channel
    {vldc r11[0]; add r11, r11, BYTES_PER_WORD}
    {vlmaccr r1[0]; add r1, r1, r9} 
    {vstc r11[0]; sub r11, r11, r10} //r10 = 32 + 1

    //MACCR 2 right
    {vldc r11[0]; add r11, r11, BYTES_PER_WORD}
    {vlmaccr r1[0]; add r1, r1, r9} 
    {vstc r11[0]; sub r11, r11, r10}

    //MACCR 3 right
    {vldc r11[0]; add r11, r11, BYTES_PER_WORD}
    {vlmaccr r1[0]; add r1, r1, r9} 
    {vstc r11[0]; sub r11, r11, r10}

    {stw r5, r11[0] ; nop }                //store to start of phase

    //MACCR 4 right
    {vldc r11[0]; add r11, r11, BYTES_PER_WORD}
    {vlmaccr r1[0]; add r1, r1, r9} 
    {vstc r11[0]; sub r11, r11, r10}

    {nop;       add r11, r11, r8 }             //advance to next phase

    {bt r4, phase_loop; sub r4, r4, 1}
 
    ldaw r11, sp[4]
    {vlsat r11[0]; nop}  //turn all accumulators into 32 bits

    ldaw r11, sp[12]
    {vstr r11[0]; nop}
    {vldc r11[0];ldap r11, right_chan}
    {vclrdr; nop}
    {vlmaccr r11[0];ldap r11, left_chan}
    {vlmaccr r11[0]; ldap r11, zeros}

       
    {vlsat r11[0]; ldaw r11, sp[0]}
    vstrpv r11[0], r6

    ldd r11, r4, sp[0]
    // load c_out from stack to r1
    ldw r1, sp[27]
    {out res[r1], r4; nop}
    {bu outer_loop;out res[r1], r11}

passthrough_loop:
    {testct r11, res[r3]; nop}
    {bt r11, exit; nop}
    {in r11, res[r3]; nop}
    {out res[r4], r11; nop}
    {in r11, res[r3]; nop}
    {bu passthrough_loop; out res[r4], r11}

exit:
    {inct r0, res[r3]; nop}
    ldw r4, sp[20]
    ldw r5, sp[21]
    ldw r6, sp[22]
    ldw r7, sp[23]
    ldw r8, sp[24]
    ldw r9, sp[25]
    ldw r10, sp[26]
    retsp NSTACKWORDS

    .cc_bottom FUNCTION_NAME.function
