source: svn/trunk/zas_dstar/hal/os/src/v2lin/lsemLib.c @ 2

/****************************************************************************
 *_Copyright (C) 2004, 2005, 2006 v2lin Team <http://v2lin.sf.net>
 *_Copyright (C) 2000,2001  Monta Vista Software Inc.
 * 
 * This file is part of the v2lin Library.
 * VxWorks is a registered trademark of Wind River Systems, Inc.
 * 
 * Initial implementation Gary S. Robertson, 2000, 2001.
 * Contributed by Andrew Skiba, skibochka@sourceforge.net, 2004.
 * Contributed by Mike Kemelmakher, mike@ubxess.com, 2005.
 * Contributed by Constantine Shulyupin, conan.sh@gmail.com, 2006.
 * 
 * The v2lin library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * 
 * The v2lin Library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 * 
 ****************************************************************************/

#define FIX_PTHREAD_COND_BUG    0

#include <errno.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <semaphore.h>
#include <sys/time.h>
#include "v2lpthread.h"
#include "vxw_defs.h"
#include "internal.h"
#include <assert.h>
#include "v2ldebug.h"
#include "vxw_hdrs.h"
#include <pthread.h>

#include "dsthalcommon.h"
#include "dstoslayer.h"
#include "os.h"
#include "os_prive.h"

#define SEM_OPT_MASK       0x0f
#define SEM_TYPE_MASK      0xf0

#define BINARY_SEMA4       0x10
#define MUTEX_SEMA4        0x20
#define COUNTING_SEMA4     0x40

#define SEND  0
#define FLUSH 1
#define KILLD 2

#if 0
___Semaphore_APIs___()
#endif
/*****************************************************************************
**  Control block for v2pthread semaphore
**
**  The basic POSIX semaphore does not provide for time-bounded waits nor
**  for selection of a thread to ready based either on FIFO or PRIORITY-based
**  waiting.  This 'wrapper' extends the POSIX pthreads semaphore to include
**  the attributes of a v2pthread semaphore.
**
*****************************************************************************/

static v2lsem_t *sem_list;

static pthread_mutex_t sem_list_lock = PTHREAD_MUTEX_INITIALIZER;

void semShow(v2lsem_t *s, FILE * out, int mem)
{
        DS_TASK_T *t;
        fprintf(out,"o:%x ", (int)s->current_owner);
        if (s->current_owner) { 
            t = taskFind(s->current_owner, 1);
                if (!t) // task not found
                        fprintf(out,"<deleted> ");
                else fprintf(out,"%s ", t->name);
        }
        fprintf(out,"rl:%i ",s->recursion_level);
        fprintf(out,"tc:%i ",s->token_count);
        fprintf(out,"(");
        for ( t = s->first_susp; t; t = t->nxt_susp ) {
                fprintf(out,"%x ",(int)t);
                fprintf(out,"%s ",t->name);
        }
        fprintf(out,") ");
        fprintf(out,"%x ",s->send_type);

    fprintf(out,"%c ", s->flags & BINARY_SEMA4 ? 'B' : s->flags & MUTEX_SEMA4 ? 'M' : s->flags & COUNTING_SEMA4 ? 'C' : 'u' );
        if ( mem ) {
                int * i;
                for ( i = (int*)s; i < (int*) (s+1);i++) 
                        fprintf(out,"%x ",*i);
        }
        fprintf(out,"\n");
}

int semList(FILE * out, int mem)
{
        int c=0;
        TRACEF();
        v2lsem_t *s = sem_list;
        while (s) {
                c++;
                fprintf(out,"%i %x: ",c, (int)s);
                semShow(s,out,mem);
                s=s->nxt_sem;
        }
        return c;
}


