source: svn/zas_dstar/hal/os/src/module/lld_sem.c

/****************************************************************************
 * Copyright (c) 2006 DST Technologies Inc.  All Rights Reserved.   
 *
 * Module:      SEMS
 *
 * Description: This is a decent implementation of semaphores under Linux.
 *
 * Notes:   This implementation poses some limitations, some of which can
 *          be changed by adjusting some defines:
 *        - Max number of semaphores, shared and local, in the system is
 *          defined by MAX_NOF_SEMS
 *        - Max number of processes that can share the same semaphore at
 *          any given time is defined by MAX_NOF_PROCS
 *        - When a semaphore is deleted, all tasks within the calling process
 *          that may be pending on the semaphore will be unlocked with an
 *          error code.
 *
 ***************************************************************************/

/*============================
 * Includes
 *===========================*/
#include "lld_os.h"
#include "dsthallocal.h"
#include "lld_local.h"
#include "os.h"

/*============================
 * Defines
 *===========================*/
#define UNUSED_SEM              0xffffffff
#define MUTEX_OBJ               0xfffffff0  /* Inidicate a mutex object     */
#define INVALID_PID             0xffffffff  /* Invalid process ID   */
#define Semaphore(i)            (SemsPtr->_Sems[i])
#define SemaphorePtr(i)         (&(SemsPtr->_Sems[i]))

/*============================
 * Structures
 *===========================*/
typedef struct 
{
    DS_U32  InitCount;                  /* = UNUSED_SEM if not used         */
                                        /* = MUTEX_OBJ for mutexes          */
    DS_U32  Count;                      /* Current count                    */
    DS_U32  Name;                       /* 0 for local semaphores           */
    DS_U32  OwnerPid[MAX_NOF_PROCS];    /* Owner process IDs. Contains      */
                                        /* INVALID_PID for non used entries */
    DS_U32  LockCount[MAX_NOF_PROCS];   /* Lock count per process           */
    WAIT_Q  WaitQ;                      /* Queue of processes waiting on    */
                                        /* the semaphore                    */
}SEM_STR;

typedef struct
{
    SEM_STR _Sems[MAX_NOF_SEMS];
}SEMS;

/*============================
 * Shared variables
 *===========================*/
static SEMS *SemsPtr = NULL;        /* Pointer to semaphores area */

/*============================
 * Prototypes
 *===========================*/
static DS_U32 SemCreate (DS_U32 SemName, DS_U32 InitCount, DS_U32 *SemId);
static DS_U32 SemLock (DS_U32 SemId, DS_U32 Timeout, DS_U32 *TimeLeft);
static DS_U32 SemUnlock (DS_U32 SemId);
static DS_U32 SemDelete (DS_U32 SemId);
static DS_U32 SemInfo (DS_U32 SemId, DS_U32 *Size, void *Dest);
static SEM_STR *GetSemPtr (DS_U32 SemId, DS_U32 *ProcIndex);

#if 0
 \|/        \|/
 "@` ______ `@"
 /  /  ..  \  \
 --|  \__/  |--
    \___U__/
#endif

/*^^***************************************************************************
 * void SemInit (void)
 *
 * Description: Initialize kernel semaphores for first use
 *
 * Entry :  None
 *
 * Return:  None
 *
 * Notes :
 *
 **************************************************************************^^*/
void SemInit (void)
{
    DS_U32 i;
    SEM_STR *SemPtr;

    /*=======================================
     * Allocate shared mem for semaphores
     *======================================*/
    if (SemsPtr == NULL)
    {
        SemsPtr = lld_os_malloc (sizeof(SEMS));

        /*=======================================
         * Initialize semaphors array
         *======================================*/
        for (i=0; i<MAX_NOF_SEMS; i++)
        {
            SemPtr = SemaphorePtr(i);
            SemPtr->InitCount = UNUSED_SEM;
        }
    }
}

/*^^***************************************************************************
 * void SemCleanup (void)
 *
 * Description: Cleanup for module shutdown
 *
 * Entry :  None
 *
 * Return:  None
 *
 * Notes :
 *
 **************************************************************************^^*/
void SemCleanup (void)
{
    if (SemsPtr != NULL)
    {
        lld_os_free (SemsPtr);
    }
}

