//////////////////////////////////////////////////////////////////////////////////
// File     : mwmr_channel.c         
// Date     : 01/04/2012
// Author   : alain greiner
// Copyright (c) UPMC-LIP6
///////////////////////////////////////////////////////////////////////////////////

#include <mwmr_channel.h>
#include <stdio.h>

//////////////////////////////////////
void mwmr_init( mwmr_channel_t*  mwmr,
                unsigned int     width,      // numer of words per item
                unsigned int     items )     // max number of items
{
    if ( ((items * width)) > 1018 )
    giet_exit("[MWMR ERROR] in mwmr_init() : buffer size larger than 4072 bytes\n");

    mwmr->ptw   = 0;
    mwmr->ptr   = 0;
    mwmr->sts   = 0;
    mwmr->width = width;
    mwmr->depth = width * items;
    mwmr->lock  = 0;

#if GIET_DEBUG_MWMR
giet_shr_printf("[MWMR DEBUG] Initialise MWMR channel\n"
                " - vbase = %x\n"
                " - width = %d\n"
                " - depth = %d\n",
                (unsigned int)mwmr, width, items );
#endif
}


///////////////////////////////////////////////////
static void mwmr_lock_acquire( unsigned int* lock ) 
{
    register unsigned int*  plock = lock;
    register unsigned int   delay = 100;
    asm volatile (
            "1:                               \n"
            "ll   $2,    0(%0)                \n" /* $2 <= lock current value */
            "bnez $2,    2f                   \n" /* retry after delay if lock busy */
            "li   $3,    1                    \n" /* $3 <= argument for sc */
            "sc   $3,    0(%0)                \n" /* try to get lock */
            "bnez $3,    3f                   \n" /* exit if atomic */
            "2:                               \n"
            "move $4,    %1                   \n" /* $4 <= delay */
            "4:                               \n"
            "beqz $4,    4b                   \n" /* test end delay */
            "addi $4,    $4,  -1              \n" /* $4 <= $4 - 1 */
            "j           1b                   \n" /* retry ll */
            "nop                              \n"
            "3:                               \n"
            :
            :"r"(plock), "r"(delay)
            :"$2", "$3", "$4");
} 


///////////////////////////////////////////////////
unsigned int nb_mwmr_write( mwmr_channel_t * mwmr, 
                            unsigned int * buffer, 
                            unsigned int nitems)
{
    unsigned int x;
    unsigned int spaces; // number of empty slots (in words)
    unsigned int nwords; // requested transfer length (in words)
    unsigned int depth;  // channel depth (in words)
    unsigned int width;  // channel width (in words)
    unsigned int sts;    // channel sts
    unsigned int ptw;    // channel ptw

    if (nitems == 0) return 0;

    // get the lock
    mwmr_lock_acquire(&mwmr->lock);

    // access fifo status
    depth = mwmr->depth;
    width = mwmr->width;
    sts = mwmr->sts;
    ptw = mwmr->ptw;
    spaces = depth - sts;
    nwords = width * nitems;

    if (spaces >= nwords) // transfer nitems, release lock and return 
    { 
        for (x = 0; x < nwords; x++) 
        {
            mwmr->data[ptw] = buffer[x];
            if ((ptw + 1) == depth)  ptw = 0; 
            else                     ptw = ptw + 1;
        }
        mwmr->sts = mwmr->sts + nwords;
        mwmr->ptw = ptw;
        mwmr->lock = 0;
        return nitems;
    }
    else if (spaces < width) // release lock and return 
    {
        mwmr->lock = 0;
        return 0;
    }
    else // transfer as many items as possible, release lock and return 
    {
        nwords = (spaces / width) * width;    // integer number of items
        for (x = 0; x < nwords; x++) 
        {
            mwmr->data[ptw] = buffer[x];
            if ((ptw + 1) == depth) ptw = 0;
            else                    ptw = ptw + 1;
        }
        mwmr->sts = sts + nwords;
        mwmr->ptw = ptw;
        mwmr->lock = 0;
        return (nwords / width);
    }
} // end nb_mwmr_write()



