//////////////////////////////////////////////////////////////////////////////////
// File     : mwmr.c         
// Date     : 01/04/2012
// Author   : alain greiner
// Copyright (c) UPMC-LIP6
///////////////////////////////////////////////////////////////////////////////////
// The mwmr.c and mwmr.h files are part of the GIET nano-kernel.
// This  middlewre implements a user level Multi-Writers / Multi-Readers
// communication channel, that can be sued by parallel multi-tasks applications
// respecting the TCG (Tasks and Communications Graph) formalism.
// 
// The mwmr_read() and mwmr_write() functions do not require a system call,
//
// The mwmr_get_base() returns the virtual base address of the MWMR channel. 
// As this function requires a system call, it is defined in the stdio.c and
// stdio.h files.
//
// ALL MWMR channels must be defined in the mapping_info data structure, 
// to be initialised by the GIET in the boot phase.
// The max number of words that can be stored in a MWMR channel is defined by the
// depth parameter, but all MWMR channels are implemented as fixed size buffers,
// and the size is defined by the GIET_MWMR_SIZE_MAX configuration parameter.
// An MWMR transaction is an integer number of unsigned int (32 bits mwords).
// Both the mwmr_read() and mwmr_write() functions are blocking functions. 
// A private lock provides exclusive access to the MWMR channel, that can have
// a variable number of producers and a variable number of consumers.
///////////////////////////////////////////////////////////////////////////////////

#include <mwmr.h>
#include <stdio.h>

#define MWMR_PART
//#define MWMR_DEBUG_READ
//#define MWMR_DEBUG_WRITE

///////////////////////////////////////////////////////////////////////////////////
//	mwmr_lock_acquire()
// We use a fixed delay befor retry, in order to avoid the system call
// associated to the rand() function.
// This is probably a weakness, that can be improved...
///////////////////////////////////////////////////////////////////////////////////
inline void mwmr_lock_acquire(unsigned int* lock)
{
    asm volatile (
            "lock_try:              \n"
            "ll   $2,    0(%0)      \n" /* $2 <= lock current value */
            "bnez $2,    lock_delay \n" /* retry if lock already taken */
            "li   $3,    1          \n" /* $3 <= argument for sc */
            "sc   $3,    0(%0)      \n" /* try to get lock */
            "bnez $3,    lock_ok    \n" /* exit if atomic */
            "lock_delay:            \n"
            "li   $4,    100        \n" /* $4 <= delay */
            "lock_loop:             \n"
            "addi $4,    $4,  -1    \n" /* $4 <= $4 - 1 */
            "beqz $4,    lock_loop  \n" /* test end delay */
            "nop                    \n"
            "j           lock_try   \n" /* retry if not atomic */
            "nop                    \n"
            "lock_ok:               \n"
            :
            :"r"(lock)
            :"$2", "$3", "$4");
} 

inline void print_state( mwmr_channel_t* mwmr)
{

    tty_printf("PTR %d", mwmr->ptr);
    tty_printf("/PTW %d", mwmr->ptw);
    tty_printf("/STS %d", mwmr->sts);
    tty_printf("/DEPTH %d", mwmr->depth);
    tty_printf("/LOCK %d\n", mwmr->lock);

}


