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

#include "mwmr_channel.h"
#include "giet_config.h"
#include "stdio.h"
#include "stdint.h"
#include "user_lock.h"

// macro to use a shared TTY
#define PRINTF(...)    lock_acquire( &tty_lock ); \
                       giet_tty_printf(__VA_ARGS__);  \
                       lock_release( &tty_lock );
extern user_lock_t tty_lock;

//////////////////////////////////////
void mwmr_init( mwmr_channel_t*  mwmr,
                uint32_t*        buffer,     // buffer base address
                uint32_t         width,      // number of words per item
                uint32_t         nitems )    // max number of items
{

#if GIET_DEBUG_USER_MWMR
uint32_t    x;
uint32_t    y;
uint32_t    lpid;
giet_proc_xyp( &x, &y, &lpid );
giet_tty_printf("\n[DEBUG MWMR] P[%d,%d,%d] initialises mwmr channel %x"
                "\n buffer = %x / width = %d / nitems = %d\n",
                x, y, lpid, (uint32_t)mwmr, (uint32_t)buffer, width, nitems );
#endif

    mwmr->ptw   = 0;
    mwmr->ptr   = 0;
    mwmr->sts   = 0;
    mwmr->width = width;
    mwmr->depth = width * nitems;
    mwmr->data  = buffer;

    lock_init( &mwmr->lock );
}


///////////////////////////////////////
void mwmr_dump( mwmr_channel_t*  mwmr )
{
    // get the lock
    lock_acquire( &mwmr->lock );

    giet_tty_printf("\n[DEBUG MWMR] &fifo = %x / width = %d / depth = %d"
                    "\n             sts = %d / ptr = %d / ptw = %d\n",
                    (uint32_t)mwmr, mwmr->width, mwmr->depth,
                     mwmr->sts, mwmr->ptr, mwmr->ptw );
    uint32_t line, word, value;
    for ( line = 0 ; line < ((mwmr->depth)>>2) ; line++ )
    {
        giet_tty_printf(" line %d :  ", line );
        for ( word = 0 ; word < 4 ; word++ )
        {
            value = mwmr->data[line*4+word];
            giet_tty_printf(" %x %x %x %x",
                            (value    ) & 0xFF,
                            (value>>8 ) & 0xFF,
                            (value>>16) & 0xFF,
                            (value>>24) & 0xFF );
        }
        giet_tty_printf("\n");
    }

    // release the lock
    lock_release( &mwmr->lock );

}


//////////////////////////////////////////////
uint32_t nb_mwmr_write( mwmr_channel_t*  mwmr, 
                        uint32_t*        buffer, 
                        uint32_t         items)
{

#if GIET_DEBUG_USER_MWMR
uint32_t    x;
uint32_t    y;
uint32_t    lpid;
giet_proc_xyp( &x, &y, &lpid );
#endif

    uint32_t n;
    uint32_t spaces; // number of empty slots (in words)
    uint32_t nwords; // requested transfer length (in words)
    uint32_t depth;  // channel depth (in words)
    uint32_t width;  // channel width (in words)
    uint32_t sts;    // channel sts
    uint32_t ptw;    // channel ptw

    if (items == 0) return 0;

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

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

    if (spaces >= nwords) // transfer items, release lock and return 
    { 
        for (n = 0; n < nwords; n++) 
        {
            mwmr->data[ptw] = buffer[n];
            if ((ptw + 1) == depth)  ptw = 0; 
            else                     ptw = ptw + 1;
        }
        mwmr->sts = mwmr->sts + nwords;
        mwmr->ptw = ptw;

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] P[%d,%d,%d] nb_mwmr_write() for %d words"
                "\n %d words written / fifo %x / sts = %d\n",
                x, y, lpid, items*width , nwords, (uint32_t)mwmr, mwmr->sts );
#endif

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

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] P[%d,%d,%d] nb_mwmr_write() for %d words"
                "\n %d words written / fifo %x / sts = %d\n",
                x, y, lpid, items*width , nwords, (uint32_t)mwmr, mwmr->sts );
#endif

        lock_release( &mwmr->lock );
        return (nwords / width);
    }
} // end nb_mwmr_write()