/*^^***************************************************************************
 * void SemProcessExit (void)
 *
 * Description: Cleanup for all semaphores when a process exits normally
 *              or upnormally. Also unlock all pending tasks/processes.
 *
 * Entry :  None
 *
 * Return:  None
 *
 * Notes :  Any tasks that belong to the terminating process and are pending
 *          on or already locked the semaphore, will not be notified.
 *          Pending tasks that belong to other processes will be awakened.
 *
 **************************************************************************^^*/
void SemProcessExit (void)
{
    DS_U32  i, j, k, ProcId;
    DS_BOOL    Removed;
    SEM_STR *SemPtr;

    ProcId = mCurrentPid;

    /*==============================
     * For all sems in the system
     *=============================*/
    for (i=0; i<MAX_NOF_SEMS; i++)
    {
        SemPtr = SemaphorePtr(i);
        /*===============================
         * If semaphore exist
         *==============================*/
        if (SemPtr->InitCount != UNUSED_SEM)
        {
            k = 0;                                  /* Sem reference count */
            Removed = _FALSE_;
            /*===============================
             * Find sem reference count
             *==============================*/
            for (j=0; j<MAX_NOF_PROCS; j++)
            {
                /*=======================================
                 * If the terminating proc owns the sem
                 * remove it from the list
                 *======================================*/
                if (SemPtr->OwnerPid[j] == ProcId)
                {
                    SemPtr->OwnerPid[j] = INVALID_PID;
                    Removed = _TRUE_;
                    /*=================================================
                     * If sem was locked by this process, unlock it
                     *================================================*/
                    if (SemPtr->LockCount[j])
                    {
                        if (SemPtr->InitCount == MUTEX_OBJ)
                        {
                            SemPtr->Count = 1;
                        }
                        else
                        {
                            SemPtr->Count += SemPtr->LockCount[j];
                        }
                        SemPtr->LockCount[j] = 0;
                    }
                }
                if (SemPtr->OwnerPid[j] != INVALID_PID)
                {
                    k++;                            /* Increment ref count */
                }
            }

            /*=======================================================
             * If no other processes reference the sem, delete it
             *======================================================*/
            if (k == 0)
            {
                SemPtr->InitCount = UNUSED_SEM;
            }
            /*============================================
             * If the process was removed,
             * Wake up any pending tasks in the system
             *===========================================*/
            if (Removed)
            {
                lld_wakeup_wait_q (&(SemPtr->WaitQ));
            }
        }
    }
}

/*^^***************************************************************************
 * DS_U32 SemOperation (OBJ_OPER *SemPrm)
 *
 * Description: Perform semaphore operation defined in SemPrm
 *
 * Entry :  SemPrm = Pointer to OBJ_OPER struc holding the requested operation.
 *
 * Return:  0 or -ERESTARTSYS
 *          SemPrm.RetCode hold return values (varies by the opcode)
 *          SemPrm.Prm2 will also be set as follows:
 *            Create: SemPrm.Prm2 = Sem ID. = 0 if any error.
 *            Lock:   SemPrm.Prm2 = Adjusted timeout if ret due to signal
 *            Info:   SemPrm.Prm2 = Info size
 *
 * Notes :
 *
 **************************************************************************^^*/
DS_U32 SemOperation (OBJ_OPER *SemPrm)
{
        //printk("b4 SemPrm->Opcode=0x%lX, SemPrm->Prm1=0x%lX, SemPrm->Prm2=0x%lX\n", SemPrm->Opcode, SemPrm->Prm1, SemPrm->Prm2);
        
    switch (SemPrm->Opcode)
    {
        case OBJ_OPCODE_CREATE:
            SemPrm->RetCode = SemCreate (SemPrm->Prm1, SemPrm->Prm2, 
                                         &(SemPrm->Prm2));
            break;
        case OBJ_OPCODE_DELETE:
            SemPrm->RetCode = SemDelete (SemPrm->Prm1);
            break;
        case OBJ_OPCODE_LOCK:
            SemPrm->RetCode = SemLock (SemPrm->Prm1, SemPrm->Prm2, 
                                       &(SemPrm->Prm2));
            if (SemPrm->RetCode == OBJ_SIGNAL_RCV)
            {
                return (ERR_RESTART_SYS);
            }
            break;
        case OBJ_OPCODE_UNLOCK:
            SemPrm->RetCode = SemUnlock (SemPrm->Prm1);
            break;
        case OBJ_OPCODE_INFO:
            SemPrm->RetCode = SemInfo (SemPrm->Prm1, &(SemPrm->Prm2), 
                                       (void *)(SemPrm->Prm2));
            break;
        default:
            SemPrm->RetCode = OBJ_INVALID_OP;
            break;
    }
        //printk("a4 SemPrm->Opcode=0x%lX, SemPrm->Prm1=0x%lX, SemPrm->Prm2=0x%lX\n", SemPrm->Opcode, SemPrm->Prm1, SemPrm->Prm2);

    return (0);
}