/*****************************************************************************
**  sem_find_lock - verifies whether the specified semaphore still exists, and if
**                so, locks exclusive access to the semaphore for the caller.
*****************************************************************************/
static int sem_find_lock(v2lsem_t * sem)
{
    int b_found = 0;
        v2lsem_t *current_smcb;

        //pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, (void *) &sem_list_lock);
        pthread_mutex_lock(&sem_list_lock);

        for (current_smcb = sem_list; current_smcb != NULL; current_smcb = current_smcb->nxt_sem) {
                if (current_smcb == sem) {
                        /*
                         ** Lock mutex for semaphore access (it is assumed that a
                         ** 'pthread_cleanup_push()' has already been performed
                         **  by the caller in case of unexpected thread termination.)
                         */
                        pthread_mutex_lock(&(sem->sem_lock));
                        b_found = 1;
                        break;
                }
        }

        pthread_mutex_unlock(&sem_list_lock);
        //pthread_cleanup_pop(0);

    if ( b_found == 0 )
    {
        fprintf(stderr, "|%s:%d| ERROR.\n", __FUNCTION__, __LINE__);
        return 0;
    }
    
        return current_smcb != NULL;
}


/*****************************************************************************
** link_smcb - appends a new semaphore control block pointer to the sem_list
*****************************************************************************/
static void link_smcb(v2lsem_t * new_sem)
{
        v2lsem_t **i;

        //pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, (void *) &sem_list_lock);
        pthread_mutex_lock(&sem_list_lock);

        new_sem->nxt_sem = (v2lsem_t *) NULL;
        i = &sem_list;
        while (*i) i =& (*i)->nxt_sem;
        *i=new_sem;
        pthread_mutex_unlock(&sem_list_lock);
        //pthread_cleanup_pop(0);
}

/*****************************************************************************
** unlink_smcb - removes a semaphore control block pointer from the sem_list
*****************************************************************************/
static void unlink_smcb(v2lsem_t * smid)
{
        v2lsem_t **i =  & sem_list;

        //pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, (void *) &sem_list_lock);
        pthread_mutex_lock(&sem_list_lock);
        while (*i) {
                if ( *i == smid ) {
                        TRACEF("%p", smid);
                        *i = (*i)->nxt_sem;     // remove
                        break;
                }
                i = &((*i)->nxt_sem);
        }
        pthread_mutex_unlock(&sem_list_lock);
        //pthread_cleanup_pop(0);
}

/*****************************************************************************
** new_sem - creates a new v2pthread semaphore using pthreads resources
*****************************************************************************/
v2lsem_t *new_sem(int count)
{
        v2lsem_t *semaphore;
  
        semaphore = (v2lsem_t *) malloc(sizeof(v2lsem_t));
        if (! semaphore ) {
                return NULL;
        }

        memset(semaphore,0,sizeof(v2lsem_t));
        semaphore->token_count = count;

        /*
         ** Mutex and Condition variable for semaphore send/pend
         */
#if 1
        pthread_mutex_init(&semaphore->sem_lock, NULL);
        pthread_cond_init(&semaphore->sem_send, NULL);

        pthread_mutex_init(&semaphore->smdel_lock, NULL);
        pthread_cond_init(&semaphore->smdel_cplt, NULL);
#else
    memcpy( &semaphore->sem_lock, &initmutex, sizeof(initmutex) );
    memcpy( &semaphore->sem_send, &initcond, sizeof(initcond) );

    memcpy( &semaphore->smdel_lock, &initmutex, sizeof(initmutex) );
    memcpy( &semaphore->smdel_cplt, &initcond, sizeof(initcond) );
#endif    

        semaphore->send_type = SEND;
        semaphore->recursion_level = 0;
        semaphore->current_owner = 0;
        semaphore->first_susp =  NULL;

        return semaphore;
}

/*****************************************************************************
** semBCreate - creates a v2pthread binary semaphore
*****************************************************************************/
v2lsem_t *semBCreate(int opt, int initial_state)
{
        v2lsem_t *semaphore;

        if ((opt & SEM_Q_PRIORITY)
                        || (opt & SEM_DELETE_SAFE)
                        || (opt & SEM_INVERSION_SAFE)) {
                errno = ENOSYS;
                return (NULL);
        }

        if (initial_state == 0)
                semaphore = new_sem(0);
        else
                semaphore = new_sem(1);

        if (! semaphore ) return NULL;
        TRACEF("%x %#x %x", (int)semaphore,(int)opt, (int)initial_state);

        semaphore->flags = (opt & SEM_Q_PRIORITY) | BINARY_SEMA4;

        link_smcb(semaphore);
        return semaphore;
}