//////////////////////////////////////////////////
uint32_t nb_mwmr_read( mwmr_channel_t*  mwmr, 
                       uint32_t*        buffer,
                       uint32_t         items) 
{

#if GIET_DEBUG_USER_MWMR
uint32_t    x;
uint32_t    y;
uint32_t    lpid;
giet_proc_xyp( &x, &y, &lpid );
#endif

    uint32_t n;
    uint32_t nwords; // requested transfer length (words)
    uint32_t depth;  // channel depth (words)
    uint32_t width;  // channel width (words)
    uint32_t sts;    // channel sts   (words)
    uint32_t ptr;    // channel ptr   (words)

    if (items == 0) return 0;

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

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

    if (sts >= nwords) // transfer items, release lock and return 
    {
        for (n = 0; n < nwords; n++) 
        {
            buffer[n] = mwmr->data[ptr];
            if ((ptr + 1) == depth)  ptr = 0;
            else                     ptr = ptr + 1;
        }
        mwmr->sts = mwmr->sts - nwords;
        mwmr->ptr = ptr;

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] P[%d,%d,%d] nb_mwmr_read() for %d words"
                "\n %d words read / fifo %x / sts = %d\n",
                x, y, lpid, items*width , nwords, (uint32_t)mwmr, mwmr->sts );
#endif

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

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] P[%d,%d,%d] nb_mwmr_read() for %d words"
                "\n %d words read / fifo %x / sts = %d\n",
                x, y, lpid, items*width , nwords, (uint32_t)mwmr, mwmr->sts );
#endif

        lock_release( &mwmr->lock );
        return (nwords / width);
    }
} // nb_mwmr_read()



////////////////////////////////////////
void mwmr_write( mwmr_channel_t*  mwmr, 
                 uint32_t *       buffer, 
                 uint32_t         items ) 
{

#if GIET_DEBUG_USER_MWMR
uint32_t    x;
uint32_t    y;
uint32_t    lpid;
giet_proc_xyp( &x, &y, &lpid );
#endif

    uint32_t          n;
    uint32_t          spaces; // number of empty slots (in words)
    uint32_t          nwords; // requested transfer length (in words)
    uint32_t          depth;  // channel depth (in words)
    uint32_t          width;  // channel width (in words)
    volatile uint32_t sts;    // channel status
    volatile uint32_t ptw;    // channel ptw

    if (items == 0)  return;

    while (1) 
    {
        // get the lock
        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 * items;

        if (spaces >= nwords) // write nwords, release lock and return
        {
            for (n = 0; n < nwords; n++) 
            {
                mwmr->data[ptw] = buffer[n];
                if ((ptw + 1) == depth)  ptw = 0; 
                else                     ptw = ptw + 1;
            }
            mwmr->ptw = ptw;
            mwmr->sts = sts + nwords;

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] P[%d,%d,%d] mwmr_write() for %d words"
                "\n %d words written / fifo %x / sts = %d\n",
                x, y, lpid, items*width , nwords, (uint32_t)mwmr, mwmr->sts );
#endif

            lock_release( &mwmr->lock );

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

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] P[%d,%d,%d] mwmr_write() for %d words"
                "\n %d words written / fifo %x / sts = %d\n",
                x, y, lpid, items*width , nwords, (uint32_t)mwmr, mwmr->sts );
#endif

            lock_release( &mwmr->lock );
        }

        // deschedule before retry
        giet_pthread_yield();
    }
} // end mwmr_write()


//////////////////////////////////////
void mwmr_read( mwmr_channel_t*  mwmr, 
                uint32_t*        buffer, 
                uint32_t         items) 
{

#if GIET_DEBUG_USER_MWMR
uint32_t    x;
uint32_t    y;
uint32_t    lpid;
giet_proc_xyp( &x, &y, &lpid );
#endif

    uint32_t n;
    uint32_t          nwords; // requested transfer length (in words)
    uint32_t          depth;  // channel depth (in words)
    uint32_t          width;  // channel width (in words)
    volatile uint32_t sts;    // channel status
    volatile uint32_t ptr;    // channel ptr

    if (items == 0) return;

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

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

        if (sts >= nwords) // read nwords, release lock and return
        {
            for (n = 0; n < nwords; n++) 
            {
                buffer[n] = mwmr->data[ptr];
                if ((ptr + 1) == depth)  ptr = 0;
                else                     ptr = ptr + 1;
            }
            mwmr->sts = mwmr->sts - nwords;
            mwmr->ptr = ptr;

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] P[%d,%d,%d] mwmr_read() for %d words"
                "\n %d words read / fifo %x / sts = %d\n",
                x, y, lpid, items*width , nwords, (uint32_t)mwmr, mwmr->sts );
