sema4.c

linux环境支持psos的操作系统。非常适合进行移植的朋友。

/*****************************************************************************
 * sema4.c - defines the wrapper functions and data structures needed
 *           to implement a Wind River pSOS+ (R) semaphore API 
 *           in a POSIX Threads environment.
 *           to implement a pSOS+ API in a POSIX Threadenvironment.
 ****************************************************************************/

#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 "p2pthread.h"

#undef DIAG_PRINTFS

#define SM_PRIOR     0x02
#define SM_NOWAIT    0x01

#define ERR_TIMEOUT  0x01
#define ERR_NODENO   0x04
#define ERR_OBJDEL   0x05
#define ERR_OBJTFULL 0x08
#define ERR_OBJNF    0x09

#define ERR_NOSCB    0x41
#define ERR_NOSEM    0x42
#define ERR_SKILLD   0x43
#define ERR_TATSDEL  0x44

#define SEND  0
#define KILLD 2

/*****************************************************************************
**  Control block for p2pthread 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 p2pthread semaphore.
**
*****************************************************************************/
typedef struct p2pt_sema4
{
        /*
        ** ID for semaphore
        */
    ULONG
        smid;

        /*
        ** Semaphore Name
        */
    char
        sname[4];

        /*
        ** Option Flags for semaphore
        */
    ULONG
        flags;

        /*
        ** Mutex and Condition variable for semaphore post/pend
        */
    pthread_mutex_t
        sema4_lock;
    pthread_cond_t
        sema4_send;

        /*
        ** Mutex and Condition variable for semaphore delete
        */
    pthread_mutex_t
        smdel_lock;
    pthread_cond_t
        smdel_cplt;

        /*
        **  Pthread semaphore used to maintain count.
        */
    sem_t
        pthread_sema4;

        /*
        ** Type of send operation last performed on semaphore
        */
    int
        send_type;

        /*
        **  Pointer to next semaphore control block in semaphore list.
        */
    struct p2pt_sema4 *
        nxt_sema4;

        /*
        ** First task control block in list of tasks waiting on semaphore
        */
    p2pthread_cb_t *
        first_susp;
} p2pt_sema4_t;

/*****************************************************************************
**  External function and data references
*****************************************************************************/
extern void *
    ts_malloc( size_t blksize );
extern void
    ts_free( void *blkaddr );
extern p2pthread_cb_t *
   my_tcb( void );
extern void
   sched_lock( void );
extern void
   sched_unlock( void );
extern ULONG
   tm_wkafter( ULONG interval );
extern void
   link_susp_tcb( p2pthread_cb_t **list_head, p2pthread_cb_t *new_entry );
extern void
   unlink_susp_tcb( p2pthread_cb_t **list_head, p2pthread_cb_t *entry );
extern int
   signal_for_my_task( p2pthread_cb_t **list_head, int pend_order );

/*****************************************************************************
**  p2pthread Global Data Structures
*****************************************************************************/

/*
**  sema4_list is a linked list of semaphore control blocks.  It is used to locate
**             semaphores by their ID numbers.
*/
static p2pt_sema4_t *
    sema4_list;

/*
**  sema4_list_lock is a mutex used to serialize access to the semaphore list
*/
static pthread_mutex_t
    sema4_list_lock = PTHREAD_MUTEX_INITIALIZER;


/*****************************************************************************
** smcb_for - returns the address of the semaphore control block for the semaphore
**            idenified by smid
*****************************************************************************/
static p2pt_sema4_t *
   smcb_for( ULONG smid )
{
    p2pt_sema4_t *current_smcb;
    int found_smid;

        if ( sema4_list != (p2pt_sema4_t *)NULL )
        {
            /*
            **  One or more semaphores already exist in the semaphore list...
            **  Scan the existing semaphores for a matching ID.
            */
            found_smid = FALSE;
            for ( current_smcb = sema4_list; 
                  current_smcb != (p2pt_sema4_t *)NULL;
                  current_smcb = current_smcb->nxt_sema4 )
            {
                if ( current_smcb->smid == smid )
                {
                    found_smid = TRUE;
                    break;
                }
            }
            if ( found_smid == FALSE )
                /*
                **  No matching ID found
                */
                current_smcb = (p2pt_sema4_t *)NULL;
        }
        else
            current_smcb = (p2pt_sema4_t *)NULL;
 
    return( current_smcb );
}