/*****************************************************************************
** semCCreate - creates a v2pthread counting semaphore
*****************************************************************************/
v2lsem_t *semCCreate(int opt, int initial_count)
{
        v2lsem_t *semaphore;

        if ((opt & SEM_Q_PRIORITY)
                        || (opt & SEM_DELETE_SAFE)
                        || (opt & SEM_INVERSION_SAFE)) {
                errno = ENOSYS;
                return (NULL);
        }

        semaphore = new_sem(initial_count);

        if (! semaphore ) return NULL;
        TRACEF("Creating counting semaphore - id %p", semaphore);

        semaphore->flags = (opt & SEM_Q_PRIORITY) | COUNTING_SEMA4;

        link_smcb(semaphore);

        return semaphore;
}

/*****************************************************************************
** semMCreate - creates a v2pthread mutual exclusion semaphore
*****************************************************************************/
v2lsem_t *semMCreate(int opt)
{
        v2lsem_t *semaphore;

        if ((opt & SEM_Q_PRIORITY)
                        || (opt & SEM_DELETE_SAFE)
                        || (opt & SEM_INVERSION_SAFE)) {
                TRACEF("WARNING not implemented");
                errno = ENOSYS;
                return (NULL);
        }
        semaphore = new_sem(1);

        if ( ! semaphore ) return NULL;
        TRACEF("Creating mutex semaphore - id %p", semaphore);

        semaphore->flags = (opt & SEM_OPT_MASK) | MUTEX_SEMA4;

        link_smcb(semaphore);

        return semaphore;
}

/*****************************************************************************
** semDelete - removes the specified semaphore from the semaphore list and
**             frees the memory allocated for the semaphore control block.
*****************************************************************************/
STATUS semDelete(v2lsem_t * semaphore)
{
        STATUS error = OK;

        //pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, &semaphore->sem_lock);
        if (! sem_find_lock(semaphore)) {
                error = S_objLib_OBJ_ID_ERROR;  /* Invalid semaphore specified */
                goto exit;
        }
        TRACEF("task @ %x delete semaphore @ %x", (int)OS_GetSelfTaskId(), (int)semaphore);
        //  Send signal and block while any tasks are still waiting on the semaphore
//      taskLock();
        if (semaphore->first_susp !=  NULL) {
                TRACEF("isemDelete - tasks pending on semaphore list @ %p", semaphore->first_susp);
                // Lock mutex for semaphore delete completion
                //pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, &semaphore->smdel_lock);
                pthread_mutex_lock(&(semaphore->smdel_lock));

                semaphore->send_type = KILLD;

                pthread_cond_broadcast(&(semaphore->sem_send));

                pthread_mutex_unlock(&(semaphore->sem_lock));

                /*
                 **  Wait for all pended tasks to receive delete message.
                 **  The last task to receive the message will signal the
                 **  delete-complete condition variable.
                 */
                while (semaphore->first_susp != (DS_TASK_T *) NULL)
                        pthread_cond_wait(&(semaphore->smdel_cplt), &(semaphore->smdel_lock));

                //pthread_cleanup_pop(0);
        }

        unlink_smcb(semaphore);
        free((void *) semaphore);
//      taskUnlock();

        exit:
        {}
        //pthread_cleanup_pop(0);

        if (error != OK) {
                errno = (int) error;
                error = ERROR;
        }
        return error;
}