#endif

            lock_release( &mwmr->lock );

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

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] P[%d,%d,%d] mwmr_read() for %d words"
                "\n %d words read / fifo %x / sts = %d\n",
                x, y, lpid, items*width , nwords, (uint32_t)mwmr, mwmr->sts );
#endif

            lock_release( &mwmr->lock );
        }

        // deschedule before retry
        giet_pthread_yield();
    }
} // end mwmr_read() 








/////////////////////////////////////////////
void mwmr_bufio_init( mwmr_bufio_t*    bufio,       
                      uint8_t*         buffer, 
                      uint32_t         size,       // number of bytes
                      uint32_t         is_input,
                      mwmr_channel_t*  mwmr )
{
    uint32_t bytes_per_item = (mwmr->width)<<2;

    giet_pthread_assert( ((size % bytes_per_item) == 0) ,
    "ERROR in mwmr_bufio_init() : BUFIO size must be multiple of MWMR item size\n"); 

    bufio->mwmr     = mwmr;
    bufio->is_input = is_input;
    bufio->base     = buffer;
    bufio->ptr      = 0;
    bufio->max      = 0;
    bufio->nitems   = size / bytes_per_item; 
    bufio->nbytes   = size;
}  // end mwmr_bufio_init()

////////////////////////////////////////////
void mwmr_bufio_dump( mwmr_bufio_t*  bufio )
{
    giet_tty_printf("\n[DEBUG MWMR] &bufio = %x / &mwmr = %x / &buffer = %x"
                    "\n             is_input = %d / nbytes = %d / ptr = %d / max = %d\n",
                    bufio , bufio->mwmr , bufio->base,
                    bufio->is_input , bufio->nbytes , bufio->ptr , bufio->max );
    uint32_t i = 0;
    while ( i < bufio->nbytes )
    {
        giet_tty_printf(" %x", bufio->base[i] );
        if ( (i & 0xF) == 0xF ) giet_tty_printf("\n");
        i++;
    }
    giet_tty_printf("\n");
}  // end mwmr_bufio_dump()
    
////////////////////////////////////////////////////
uint8_t mwmr_bufio_read_byte( mwmr_bufio_t*  bufio )
{
    giet_pthread_assert( ( bufio->is_input ) ,
    "ERROR in mwmr_bufio_read_byte() : bufio not input\n"); 

    uint8_t ret;

    if ( bufio->ptr == 0 )  // refill
    {
        uint32_t items;
        do
        {
            items = nb_mwmr_read( bufio->mwmr , (uint32_t*)bufio->base , bufio->nitems );

            // deschedule if channel empty
            if (items == 0 ) giet_pthread_yield();
        } 
        while ( items == 0 );
        bufio->max = items * ((bufio->mwmr->width)<<2);

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] mwmr_bufio_read_byte() read %d bytes from mwmr\n",
                bufio->max );
uint32_t i = 0;
while ( i < bufio->max )
{
    giet_tty_printf(" %x", bufio->base[i] );
    if ( (i & 0xF) == 0xF ) giet_tty_printf("\n");
    i++;
}
giet_tty_printf("\n");
#endif 

    }

    ret = bufio->base[bufio->ptr];
    bufio->ptr++;
    if ( bufio->ptr == bufio->max ) bufio->ptr = 0;
    return ret;

}  // end mwmr_bufio_read_byte()