//////////////////////////////////////////////////
unsigned int nb_mwmr_read( mwmr_channel_t * mwmr, 
                           unsigned int *   buffer,
                           unsigned int     nitems) 
{
    unsigned int x;
    unsigned int nwords; // requested transfer length (in words)
    unsigned int depth;  // channel depth (in words)
    unsigned int width;  // channel width (in words)
    unsigned int sts;    // channel sts
    unsigned int ptr;    // channel ptr

    if (nitems == 0) return 0;

    // get the lock
    mwmr_lock_acquire(&mwmr->lock);

    // access fifo status
    depth = mwmr->depth;
    width = mwmr->width;
    sts = mwmr->sts;
    ptr = mwmr->ptr;
    nwords = width * nitems;

    if (sts >= nwords) // transfer nitems, release lock and return 
    {
        for (x = 0; x < nwords; x++) 
        {
            buffer[x] = mwmr->data[ptr];
            if ((ptr + 1) == depth)  ptr = 0;
            else                     ptr = ptr + 1;
        }
        mwmr->sts = mwmr->sts - nwords;
        mwmr->ptr = ptr;
        mwmr->lock = 0;
        return nitems;
    }
    else if (sts < width) // release lock and return 
    {
        mwmr->lock = 0;
        return 0;
    }
    else // transfer as many items as possible, release lock and return 
    {
        nwords = (sts / width) * width; // integer number of items
        for (x = 0 ; x < nwords ; x++) 
        {
            buffer[x] = mwmr->data[ptr];
            if ((ptr + 1) == depth)  ptr = 0;
            else                     ptr = ptr + 1;
        }
        mwmr->sts = sts - nwords;
        mwmr->ptr = ptr;
        mwmr->lock = 0;
        return (nwords / width);
    }
} // nb_mwmr_read()



////////////////////////////////////////
void mwmr_write( mwmr_channel_t * mwmr, 
                 unsigned int *   buffer, 
                 unsigned int     nitems ) 
{
    unsigned int x;
    unsigned int spaces; // number of empty slots (in words)
    unsigned int nwords; // requested transfer length (in words)
    unsigned int depth;  // channel depth (in words)
    unsigned int width;  // channel width (in words)
    unsigned int sts;    // channel sts
    unsigned int ptw;    // channel ptw

    if (nitems == 0)  return;

    while (1) 
    {
        // get the lock
        mwmr_lock_acquire(&mwmr->lock);

        // compute spaces and nwords
        depth = mwmr->depth;
        width = mwmr->width;
        sts  = mwmr->sts;
        ptw  = mwmr->ptw;
        spaces = depth - sts;
        nwords = width * nitems;

        if (spaces >= nwords) // write nwords, release lock and return
        {
            for (x = 0; x < nwords; x++) 
            {
                mwmr->data[ptw] = buffer[x];
                if ((ptw + 1) == depth)  ptw = 0; 
                else                     ptw = ptw + 1;
            }
            mwmr->ptw = ptw;
            mwmr->sts = sts + nwords;
            mwmr->lock = 0;
            return;
        }
        else if (spaces < width) // release lock and deschedule           
        {
            mwmr->lock = 0;
        }
        else // write as many items as possible, release lock and deschedule
        {
            nwords = (spaces / width) * width;  // integer number of items
            for (x = 0; x < nwords; x++) 
            {
                mwmr->data[ptw] = buffer[x];
                if ((ptw + 1) == depth)  ptw = 0; 
                else                     ptw = ptw + 1;
            }
            mwmr->sts = sts + nwords;
            mwmr->ptw = ptw;
            buffer = buffer + nwords;
            nitems = nitems - (nwords/width);
            mwmr->lock = 0;
        }
        giet_context_switch();
    }
} // end mwmr_write()


//////////////////////////////////////
void mwmr_read( mwmr_channel_t * mwmr, 
                unsigned int *   buffer, 
                unsigned int     nitems) 
{
    unsigned int x;
    unsigned int nwords; // requested transfer length (in words)
    unsigned int depth;  // channel depth (in words)
    unsigned int width;  // channel width (in words)
    unsigned int sts;    // channel sts
    unsigned int ptr;    // channel ptr

    if (nitems == 0) return;

    while (1) 
    {
        // get the lock
        mwmr_lock_acquire(&mwmr->lock);

        // compute nwords
        depth = mwmr->depth;
        width = mwmr->width;
        sts = mwmr->sts;
        ptr = mwmr->ptr;
        nwords = width * nitems;

        if (sts >= nwords) // read nwords, release lock and return
        {
            for (x = 0; x < nwords; x++) 
            {
                buffer[x] = mwmr->data[ptr];
                if ((ptr + 1) == depth)  ptr = 0;
                else                     ptr = ptr + 1;
            }
            mwmr->sts = mwmr->sts - nwords;
            mwmr->ptr = ptr;
            mwmr->lock = 0;
            return;
        }
        else if (sts < width) // release lock and deschedule
        {
            mwmr->lock = 0;
        }
        else // read as many items as possible, release lock and deschedule
        {   
            nwords = (sts / width) * width; // integer number of items
            for (x = 0; x < nwords; x++) 
            {
                buffer[x] = mwmr->data[ptr];
                if ((ptr + 1) == depth) ptr = 0;
                else                    ptr = ptr + 1;
            }
            mwmr->sts = sts - nwords;
            mwmr->ptr = ptr;
            buffer = buffer + nwords;
            nitems = nitems - (nwords/width);
            mwmr->lock = 0;
        }
        giet_context_switch();
    }
} // end mwmr_read() 


// Local Variables:
// tab-width: 4
// c-basic-offset: 4
// c-file-offsets:((innamespace . 0)(inline-open . 0))
// indent-tabs-mode: nil
// End:
// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4