/*^^***************************************************************************
 * static DS_U32 SemCreate (DS_U32 SemName, DS_U32 InitCount, DS_U32 *SemId)
 *
 * Description: Creates a semaphore object with the specified count.
 *
 * Entry :  SemName     = Unique number identifying the semaphore.
 *                        Set to NULL to create unnamed semaphore (can not be
 *                        used for inter-process synchronization)
 *          InitCount   = Initial semaphore count (0 to 255)
 *                        or MUTEX_OBJ for a mutex
 *          SemId       = Pointer to receive semaphore ID
 *
 * Return:  Entry SemId holds semaphore ID. =0 if any error. Return codes are:
 *          OBJ_OK              Operation completed
 *          OBJ_INVALID_ID      Invalid sem id or process does not own sem
 *          OBJ_MAX_PROCS       Max number of procs reached
 *          OBJ_MAX_OBJS        Max number of semaphores reached
 *          OBJ_COUNT_MISMATCH  Shared sem init count does not match
 *
 * Notes :  If the same process creates a shared semaphore multiple times,
 *          it's pid will appear in the semaphore's OwnerPid list only once.
 *          In this case, only one delete is required by the calling process
 *          to delete the semaphore.
 *
 **************************************************************************^^*/
static DS_U32 SemCreate (DS_U32 SemName, DS_U32 InitCount, DS_U32 *SemId)
{
    DS_U32 i, j;
    SEM_STR *SemPtr = NULL;

    *SemId = 0;
    if (((InitCount > 255) && (InitCount != MUTEX_OBJ)) || (SemsPtr == NULL))
    {
        return (OBJ_INVALID_ID);
    }

    /*================================
     * If shared semaphore
     *===============================*/
    if (SemName != 0)
    {
        /*===========================================
         * Find if shared sem exist (same name)
         *==========================================*/
        for (i=0; i<MAX_NOF_SEMS; i++)
        {
            SemPtr = SemaphorePtr(i);
            if ((SemPtr->InitCount != UNUSED_SEM) && (SemPtr->Name == SemName))
            {
                if (SemPtr->InitCount != InitCount)
                {
                    return (OBJ_COUNT_MISMATCH);
                }
                /*===============================================
                 * Find if current process already owns the sem
                 *==============================================*/
                for (j=0; j<MAX_NOF_PROCS; j++)
                {
                    if (SemPtr->OwnerPid[j] == mCurrentPid)
                    {
                        *SemId = (i+1);
                        return (OBJ_OK);
                    }
                }
                /*===============================================
                 * No, add process to owners list
                 *==============================================*/
                for (j=0; j<MAX_NOF_PROCS; j++)
                {
                    if (SemPtr->OwnerPid[j] == INVALID_PID)
                    {
                        SemPtr->OwnerPid[j] = mCurrentPid;
                        SemPtr->LockCount[j] = 0;
                        *SemId = (i+1);
                        return (OBJ_OK);
                    }
                }
                return (OBJ_MAX_PROCS);
            }
        }
    }

    /*======================================================
     * If sem does not exist or local sem, create new one
     *=====================================================*/
    for (i=0; i<MAX_NOF_SEMS; i++)
    {
        SemPtr = SemaphorePtr(i);
        if (SemPtr->InitCount == UNUSED_SEM)
        {
            SemPtr->InitCount = InitCount;
            SemPtr->Count = InitCount;
            if (InitCount == MUTEX_OBJ)
            {
                SemPtr->Count = 1;
            }
            SemPtr->Name = SemName;
            for (j=0; j<MAX_NOF_PROCS; j++)
            {
                SemPtr->OwnerPid[j] = INVALID_PID;
            }
            SemPtr->OwnerPid[0] = mCurrentPid;
            SemPtr->LockCount[0] = 0;
            lld_init_wait_q (&(SemPtr->WaitQ));
            *SemId = (i+1);
            return (OBJ_OK);
        }
    }
    return (OBJ_MAX_OBJS);
}