//////////////////////////////////////////////////////////////////////////////
//	mwmr_write()
// This blocking function returns only when the transfer is completed.
// It takes the lock for exclusive access before testing the channel state.
// If there is not enough space in mwmr channel to write nwords words,
// it writes as many words as possible, release the lock, and retry after 
// a random delay.
//////////////////////////////////////////////////////////////////////////////
void mwmr_write( mwmr_channel_t*   	mwmr, 
                 unsigned int*		buffer,
                 unsigned int 		nwords )
{
    unsigned int	x;
    unsigned int	spaces;

    while(1)
    {
        // get the lock
        mwmr_lock_acquire((unsigned int*)&mwmr->lock);
#ifdef MWMR_DEBUG_WRITE
tty_printf("write: lock done\n");
print_state(mwmr);
#endif

        // compute number of empty slots
        spaces = mwmr->depth - mwmr->sts;

        if( spaces >= nwords ) 	// write nwords, release lock and return
        {
            for ( x = 0 ; x < nwords ; x++ )  
            {
                mwmr->data[mwmr->ptw] = buffer[x];
                mwmr->ptw  = (mwmr->ptw + 1) % mwmr->depth;
#ifdef MWMR_DEBUG_WRITE
tty_printf("/PTW %d\n", mwmr->ptw);
#endif
            }
            mwmr->sts  = mwmr->sts + nwords;
#ifdef MWMR_DEBUG_WRITE
tty_printf("write done\n");
print_state(mwmr);
#endif
            mwmr->lock = 0;
            return;
        }
        else if ( spaces == 0 )	// release lock and retry after delay
        {
            mwmr->lock = 0;
            for ( x = rand()>>8 ; x > 0 ; x-- ) asm volatile ( "nop" );
        }
        else		// write spaces, release lock and retry after delay
        {
#ifdef MWMR_PART
            for ( x = 0 ; x < spaces ; x++ )  
            {
                mwmr->data[mwmr->ptw] = buffer[x];
                mwmr->ptw  = (mwmr->ptw + 1) % mwmr->depth;
            }
            mwmr->sts  = mwmr->depth;
            nwords     = nwords - spaces;
            buffer     = buffer + spaces;
#ifdef MWMR_DEBUG_WRITE
tty_printf("write part done\n");
print_state(mwmr);
#endif
#endif
            mwmr->lock = 0;
        }

        // random delay before retry
        for ( x = rand()>>6 ; x > 0 ; x-- ) asm volatile ( "nop" );
    }
} 


//////////////////////////////////////////////////////////////////////////////
//	mwmr_read()
// This blocking function returns only when the transfer is completed.
// It takes the lock for exclusive access before testing the channel state.
// If there is not enough data in mwmr channel to read nwords words,
// it reads as many words as possible, release the lock, and retry after 
// a random delay.
//////////////////////////////////////////////////////////////////////////////
void mwmr_read( mwmr_channel_t*    	mwmr, 
                unsigned int*		buffer,
                unsigned int 		nwords )
{
    unsigned int	x;
    unsigned int	dispos;

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

#ifdef MWMR_DEBUG_READ
tty_printf("lock done\n");
print_state(mwmr);
#endif

        // compute number of available words
        dispos = mwmr->sts;

        if( dispos >= nwords ) 	// read nwords, release lock and return
        {
            for ( x = 0 ; x < nwords ; x++ )  
            {
                buffer[x] = mwmr->data[mwmr->ptr];
                mwmr->ptr  = (mwmr->ptr + 1) % mwmr->depth;
#ifdef MWMR_DEBUG_WRITE
tty_printf("PTR %d\n", mwmr->ptr);
#endif
            }
            mwmr->sts  = mwmr->sts - nwords;
#ifdef MWMR_DEBUG_READ
tty_printf("read done\n");
print_state(mwmr);
#endif
            mwmr->lock = 0;
            return;
        }
        else if ( dispos == 0 )	// release lock and retry after delay
        {
            mwmr->lock = 0;
            for ( x = rand()>>8 ; x > 0 ; x-- ) asm volatile ( "nop" );
        }
        else		// read dispos, release lock and retry after delay
        {
#ifdef MWMR_PART
            for ( x = 0 ; x < dispos ; x++ )  
            {
                buffer[x] = mwmr->data[mwmr->ptr];
                mwmr->ptr  = (mwmr->ptr + 1) % mwmr->depth;
            }
            mwmr->sts  = 0;
            nwords     = nwords - dispos;
            buffer     = buffer + dispos;

#ifdef MWMR_DEBUG_READ
tty_printf("read part done\n");
print_state(mwmr);
#endif

#endif
            mwmr->lock = 0;
        }

        // random delay before retry
        for ( x = rand()>>6 ; x > 0 ; x-- ) asm volatile ( "nop" );
    }
}