///////////////////////////////////////////
void mwmr_bufio_skip( mwmr_bufio_t*  bufio,
                      uint32_t       length )
{
    giet_pthread_assert( ( bufio->is_input ) ,
    "ERROR in mwmr_bufio_skip() : bufio not input\n"); 

    while ( length )
    {
        if ( bufio->ptr == 0 )  // refill
        {
            uint32_t items;
            do
            {
                items = nb_mwmr_read( bufio->mwmr , (uint32_t*)bufio->base , bufio->nitems );

                // deschedule if channel empty
                if (items == 0 ) giet_pthread_yield();
            } 
            while ( items == 0 );
            bufio->max = items * ((bufio->mwmr->width)<<2);

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] mwmr_bufio_skip() read %d bytes from mwmr\n",
                bufio->max );
uint32_t i = 0;
while ( i < bufio->max )
{
    giet_tty_printf(" %x", bufio->base[i] );
    if ( (i & 0xF) == 0xF ) giet_tty_printf("\n");
    i++;
}
giet_tty_printf("\n");
#endif 

        }

        bufio->ptr++;
        if ( bufio->ptr == bufio->max ) bufio->ptr = 0;
        length--;
    }
}  // end mwmr_bufio_skip()
                     
/////////////////////////////////////////////
void mwmr_bufio_align( mwmr_bufio_t*  bufio )
{
    giet_pthread_assert( ( bufio->is_input ) ,
    "ERROR in mwmr_bufio_align() : bufio not input\n");

    uint32_t bytes_per_item = (bufio->mwmr->width)<<2;
    uint32_t offset = bufio->ptr % bytes_per_item;

    // align ptr on next item boundary if required 
    if ( offset )
    {
        bufio->ptr = bufio->ptr + bytes_per_item - offset;
        if ( bufio-> ptr == bufio->max ) bufio->ptr = 0;
    }
}  // end mwmr_bufio_align()


/////////////////////////////////////////////////
void mwmr_bufio_write_byte( mwmr_bufio_t*  bufio,
                            uint8_t        value )
{
    giet_pthread_assert( ( !bufio->is_input ) ,
    "ERROR in mwmr_bufio_write_byte() : bufio not output\n"); 

    bufio->base[bufio->ptr] = value;

    bufio->ptr++;
    if ( bufio->ptr == bufio->nbytes )  // flush bufio
    {
        // move data to mwmr channel
        mwmr_write( bufio->mwmr , (uint32_t*)bufio->base , bufio->nitems );

        // reinitialise bufio
        bufio->ptr = 0;

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] mwmr_bufio_write_byte() write %d bytes to mwmr\n", 
                bufio->nbytes );
uint32_t i = 0;
while ( i < bufio->nbytes )
{
    giet_tty_printf(" %x", bufio->base[i] );
    if ( (i & 0xF) == 0xF ) giet_tty_printf("\n");
    i++;
}
giet_tty_printf("\n");
#endif 
    }
}  // end mwmr_bufio_write_byte()

/////////////////////////////////////////////
void mwmr_bufio_flush( mwmr_bufio_t*  bufio )
{
    giet_pthread_assert( ( !bufio->is_input ) ,
    "ERROR in mwmr_bufio_flush() : bufio not output\n"); 

    uint32_t i;

    uint32_t bytes_per_item = (bufio->mwmr->width)<<2;

    // do nothing if bufio empty
    if ( bufio->ptr == 0 ) return;
    
    // compute number of items and extra bytes to be moved to MWMR channel
    uint32_t nitems = bufio->ptr / bytes_per_item;
    uint32_t offset = bufio->ptr % bytes_per_item; 

    // completes last item with 0 if required
    if ( offset )
    { 
        for( i = bufio->ptr;
             i < bufio->ptr + bytes_per_item - offset ; 
             i++ ) bufio->base[i] = 0;

        nitems++;
    }

#if GIET_DEBUG_USER_MWMR
giet_tty_printf("\n[DEBUG MWMR] mwmr_bufio_flush() write %d bytes to mwmr\n", 
                nitems * bytes_per_item );
uint32_t j = 0;
while ( j < (nitems * bytes_per_item) )
{
    giet_tty_printf(" %x", bufio->base[j] );
    if ( (j & 0xF) == 0xF ) giet_tty_printf("\n");
    j++;
}
giet_tty_printf("\n");
#endif 

    // move nitems to mwmr channel
    mwmr_write( bufio->mwmr , (uint32_t*)bufio->base , nitems );

    // reinitialise bufio
    bufio->ptr = 0;

}  // end mwmr_bufio_flush()


    
// 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