/*****************************************************************************
** semFlush - unblocks all tasks waiting on the specified semaphore
*****************************************************************************/
STATUS semFlush(v2lsem_t * semaphore)
{
        STATUS error = OK;
        TRACEF("%x", (int)semaphore);
        //pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, &semaphore->sem_lock);
        if (!sem_find_lock(semaphore)) {
                error = S_objLib_OBJ_ID_ERROR;  /* Invalid semaphore specified */
                goto exit;
        }
        if ((semaphore->flags & SEM_TYPE_MASK) == MUTEX_SEMA4) {
                error = S_semLib_INVALID_OPERATION;     /* Flush invalid for mutex */
                pthread_mutex_unlock(&semaphore->sem_lock);
                goto exit;
        }
        TRACEF("task @ %x flush semaphore list @ %x", (int)OS_GetSelfTaskId(), (int)&(semaphore->first_susp));
        /*
         **  Send signal and block while any tasks are still waiting
         **  on the semaphore
         */
        //taskLock();
        if (semaphore->first_susp ) {
                // Lock mutex for semaphore delete completion
                //pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, &semaphore->smdel_lock);
                pthread_mutex_lock(&(semaphore->smdel_lock));

                semaphore->send_type = FLUSH;
                pthread_cond_broadcast(&semaphore->sem_send);
                pthread_mutex_unlock(&semaphore->sem_lock);

                /*
                 **  Wait for all pended tasks to receive delete message.
                 **  The last task to receive the message will signal the
                 **  delete-complete condition variable.
                 */
                while (semaphore->first_susp != NULL)
                        pthread_cond_wait(&(semaphore->smdel_cplt), &(semaphore->smdel_lock));

                pthread_mutex_unlock(&(semaphore->smdel_lock));
                //pthread_cleanup_pop(0);
        }
        // TODO ??? else pthread_mutex_unlock(&(semaphore->sem_lock));
        //taskUnlock();
exit:
        {}
        //pthread_cleanup_pop(0);

        if (error != OK) {
                errno = (int) error;
                error = ERROR;
        }

        return error;
}

/*****************************************************************************
** semGive - releases a v2pthread semaphore token and awakens the first
**           selected task waiting on the semaphore.
*****************************************************************************/
STATUS semGive(v2lsem_t * semaphore)
{
        STATUS error = OK;

    SysASSERT( semaphore );
        
        //pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, &semaphore->sem_lock);

        if (! sem_find_lock(semaphore)) {
                TRACEF("WARNING not found %x",(int)semaphore);
                error = S_objLib_OBJ_ID_ERROR;
                goto exit;
        }
        TRACEF("%x %i %i", (int)semaphore, (int)semaphore->recursion_level, (int)semaphore->token_count);
        //  If semaphore is a mutex, make sure we own it before giving up the token.
        if (((semaphore->flags & SEM_TYPE_MASK) == MUTEX_SEMA4) &&
                        (semaphore->current_owner != OS_GetSelfTaskId())) {
                error = S_semLib_INVALID_OPERATION;     
                goto unlock;
        }

        /*
         **  Either semaphore isn't a mutex or we currently own the mutex.
         **  If semaphore is a mutex, recursion level should be > zero.
         **  In this case, decrement recursion level, and if level == zero
         **  after decrement, relinquish mutex ownership.
         */
        //TRACEF("Semaphore list @ %p recursion level = %d", &semaphore->first_susp, semaphore->recursion_level);
        if (semaphore->recursion_level > 0) {
                --semaphore->recursion_level;
                if ( ! semaphore->recursion_level ) {
                        semaphore->token_count++;
                        TRACEF("--rl=%i ++tc=%i",semaphore->recursion_level,semaphore->token_count);
                        semaphore->current_owner = 0;
#if 0
                        if (semaphore->flags & SEM_DELETE_SAFE)
                                // Task was made deletion-safe when mutex acquired...  Remove deletion safety now.
                                taskUnsafe();
#endif

#if USE_PRIORITY_INVERSION
                        if (semaphore->flags & SEM_INVERSION_SAFE) {
                                /*
                                 **  Task priority may have been boosted during
                                 **  ownership of semaphore...
                                 **  Restore to original priority now.  Call taskLock
                                 **  so taskUnlock can be called to restore priority.
                                 */
                                TRACEF("Restoring task priority for owner of semaphore list @ %p",
                                                &(semaphore->first_susp));
                                taskLock();
                                taskUnlock();
                        }
#endif
                }
                // else semaphore->recursion_level > 0
        } else  {
                ++semaphore->token_count;
                TRACEF("++tc=%i",semaphore->token_count);
        }

#if 1//!FIX_PTHREAD_COND_BUG
        if (semaphore->first_susp) {
                pthread_cond_broadcast(&semaphore->sem_send);
        }
#endif
unlock:
    pthread_mutex_unlock(&semaphore->sem_lock);
        TRACEF("%x %i %i", (int)semaphore, (int)semaphore->recursion_level, (int)semaphore->token_count);
exit:
        {}
        //pthread_cleanup_pop(0);

        if (error != OK) {
                errno = (int) error;
                error = ERROR;
        }
        TRACEF("%x",(int)semaphore);
        return error;
}