/*****************************************************************************
** new_smid - automatically returns a valid, unused semaphore ID
*****************************************************************************/
static ULONG
   new_smid( void )
{
    p2pt_sema4_t *current_smcb;
    ULONG new_sema4_id;

    /*
    **  Protect the semaphore list while we examine it.
    */
    pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                          (void *)&sema4_list_lock );
    pthread_mutex_lock( &sema4_list_lock );

    /*
    **  Get the highest previously assigned semaphore id and add one.
    */
    if ( sema4_list != (p2pt_sema4_t *)NULL )
    {
        /*
        **  One or more semaphores already exist in the semaphore list...
        **  Find the highest semaphore ID number in the existing list.
        */
        new_sema4_id = sema4_list->smid;
        for ( current_smcb = sema4_list; 
              current_smcb->nxt_sema4 != (p2pt_sema4_t *)NULL;
              current_smcb = current_smcb->nxt_sema4 )
        {
            if ( (current_smcb->nxt_sema4)->smid > new_sema4_id )
            {
                new_sema4_id = (current_smcb->nxt_sema4)->smid;
            }
        }

        /*
        **  Add one to the highest existing semaphore ID
        */
        new_sema4_id++;
    }
    else
    {
        /*
        **  this is the first semaphore being added to the semaphore list.
        */
        new_sema4_id = 1;
    }
 
    /*
    **  Re-enable access to the semaphore list by other threads.
    */
    pthread_mutex_unlock( &sema4_list_lock );
    pthread_cleanup_pop( 0 );

    return( new_sema4_id );
}

/*****************************************************************************
** link_smcb - appends a new semaphore control block pointer to the sema4_list
*****************************************************************************/
static void
   link_smcb( p2pt_sema4_t *new_sema4 )
{
    p2pt_sema4_t *current_smcb;

    /*
    **  Protect the semaphore list while we examine and modify it.
    */
    pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                          (void *)&sema4_list_lock );
    pthread_mutex_lock( &sema4_list_lock );

    new_sema4->nxt_sema4 = (p2pt_sema4_t *)NULL;
    if ( sema4_list != (p2pt_sema4_t *)NULL )
    {
        /*
        **  One or more semaphores already exist in the semaphore list...
        **  Insert the new entry in ascending numerical sequence by smid.
        */
        for ( current_smcb = sema4_list; 
              current_smcb->nxt_sema4 != (p2pt_sema4_t *)NULL;
              current_smcb = current_smcb->nxt_sema4 )
        {
            if ( (current_smcb->nxt_sema4)->smid > new_sema4->smid )
            {
                new_sema4->nxt_sema4 = current_smcb->nxt_sema4;
                break;
            }
        }
        current_smcb->nxt_sema4 = new_sema4;
#ifdef DIAG_PRINTFS 
        printf( "\r\nadd semaphore cb @ %p to list @ %p", new_sema4,
                current_smcb );
#endif
    }
    else
    {
        /*
        **  this is the first semaphore being added to the semaphore list.
        */
        sema4_list = new_sema4;
#ifdef DIAG_PRINTFS 
        printf( "\r\nadd semaphore cb @ %p to list @ %p", new_sema4,
                &sema4_list );
#endif
    }
 
    /*
    **  Re-enable access to the semaphore list by other threads.
    */
    pthread_mutex_unlock( &sema4_list_lock );
    pthread_cleanup_pop( 0 );
}

/*****************************************************************************
** unlink_smcb - removes a semaphore control block pointer from the sema4_list
*****************************************************************************/
static p2pt_sema4_t *
   unlink_smcb( ULONG smid )
{
    p2pt_sema4_t *current_smcb;
    p2pt_sema4_t *selected_smcb;

    selected_smcb =  (p2pt_sema4_t *)NULL;

    if ( sema4_list != (p2pt_sema4_t *)NULL )
    {
        /*
        **  One or more semaphores exist in the semaphore list...
        **  Protect the semaphore list while we examine and modify it.
        */
        pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                              (void *)&sema4_list_lock );
        pthread_mutex_lock( &sema4_list_lock );

        /*
        **  Scan the semaphore list for an smcb with a matching semaphore ID
        */
        if ( sema4_list->smid == smid )
        {
            /*
            **  The first semaphore in the list matches the selected ID
            */
            selected_smcb = sema4_list; 
            sema4_list = selected_smcb->nxt_sema4;
#ifdef DIAG_PRINTFS 
            printf( "\r\ndel semaphore cb @ %p from list @ %p", selected_smcb,
                    &sema4_list );