/*^^***************************************************************************
 * static DS_U32 SemLock (DS_U32 SemId, DS_U32 Timeout, DS_U32 *TimeLeft)
 *
 * Description: Locks a semaphore (decrement count) with a timeout
 *
 * Entry :  SemId    = Returned by SemCreate
 *          Timeout  = In 1/100 of a second increments.
 *                     = 0 to return immidiately if the resource not available
 *                     = 0xffffffff to wait forever on the resource
 *          TimeLeft = Pointer to DS_U32 to receive remaining timeout if the
 *                     lock operation interrupted by a signal (if return value
 *                     is OBJ_SIGNAL_RCV)
 *
 * Return:  OBJ_OK              Operation completed
 *          OBJ_INVALID_ID      Invalid sem id or process does not own sem
 *          OBJ_TIMEOUT         Timedout
 *          OBJ_SIGNAL_RCV      Signal received
 *
 * Notes :  If sem count > 0, it will be decremented and the function will 
 *          return 0
 *          If sem count = 0 and Timeout = 0, the function will return timeout
 *          If sem count = 0 and Timeout > 0, other processes will be 
 *          scheduled until either timeout is elapsed (return TIMEOUT), the 
 *          calling process detects that sem count > 0 (return OK) or the 
 *          process receives a signal (return SIGNAL_RCV)
 *          
 *          **** VERY IMPORTANT ***
 *          This locking mechanism does not support priority inversion since
 *          linux does not support (_TRUE_) task priorities.
 *
 **************************************************************************^^*/
static DS_U32 SemLock (DS_U32 SemId, DS_U32 Timeout, DS_U32 *TimeLeft)
{
    DS_U32 i, Idx;
    SEM_STR *SemPtr;

    while (_TRUE_)
    {
        /*===========================================
         * Make sure sem still exist (not deleted)
         *==========================================*/
        SemPtr = GetSemPtr(SemId, &Idx);
        if (SemPtr == NULL)
        {
            return (OBJ_INVALID_ID);
        }

        /*==========================
         * If Count > 0
         *=========================*/
        if (SemPtr->Count != 0)
        {
            SemPtr->Count--;
            SemPtr->LockCount[Idx]++;
            return (OBJ_OK);
        }

        /*================================
         * If Count == 0 and no timeout
         *===============================*/
        if (Timeout == 0)
        {
            return (OBJ_TIMEOUT);
        }

        /*===========================================================
         * If Count == 0 and timeout, wait unlock or timeout event
         *==========================================================*/
        if (Timeout == OS_WAIT_FOREVER)
        {
            lld_wait_on_q (&(SemPtr->WaitQ));
            i = Timeout;                        /* Any non zero value */
        }
        else
        {
            i = lld_timeout_wait_on_q(&(SemPtr->WaitQ), Timeout);
        }
        if (i == 0)
        {
            return (OBJ_TIMEOUT);
        }
        if (lld_os_signal_detected())
        {
            *TimeLeft = i;
            return (OBJ_SIGNAL_RCV);
        }
        if (Timeout != OS_WAIT_FOREVER)
        {
            Timeout = i;
        }
    }
}

/*^^***************************************************************************
 * static DS_U32 SemUnlock (DS_U32 SemId)
 *
 * Description: Unlock a semaphore (Increment count)
 *
 * Entry :  SemId   = Returned by SemCreate
 *
 * Return:  OBJ_OK              Operation completed
 *          OBJ_INVALID_ID      Invalid sem id or process does not own sem
 *          OBJ_BAD_COUNT       Bad count
 *
 * Notes:   The semaphore count will be incremented even if it is > creation
 *          time initial count. First scheduled pending task will be awakended
 *          to lock the semaphore.
 *
 **************************************************************************^^*/
static DS_U32 SemUnlock (DS_U32 SemId)
{
    DS_U32 Idx;
    SEM_STR *SemPtr;

    SemPtr = GetSemPtr(SemId, &Idx);
    if (SemPtr == NULL)
    {
        return (OBJ_INVALID_ID);
    }
    if ((SemPtr->InitCount == MUTEX_OBJ) && (SemPtr->Count == 1))
    {
        return (OBJ_OK);
    }
    SemPtr->Count++;
    /*==============================================================
     * Decrease lock count only if sem was locked by this process
     *=============================================================*/
    if (SemPtr->LockCount[Idx])
    {
        SemPtr->LockCount[Idx]--;
    }
    lld_wakeup_wait_q (&(SemPtr->WaitQ));
    return (OBJ_OK);
}