BOOL time_expired(struct timespec *timeout)
{
        struct timeval now;
        gettimeofday(&now, NULL);
        if (timeout->tv_sec < now.tv_sec)
                return TRUE;
        if (timeout->tv_sec > now.tv_sec)
                return FALSE;
        assert(timeout->tv_sec == now.tv_sec);
        return timeout->tv_nsec <= now.tv_usec * 1000;
}

/*****************************************************************************
** waiting_on_sem - returns a nonzero result unless a qualifying event
**                    occurs on the specified semaphore which should cause the
**                    pended task to be awakened.  The qualifying events
**                    are:
**                        1. a token is returned to the semaphore and the
**                            current task is selected to receive it
**                        2. the semaphore is deleted or flushed
*****************************************************************************/
static int waiting_on_sem(v2lsem_t * semaphore, struct timespec *timeout, int *retcode)
{
        int result;
        TRACEF("%x %i", (int)semaphore, (int)semaphore->token_count);
        if ((semaphore->send_type & KILLD) || (semaphore->send_type & FLUSH)) {
                //  Semaphore has been killed... waiting is over.
                result = 0;
                *retcode = 0;
                goto exit;
        }
        // Semaphore still in service... check for token availability.
        //  Initially assume no token available for our task
        result = 1;

        /*  Multiple posts to the semaphore may be represented by only
         **  a single signal to the condition variable, so continue
         **  checking for a token for our task as long as more tokens
         **  are available. */

        while (semaphore->token_count > 0) {
                //  Available token arrived... see if it's for our task.
                if ((signal_for_my_task(&semaphore->first_susp, (semaphore->flags & SEM_Q_PRIORITY)))) {
                        /**  Token was destined for our task specifically...
                         **  waiting is over.    */
                        semaphore->token_count--;
                        result = 0;
                        *retcode = 0;
                        break;
                } else {
                        /*
                         **  Token isn't for our task...  Sleep awhile to
                         **  allow other tasks ahead of ours in the queue of tasks
                         **  waiting on the semaphore to get their tokens, bringing
                         **  our task to the head of the list.
                         */

                        pthread_mutex_unlock(&(semaphore->sem_lock));
                        OS_mDelay(10);
                        pthread_mutex_lock(&(semaphore->sem_lock));
                }

                if ( timeout && time_expired (timeout))
                                break;
        }

exit:
        return result;
}

#if USE_PRIORITY_INVERSION
void priority_inversion(v2lsem_t * semaphore, int max_wait, task_t * our_task)
{
        task_t *task;
        int my_priority, owners_priority, sched_policy;
        taskLock();

        my_priority = our_task->prv_priority.sched_priority;

        task = semaphore->current_owner;
        owners_priority = task->prv_priority.sched_priority;

        TRACEF("Task @ %p priority %d owns mutex", task, owners_priority);
        TRACEF("Calling task @ %p priority %d wants mutex", our_task, my_priority);
        /*
         **  If mutex owner's priority is lower than ours, boost it
         **  to our priority level tempororily until owner releases mutex.
         **  This avoids 'priority inversion'.
         */
        if (owners_priority < my_priority) {
                struct sched_param schedparam;
                pthread_attr_getschedpolicy(&task->attr, &sched_policy);
                pthread_attr_getschedparam(&task->attr, &schedparam);
                schedparam.sched_priority = my_priority;
                pthread_attr_setschedparam(&task->attr, &schedparam);
                pthread_setschedparam(task->pthrid, sched_policy, &schedparam);
        }

        taskUnlock();

}
#endif

