source: soft/giet_vm/libs/mwmr_channel.c @ 163

//////////////////////////////////////////////////////////////////////////////////
// File     : mwmr_channel_.c         
// Date     : 01/04/2012
// Author   : alain greiner
// Copyright (c) UPMC-LIP6
///////////////////////////////////////////////////////////////////////////////////
// The mwmr_channel.c and mwmr_channel.h files are part of the GIET nano-kernel.
// This  middleware implements a user level Multi-Writers / Multi-Readers
// communication channel, that can be used 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 channel itself must have been allocated in a non cacheable segment,
// if the platform does not provide hardwate cache coherence.
//
// ALL MWMR channels must be defined in the mapping_info data structure, 
// to be initialised by the GIET in the boot phase.
// The vobj_get_vbase() system call (defined in stdio.c and stdio.h files)
// can be used to get the virtual base address of the channel from it's name.
//
// An MWMR transaction transfer an integer number of items, and an item is
// an integer number of unsigned int (32 bits words).
// The max number of words that can be stored in a MWMR channel is defined by the
// "depth" parameter, and the "width" parameter define the minimal number of
// word contained in an atomic item. Therefore, the "depth" parameter must be
// a multiple of the "width" parameter.
//
// 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_channel.h>
#include <stdio.h>

//////////////////////////////////////////////////////////////////////////////
//  mwmr_lock_aquire()
// This blocking function returns only when the lock has been taken.
// If the lock is already taken a random delay is introduced before retry.
//////////////////////////////////////////////////////////////////////////////
void mwmr_lock_acquire(unsigned int* plock)
{
    asm volatile (
            "mwmr_lock_try:                                     \n"
            "ll   $2,    0(%0)                          \n" /* $2 <= lock current value */
            "bnez $2,    mwmr_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,    mwmr_lock_ok           \n" /* exit if atomic */

            "mwmr_lock_delay:                           \n"
            "jal  rand                                  \n" /* rand() system call */
            "nop                                                        \n"
            "andi $4,   $2,     0xFF                    \n"     /* $4 <= random delay < 256 cycles */

            "mwmr_lock_loop:                            \n"
            "addi $4,    $4,  -1                        \n" /* $4 <= $4 - 1 */
            "beqz $4,    mwmr_lock_loop         \n" /* test end delay */
            "nop                                                        \n"
            "j           mwmr_lock_try          \n" /* retry */
            "nop                                                        \n"
            "mwmr_lock_ok:                                      \n"
            :
            :"r"(plock)
            :"$2", "$3", "$4");
} 

//////////////////////////////////////////////////////////////////////////////
//      mwmr_write()
// This blocking function returns only when the transfer is completed.
// The nitems parameter is the number of items to be transfered.
// The requested transfer is therefore (nitems * width) words.
// It takes the lock for exclusive access before testing the channel state.
// If there is not enough space in mwmr channel to write nitems,
// it writes as many items as possible, releases the lock, and retry 
// after a random delay.
//////////////////////////////////////////////////////////////////////////////
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

    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 retry after delay
        {
            mwmr->lock = 0;
            for ( x = rand()>>8 ; x > 0 ; x-- ) asm volatile ( "nop" );
        }
        else    // write as many items as possible, release lock and retry after delay
        {
            nwords = (spaces/width) * width;  // integer nmber 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;
        }
        // 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.
// The nitems parameter is the number of items to be transfered.
// The requested transfer is therefore (nitems * width) words.
// It takes the lock for exclusive access before testing the channel state.
// If there is not enough data in mwmr channel to read nitems,
// it reads as many items as possible, releases the lock, and retry 
// after a random delay.
//////////////////////////////////////////////////////////////////////////////
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 ptw

    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 retry after delay
        {
            mwmr->lock = 0;
            for ( x = rand()>>8 ; x > 0 ; x-- ) asm volatile ( "nop" );
        }
        else    // read as many items as possible, release lock and retry after delay
        {
            nwords = (sts/width) * width;       // integer number of items
            for ( x = 0 ; x < nwords ; x++ )  
            {
                buffer[x] = mwmr->data[mwmr->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;
        }
        // random delay before retry
        for ( x = rand()>>6 ; x > 0 ; x-- ) asm volatile ( "nop" );
    }
}