/*^^***************************************************************************
 * static DS_U32 SemDelete (DS_U32 SemId)
 * 
 * Description: Deletes a semaphore.
 *
 * Entry :  SemId   = Returned by SemCreate
 *
 * Return:  OBJ_OK              Operation completed
 *          OBJ_INVALID_ID      Invalid sem id or process does not own sem
 *
 * Notes:   The semaphore will be deleted only if its reference count is 0.
 *          All tasks waiting on the semaphore will be unblocked with error
 *          return code. Tasks that already locked the semaphore will get
 *          an error the next time they perform any semaphore operation.
 *          The only caveat here is:
 *          If a taskA deletes a semaphore while other task(s) are pending or
 *          already locked it, and TaskA then creates another semaphore before
 *          the other tasks gets notified of the deleted semaphore, it is
 *          possible that the returned SemId will be the same as the just
 *          deleted semaphore and hence other pending/locked task(s) will 
 *          continue to run normally.
 *
 **************************************************************************^^*/
static DS_U32 SemDelete (DS_U32 SemId)
{
    DS_U32 i, j;
    SEM_STR *SemPtr;

    SemPtr = GetSemPtr(SemId, &j);
    if (SemPtr == NULL)
    {
        return (OBJ_INVALID_ID);
    }

    /*======================================
     * Remove process from owners list
     *=====================================*/
    SemPtr->OwnerPid[j] = INVALID_PID;

    /*==================================================
     * Check if other process still use the semaphore
     *=================================================*/
    j = 0;
    for (i=0; i<MAX_NOF_PROCS; i++)
    {
        if (SemPtr->OwnerPid[i] != INVALID_PID)
        {
            j = 1;                          /* Other process is using it */
            break;
        }
    }

    /*================================================
     * If no other process is using the semaphore
     *===============================================*/
    if (j == 0)
    {
        SemPtr->InitCount = UNUSED_SEM;
    }

    /*======================================
     * Awake all tasks waiting on the sem
     *=====================================*/
    lld_wakeup_wait_q (&(SemPtr->WaitQ));
    return (OBJ_OK);
}

/*^^***************************************************************************
 * static void SemInfo (DS_U32 SemIdx, DS_U32 *Size, void *Dest)
 * 
 * Description: Return a copy of kernel structure for a semaphore.
 *
 * Entry :  SemId   = User mode sem ID
 *          Size    = Pointer to receive size of sem structure
 *          Dest    = Area to copy sem info to
 *
 * Return:  
 *
 * Notes:   This fucntion is provided for diagnostic purpose only.
 *
 **************************************************************************^^*/
static DS_U32 SemInfo (DS_U32 SemId, DS_U32 *Size, void *Dest)
{
    SEM_STR *SemPtr;

    SemId--;
    if ((SemId >= MAX_NOF_SEMS) || (SemsPtr == NULL))
    {
        return (OBJ_INVALID_ID);
    }
    SemPtr = SemaphorePtr(SemId);
    *Size = sizeof (SEM_STR);
    lld_os_copy_to_user(Dest, (DS_S8 *)SemPtr, sizeof(SEM_STR));
    return (OBJ_OK);
}

/*^^***************************************************************************
 * static SEM_STR *GetSemPtr (DS_U32 SemId, DS_U32 *ProcIndex)
 * 
 * Description: Find semaphore corresponding to a sem ID and process ID
 *
 * Entry :  SemId       = User mode sem ID
 *          ProcIndex   = Ptr to receive process index in OwnerPid array or
 *                        NULL if not needed
 *
 * Return:  Pointer to semaphore struc or NULL
 *
 * Notes:   This call will fail if the semaphore does not exist or if
 *          the caller process did not create the semaphore.
 *
 **************************************************************************^^*/
static SEM_STR *GetSemPtr (DS_U32 SemId, DS_U32 *ProcIndex)
{
    SEM_STR *SemPtr;
    DS_U32  i, ProcId;

    SemId--;
    if ((SemId >= MAX_NOF_SEMS) || (SemsPtr == NULL))
    {
        return (NULL);
    }
    ProcId = mCurrentPid;                  /* Current process ID */
    SemPtr = SemaphorePtr(SemId);
    if (SemPtr->InitCount == UNUSED_SEM)
    {
        return (NULL);
    }
    for (i=0; i<MAX_NOF_PROCS; i++)
    {
        if (SemPtr->OwnerPid[i] == ProcId)
        {
            if (ProcIndex != NULL)
            {
                *ProcIndex = i;
            }
            return (SemPtr);
        }
    }
    return (NULL);
}