/*****************************************************************************
** wait_for_token - blocks the calling task until a token is available on the
**                  specified v2pthread semaphore.  If a token is acquired and
**                  the semaphore is a mutex type, this function also handles
**                  priority inversion and deletion safety issues as needed.
*****************************************************************************/
STATUS wait_for_token(v2lsem_t * semaphore, int max_wait, DS_TASK_T * our_task)
{
        struct timeval now;
        struct timespec timeout;
        int retcode = 0;
        long sec, usec;
        STATUS error = OK;

        TRACEF("%x", (int)our_task);
        link_susp_task(&semaphore->first_susp, our_task);

        if (max_wait == NO_WAIT) {
                //  Caller specified no wait on semaphore token...
                //  Check the condition variable with an immediate timeout.
                gettimeofday(&now, (struct timezone *) NULL);
                timeout.tv_sec = now.tv_sec;
                timeout.tv_nsec = now.tv_usec * 1000;
                while ((waiting_on_sem(semaphore, &timeout, &retcode)) && (retcode != ETIMEDOUT)) {
                        TRACEF("pthread_cond_timedwait {");
                        retcode = pthread_cond_timedwait(&semaphore->sem_send, &semaphore->sem_lock, &timeout);
                        TRACEF("pthread_cond_timedwait }");
                }
                if (retcode &&  retcode != ETIMEDOUT) {
                        error = S_objLib_OBJ_UNAVAILABLE;
                        TRACEF("...no token available");
                }
        } else {
                //  Caller expects to wait on semaphore, with or without a timeout.
                //  If the semaphore is a mutex type, check for and handle any
                //  priority inversion issues.
#if USE_PRIORITY_INVERSION
                if ((semaphore->flags & SEM_INVERSION_SAFE) && semaphore->current_owner ) {
                        priority_inversion(semaphore, max_wait, our_task);
                }
#endif
                if (max_wait == WAIT_FOREVER) {
                        while (waiting_on_sem(semaphore, 0, &retcode)) {
                                TRACEF("pthread_cond_timedwait {");
                                pthread_cond_wait(&semaphore->sem_send, &semaphore->sem_lock);
                                TRACEF("pthread_cond_timedwait }");
                        }
                } else {
                        /*
                         **  Wait on semaphore message arrival with timeout...
                         **  Calculate timeout delay in seconds and microseconds.
                         */
                        sec = 0;
                        usec = max_wait * V2PT_TICK * 1000;
                        gettimeofday(&now, (struct timezone *) NULL);
                        usec += now.tv_usec;
                        if (usec > 1000000) {
                                sec = usec / 1000000;
                                usec = usec % 1000000;
                        }
                        
                        timeout.tv_sec = now.tv_sec + sec;
                        timeout.tv_nsec = usec * 1000;

                        /*
                         **  Wait for a semaphore message for the current task or
                         **  for the timeout to expire.  The loop is required since
                         **  the task may be awakened by signals for semaphore
                         **  tokens which are not ours, or for signals other than
                         **  from a semaphore token being returned.
                         */
                        while ((waiting_on_sem(semaphore, &timeout, &retcode)) && (retcode != ETIMEDOUT)) {
#if !FIX_PTHREAD_COND_BUG
                                TRACEF("pthread_cond_timedwait {");
                                retcode = pthread_cond_timedwait(&(semaphore->sem_send), &(semaphore->sem_lock), 
                                    &timeout);
#endif
                                TRACEF("pthread_cond_timedwait }");
            
                if ( time_expired( &timeout ) )
                {
                    retcode = ETIMEDOUT;
                    break;
                }
                else
                {
                    retcode = 0;
                }
                
                        pthread_mutex_unlock(&(semaphore->sem_lock));
                        OS_mDelay(10);
                        pthread_mutex_lock(&(semaphore->sem_lock));
                        }
                }
                if (retcode == ETIMEDOUT) {
                        error = S_objLib_OBJ_TIMEOUT;
                        TRACEF("...timed out");
                }
        }

        /*
         **  Remove the calling task's task from the waiting task list
         **  for the semaphore.  Clear our TCB's suspend list pointer in
         **  case the semaphore was killed & its ctrl blk deallocated.
         */
        if (our_task) {
                TRACEF("%x", (int)our_task);
                unlink_susp_task(&semaphore->first_susp, our_task);
                //our_task->suspend_list = NULL;
        }

        //  See if we were awakened due to a semDelete or a semFlush.
        if ((semaphore->send_type & KILLD) || (semaphore->send_type & FLUSH)) {
                if (semaphore->send_type & KILLD) {
                        error = S_objLib_OBJ_ID_ERROR;  // Semaphore deleted 
                        TRACEF("...semaphore deleted");
                } else {
                        TRACEF("...semaphore flushed");
                }

                if ( ! semaphore->first_susp ) {
                        //pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, &semaphore->smdel_lock);
                        pthread_mutex_lock(&semaphore->smdel_lock);
                        pthread_cond_broadcast(&semaphore->smdel_cplt);
                        semaphore->send_type = SEND;
                        //pthread_cleanup_pop(1);
                }
                return error;
        } 

        if ( retcode == OK) {
                /*
                 **  Just received a token from the semaphore...
                 **  If the semaphore is a mutex, indicate the mutex is now owned
                 **  by the current task, and then see if the task owning the
                 **  token is to be made deletion-safe.  
                 */
                semaphore->current_owner = our_task->taskId;
                if ((semaphore->flags & SEM_TYPE_MASK) == MUTEX_SEMA4) {
                        semaphore->current_owner = our_task->taskId;
                        semaphore->recursion_level++; // is it a good place?
#if 0
                        if (semaphore->flags & SEM_DELETE_SAFE)
                                taskSafe();
#endif
                }
                TRACEF("OK");
        }

        return error;
}

