semlib.c

如何将基于vxworks的应用程序移植到LINUX操作系统。

**                        (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_sema4( vxwk_sema4_t *semaphore, struct timespec *timeout,
                      int *retcode )
{
    int result;
    struct timeval now;
    ulong usec;

    if ( (semaphore->send_type & KILLD) || (semaphore->send_type & FLUSH) )
    {
        /*
        **  Semaphore has been killed... waiting is over.
        */
        result = 0;
        *retcode = 0;
    }
    else
    {
        /*
        **  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->sema4_lock) );
                taskDelay( 1 );
                pthread_mutex_lock( &(semaphore->sema4_lock) );
            }

            /*
            **  If a timeout was specified, make sure we respect it and
            **  exit this loop if it expires.
            */
            if ( timeout != (struct timespec *)NULL )
            {
                gettimeofday( &now, (struct timezone *)NULL );
                if ( timeout->tv_nsec > (now.tv_usec * 1000) )
                {
                    usec = (timeout->tv_nsec - (now.tv_usec * 1000)) / 1000;
                    if ( timeout->tv_sec < now.tv_sec )
                        usec = 0;
                    else
                        usec += ((timeout->tv_sec - now.tv_sec) * 1000000);
                }
                else
                {
                    usec = ((timeout->tv_nsec + 1000000000) -
                            (now.tv_usec * 1000)) / 1000;
                    if ( (timeout->tv_sec - 1) < now.tv_sec )
                        usec = 0;
                    else
                        usec += (((timeout->tv_sec - 1) - now.tv_sec)
                                 * 1000000);
                }
                if ( usec == 0 )
                    break;
            }
        }
    }

    return( result );
}

/*****************************************************************************
** wait_for_token - blocks the calling task until a token is available on the
**                  specified VxWorks 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( vxwk_sema4_t *semaphore, int max_wait,
                   vxwk2pthread_cb_t *our_tcb )
{
    struct timeval now;
    struct timespec timeout;
    int retcode;
    long sec, usec;
    STATUS error;
    vxwk2pthread_cb_t *tcb;
    int my_priority, owners_priority, sched_policy;

    error = OK;

    /*
    **  Add tcb for task to list of tasks waiting on semaphore
    */
#ifdef DIAG_PRINTFS 
    printf( "\r\ntask @ %p wait on semaphore list @ %p", our_tcb,
            &(semaphore->first_susp) );
#endif

    link_susp_tcb( &(semaphore->first_susp), our_tcb );

    retcode = 0;

    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_sema4( semaphore, &timeout, &retcode )) &&
                (retcode != ETIMEDOUT) )
        {
            retcode = pthread_cond_timedwait( &(semaphore->sema4_send),
                                              &(semaphore->sema4_lock),
                                              &timeout );
        }
    }
    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 ( (semaphore->flags & SEM_INVERSION_SAFE) &&
             (semaphore->current_owner != (vxwk2pthread_cb_t *)NULL) )
        {
            /*
            **  Ensure against preemption by other tasks
            */
            taskLock();

            /*
            **  Get pthreads priority of current task
            */
            my_priority = our_tcb->prv_priority.sched_priority;

            /*
            **  Get pthreads priority of task which owns mutex
            */
            tcb = semaphore->current_owner;
            owners_priority = tcb->prv_priority.sched_priority;

#ifdef DIAG_PRINTFS 
            printf( "\r\nTask @ %p priority %d owns mutex", tcb,
                    owners_priority );
            printf( "\r\nCalling task @ %p priority %d wants mutex", our_tcb,
                    my_priority );
#endif
            /*
            **  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 )
            {
                pthread_attr_getschedpolicy( &(tcb->attr), &sched_policy );
                ((tcb->attr).__schedparam).sched_priority = my_priority;
                pthread_setschedparam( tcb->pthrid, sched_policy,
                          (struct sched_param *)&((tcb->attr).__schedparam) );
            }

            /*
            **  Re-enable preemption by other tasks
            */
            taskUnlock();
        }

        if ( max_wait == WAIT_FOREVER )
        {
            /*
            **  Infinite wait was specified... wait without timeout.
            */
            while ( waiting_on_sema4( semaphore, 0, &retcode ) )
            {
                pthread_cond_wait( &(semaphore->sema4_send),
                                   &(semaphore->sema4_lock) );
            }
        }
        else
        {
            /*
            **  Wait on semaphore message arrival with timeout...
            **  Calculate timeout delay in seconds and microseconds.
            */
            sec = 0;
            usec = max_wait * VXWK_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_sema4( semaphore, &timeout, &retcode )) &&
                    (retcode != ETIMEDOUT) )
            {
                retcode = pthread_cond_timedwait( &(semaphore->sema4_send),
                                                  &(semaphore->sema4_lock),
                                                  &timeout );
            }
        }
    }

    /*
    **  Remove the calling task's tcb 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.
    */
    unlink_susp_tcb( &(semaphore->first_susp), our_tcb );
    our_tcb->suspend_list = (vxwk2pthread_cb_t **)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 */