/*****************************************************************************
** semTake - blocks the calling task until a token is available on the
**           specified v2pthread semaphore.
*****************************************************************************/
STATUS semTake(v2lsem_t * semaphore, int max_wait)
{
        DS_TASK_T *our_task;
        STATUS error = OK;
        TRACEF();
        SysASSERT( semaphore );
        
        //pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, &semaphore->sem_lock);
        if (!sem_find_lock(semaphore)) {
                TRACEF("WARNING not found %x ", (int)semaphore);
                error = S_objLib_OBJ_ID_ERROR;
                goto exit;
        }
        
        //TRACEF("%i %x %i %i",max_wait, semaphore,semaphore->recursion_level, semaphore->token_count);
        // If the semaphore is a mutex, check to see if this task already owns the token.
        our_task = taskFind(0, 1);
        if ( our_task ) our_task->waiting = (DS_U32 *)semaphore;
        SysASSERT( our_task );
        
        if (((semaphore->flags & SEM_TYPE_MASK) == MUTEX_SEMA4) &&
                        (semaphore->current_owner == our_task->taskId)) {
                // TODO: why not just use pthread_mutex_lock with PTHREAD_MUTEX_RECURSIVE mutex ?
                // because of timeout? -> pthread_cond_timedwait

                //  Current task already owns the mutex... 
                // simply increment the  ownership recursion level and return.
                semaphore->recursion_level++; // is i a good place?
                //TRACEF("... recursion level = %d", semaphore->recursion_level);
        } else {
                // Either semaphore is not a mutex or current task doesn't own it
                // Wait for timeout or acquisition of token
                error = wait_for_token(semaphore, max_wait, our_task);
        }
        if ( our_task ) our_task->waiting = (DS_U32 *)NULL;
        if ( error == OK ) {
                //++semaphore->recursion_level; // alternate place
                // TRACEF("++rl  = %i", semaphore->recursion_level);
        }
        pthread_mutex_unlock(&(semaphore->sem_lock));
        TRACEF("%x %i %i", (int)semaphore, (int)semaphore->recursion_level, (int)semaphore->token_count);
exit:
        {}
        //pthread_cleanup_pop(0);

//      if (error != OK) {
//              errno = (int) error;
//              error = ERROR;
//      }
        return error;
}