#ifdef DIAG_PRINTFS 
            printf( "...semaphore deleted" );
#endif
        }
        else
        {
#ifdef DIAG_PRINTFS 
            printf( "...semaphore flushed" );
#endif
        }

        if ( semaphore->first_susp == (vxwk2pthread_cb_t *)NULL )
        {
            /*
            ** Lock mutex for semaphore delete completion
            */
            pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                                  (void *)&(semaphore->smdel_lock) );
            pthread_mutex_lock( &(semaphore->smdel_lock) );

            /*
            **  Signal the delete-complete condition variable
            **  for the semaphore
            */
            pthread_cond_broadcast( &(semaphore->smdel_cplt) );

            semaphore->send_type = SEND;

            /*
            **  Unlock the semaphore delete completion mutex. 
            */
            pthread_cleanup_pop( 1 );
        }
    }
    else
    {
        /*
        **  See if we timed out or if we got a token
        */
        if ( retcode == ETIMEDOUT )
        {
            /*
            **  Timed out without a token
            */
            if ( max_wait == NO_WAIT )
            {
                error = S_objLib_OBJ_UNAVAILABLE;
#ifdef DIAG_PRINTFS 
                printf( "...no token available" );
#endif
            }
            else
            {
                error = S_objLib_OBJ_TIMEOUT;
#ifdef DIAG_PRINTFS 
                printf( "...timed out" );
#endif
            }
        }
        else
        {
            /*
            **  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.  
            */
            if ( (semaphore->flags & SEM_TYPE_MASK) == MUTEX_SEMA4 )
            {
                semaphore->current_owner = our_tcb;
                semaphore->recursion_level++;
                if ( semaphore->flags & SEM_DELETE_SAFE )
                    taskSafe();
            }

#ifdef DIAG_PRINTFS 
            printf( "...rcvd semaphore token" );
#endif
        }
    }

    return( error );
}

/*****************************************************************************
** semTake - blocks the calling task until a token is available on the
**           specified VxWorks semaphore.
*****************************************************************************/
STATUS
   semTake( vxwk_sema4_t *semaphore, int max_wait )
{
    vxwk2pthread_cb_t *our_tcb;
    STATUS error;

    error = OK;

    /*
    **  First ensure that the specified semaphore exists and that we have
    **  exclusive access to it.
    */
    pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                          (void *)&(semaphore->sema4_lock));
    if ( sema4_valid( semaphore ) )
    {
        /*
        **  If the semaphore is a mutex, check to see if this task already
        **  owns the token.
        */
        our_tcb = my_tcb();
        if ( ((semaphore->flags & SEM_TYPE_MASK) == MUTEX_SEMA4) &&
             (semaphore->current_owner == our_tcb) )
        {
            /*
            **  Current task already owns the mutex... simply increment the
            **  ownership recursion level and return.
            */
            semaphore->recursion_level++;
#ifdef DIAG_PRINTFS 
            printf( "... recursion level = %d", semaphore->recursion_level );
#endif
        }
        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_tcb );
        } 

        /*
        **  Unlock the mutex for the condition variable and clean up.
        */
        pthread_mutex_unlock( &(semaphore->sema4_lock) );
    }
    else
    {
        error = S_objLib_OBJ_ID_ERROR;       /* Invalid semaphore specified */
    }

    /*
    **  Clean up the opening pthread_cleanup_push()
    */
    pthread_cleanup_pop( 0 );

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

    return( error );
}