task.c

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

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

#include <errno.h>
#include <unistd.h>
#include <sched.h>
#include <sys/mman.h>
#include <stdlib.h>
#include <stdio.h>
#include <signal.h>
#include <sys/time.h>
#include <string.h>
#include "p2pthread.h"

#undef DIAG_PRINTFS

#define T_NOPREEMPT  0x01
#define T_TSLICE     0x02

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

#define ERR_PRIOR    0x11
#define ERR_ACTIVE   0x12
#define ERR_SUSP     0x14
#define ERR_NOTSUSP  0x15
#define ERR_REGNUM   0x17

/*
**  user_sysroot is a user-defined function.  It contains all initialization
**               calls to create any tasks and other objects reqired for
**               startup of the user's RTOS system environment.  It is called
**               from (and runs in) the system initialization pthread context.
**               It may optionally wait for some condition, shut down the
**               user's RTOS system environment, clean up the resources used
**               by the various RTOS objects, and return to the initialization
**               pthread.  The system initialization pthread will then
**               terminate, as will the parent process.
*/
// extern void user_sysroot( void );

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

/*
**  task_list is a linked list of pthread task control blocks.
**            It is used to perform en-masse operations on all p2pthread
**            tasks at once.
*/
static p2pthread_cb_t *
    task_list = (p2pthread_cb_t *)NULL;

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

/*
**  p2pt_sched_lock is a mutex used to make sched_lock exclusive to one thread
**                  at a time.
*/
pthread_mutex_t
    p2pt_sched_lock = PTHREAD_MUTEX_INITIALIZER;

/*
**  scheduler_locked contains the pthread ID of the thread which currently
**                   has the scheduler locked (or NULL if it is unlocked).
*/
static pthread_t
    scheduler_locked = (pthread_t)NULL;

/*
**  sched_lock_level tracks recursive nesting levels of sched_lock/unlock calls
**                   so the scheduler is only unlocked at the outermost
**                   sched_unlock call.
*/
static unsigned long
    sched_lock_level = 0;

/*
**  sched_lock_change is a condition variable which signals a change from
**                    locked to unlocked or vice-versa.
*/
static pthread_cond_t
    sched_lock_change = PTHREAD_COND_INITIALIZER;

/*****************************************************************************
**  thread-safe malloc
*****************************************************************************/
void *ts_malloc( size_t blksize )
{
    void *blkaddr;
    static pthread_mutex_t
        malloc_lock = PTHREAD_MUTEX_INITIALIZER;

    pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                          (void *)&malloc_lock );
    pthread_mutex_lock( &malloc_lock );

    blkaddr = malloc( blksize );

    pthread_mutex_unlock( &malloc_lock );
    pthread_cleanup_pop( 0 );

    return( blkaddr );
}
    
/*****************************************************************************
**  thread-safe free
*****************************************************************************/
void ts_free( void *blkaddr )

{
    static pthread_mutex_t
        free_lock = PTHREAD_MUTEX_INITIALIZER;

    pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                          (void *)&free_lock );
    pthread_mutex_lock( &free_lock );

    free( blkaddr );

    pthread_mutex_unlock( &free_lock );
    pthread_cleanup_pop( 0 );
}
    
/*****************************************************************************
**  my_tcb - returns a pointer to the task control block for the calling task
*****************************************************************************/
p2pthread_cb_t *
   my_tcb( void )
{
    pthread_t my_pthrid;
    p2pthread_cb_t *current_tcb;

    /*
    **  Get caller's pthread ID
    */
    my_pthrid = pthread_self();

    /*
    **  If the task_list contains tasks, scan it for the tcb
    **  whose thread id matches the one to be deleted.  No locking
    **  of the task list is done here since the access is read-only.
    **  NOTE that a tcb being appended to the task_list MUST have its
    **  nxt_task member initialized to NULL before being linked into
    **  the list. 
    */
    if ( task_list != (p2pthread_cb_t *)NULL )
    {
        for ( current_tcb = task_list;
              current_tcb != (p2pthread_cb_t *)NULL;
              current_tcb = current_tcb->nxt_task )
        {
            if ( my_pthrid == current_tcb->pthrid )
            {
                /*
                **  Found the task control_block.
                */
                return( current_tcb );
            }
        }
    }

    /*
    **  No matching task found... return NULL
    */
    return( (p2pthread_cb_t *)NULL );
}

/*****************************************************************************
** tcb_for - returns the address of the task control block for the task
**           idenified by taskid
*****************************************************************************/
p2pthread_cb_t *
   tcb_for( ULONG taskid )
{
    p2pthread_cb_t *current_tcb;
    int found_taskid;

        if ( task_list != (p2pthread_cb_t *)NULL )
        {
            /*
            **  One or more tasks already exist in the task list...
            **  Scan the existing tasks for a matching ID.
            */
            found_taskid = FALSE;
            for ( current_tcb = task_list; 
                  current_tcb != (p2pthread_cb_t *)NULL;
                  current_tcb = current_tcb->nxt_task )
            {
                if ( current_tcb->taskid == taskid )
                {
                    found_taskid = TRUE;
                    break;
                }
            }
            if ( found_taskid == FALSE )
                /*
                **  No matching ID found
                */
                current_tcb = (p2pthread_cb_t *)NULL;
        }
        else
            current_tcb = (p2pthread_cb_t *)NULL;
 
    return( current_tcb );
}

/*****************************************************************************
** sched_lock - 'locks the scheduler' to prevent preemption of the current task
**           by other task-level code.  Because we cannot actually lock the
**           scheduler in a pthreads environment, we temporarily set the
**           dynamic priority of the calling thread above that of any other
**           thread, thus guaranteeing that no other tasks preempt it.
*****************************************************************************/
void
   sched_lock( void )
{
    pthread_t my_pthrid;
    p2pthread_cb_t *tcb;
    int max_priority, sched_policy, got_lock;

    /*
    **  p2pt_sched_lock ensures that only one p2pthread pthread at a time gets
    **  to run at max_priority (effectively locking out all other p2pthread
    **  pthreads).  Due to the semantics of the pthread_cleanup push/pop
    **  pairs (which protect against deadlocks in the event a thread gets
    **  terminated while holding the mutex lock), we cannot safely leave
    **  the mutex itself locked until sched_unlock() is called.  Therefore,
    **  we instead use the mutex to provide 'atomic access' to a global
    **  flag indicating if the scheduler is currently locked.  We will
    **  'spin' and briefly suspend until the scheduler is unlocked, and
    **  will then lock it ourselves before proceeding.
    */
    got_lock = FALSE;
    my_pthrid = pthread_self();

    /*
    **  'Spin' here until scheduler_locked == NULL or our pthread ID
    **  This effectively prevents more than one pthread at a time from
    **  setting its priority to max_priority.
    */
    do {
        /*
        **  The pthread_cleanup_push/pop pair ensure the mutex will be
        **  unlocked if the calling thread gets killed within this loop.
        */
        pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                              (void *)&p2pt_sched_lock );
        /*
        **  The mutex lock/unlock guarantees 'atomic' access to the
        **  scheduler_locked flag.  Locking via pthread ID allows recursive
        **  locking by the same pthread while excluding all other pthreads.
        */
        pthread_mutex_lock( &p2pt_sched_lock );
        if ( (scheduler_locked == (pthread_t)NULL) ||
             (scheduler_locked == my_pthrid) )
        {
            scheduler_locked = my_pthrid;
            sched_lock_level++;
			/* 
			**  Note: since the type of 'sched_lock_level is defined as
			**	'unsigned', so the condition below is only occur when 
			**  sched_lock_level exceed it's maxium value. It's a bug 
			**  although it is right in most of time.
            */
            if ( sched_lock_level == 0L )
                sched_lock_level--;
            got_lock = TRUE;
			/* 
			**  Note: i think maybe the statement below is useless.
			*/
			pthread_cond_broadcast( &sched_lock_change );
#ifdef DIAG_PRINTFS 
            printf( "\r\nsched_lock sched_lock_level %lu locking tid %ld",
                sched_lock_level,  scheduler_locked );
#endif
        }
        else
        {
#ifdef DIAG_PRINTFS 
            printf( "\r\nsched_lock locking tid %ld my tid %ld",
                    scheduler_locked, my_pthrid );
#endif
            pthread_cond_wait( &sched_lock_change, &p2pt_sched_lock );
        }
        pthread_mutex_unlock( &p2pt_sched_lock );

        /*
        **  Add a cancellation point to this loop, since there are no others.
        */
        pthread_testcancel();
        pthread_cleanup_pop( 0 );
    } while ( got_lock == FALSE );

    /*
    **  task_list_lock prevents other p2pthread pthreads from modifying
    **  the p2pthread pthread task list while we're searching it and modifying
    **  the calling task's priority level.
    */
    pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                          (void *)&task_list_lock );
    pthread_mutex_lock( &task_list_lock );
    tcb = my_tcb();
    if ( tcb != (p2pthread_cb_t *)NULL )
    {
        pthread_attr_getschedpolicy( &(tcb->attr), &sched_policy );
        max_priority = sched_get_priority_max( sched_policy );
        ((tcb->attr).__schedparam).sched_priority = max_priority;
		/*
		** Note: here we set the priority for the given pthread.
		*/
        pthread_setschedparam( tcb->pthrid, sched_policy,
                          (struct sched_param *)&((tcb->attr).__schedparam) );
    }
    pthread_mutex_unlock( &task_list_lock );
    pthread_cleanup_pop( 0 );
}

/*****************************************************************************
** sched_unlock - 'unlocks the scheduler' to allow preemption of the current
**             task by other task-level code.  Because we cannot actually lock
**             the scheduler in a pthreads environment, the dynamic priority of
**             the calling thread was temporarily raised above that of any
**             other thread.  Therefore, we now restore the priority of the
**             calling thread to its original value to 'unlock' the task
**             scheduler.
*****************************************************************************/
void
   sched_unlock( void )
{
    p2pthread_cb_t *tcb;
    int sched_policy;

    /*
    **  scheduler_locked ensures that only one p2pthread pthread at a time gets
    **  to run at max_priority (effectively locking out all other p2pthread
    **  pthreads).  Unlock it here to complete 'unlocking' of the scheduler.
    */
    pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                          (void *)&p2pt_sched_lock );
    pthread_mutex_lock( &p2pt_sched_lock );

    if ( scheduler_locked == pthread_self() )
    {
        if ( sched_lock_level > 0L )
            sched_lock_level--;
        if ( sched_lock_level < 1L )
        {
            /*
            **  task_list_lock prevents other p2pthread pthreads from modifying
            **  the p2pthread pthread task list while we're searching it and
            **  modifying the calling task's priority level.
            */
            pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                                  (void *)&task_list_lock );
            pthread_mutex_lock( &task_list_lock );
            tcb = my_tcb();
            if ( tcb != (p2pthread_cb_t *)NULL )
            {
                pthread_attr_getschedpolicy( &(tcb->attr), &sched_policy );
                ((tcb->attr).__schedparam).sched_priority = 
                                           tcb->prv_priority.sched_priority;
                pthread_setschedparam( tcb->pthrid, sched_policy,
                          (struct sched_param *)&((tcb->attr).__schedparam) );
            }
            pthread_mutex_unlock( &task_list_lock );
            pthread_cleanup_pop( 0 );

            scheduler_locked = (pthread_t)NULL;
            pthread_cond_broadcast( &sched_lock_change );
        }
#ifdef DIAG_PRINTFS 
        printf( "\r\nsched_unlock sched_lock_level %lu locking tid %ld",
                sched_lock_level,  scheduler_locked );
#endif
    }
#ifdef DIAG_PRINTFS 
    else
        printf( "\r\nsched_unlock locking tid %ld my tid %ld", scheduler_locked,
                pthread_self() );
#endif

    pthread_mutex_unlock( &p2pt_sched_lock );
    pthread_cleanup_pop( 0 );
}

/*****************************************************************************
** link_susp_tcb - appends a new tcb pointer to a linked list of tcb pointers
**                 for tasks suspended on the object owning the list.
*****************************************************************************/
void
   link_susp_tcb( p2pthread_cb_t **list_head, p2pthread_cb_t *new_entry )
{
    p2pthread_cb_t *nxt_entry;

    if ( list_head != (p2pthread_cb_t **)NULL )
    {
        pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                              (void *)&task_list_lock );
        pthread_mutex_lock( &task_list_lock );
        new_entry->nxt_susp = (p2pthread_cb_t *)NULL;
        if ( *list_head != (p2pthread_cb_t *)NULL )
        {
            for ( nxt_entry = *list_head; 
                  nxt_entry->nxt_susp != (p2pthread_cb_t *)NULL;
                  nxt_entry = nxt_entry->nxt_susp ) ;
            nxt_entry->nxt_susp = new_entry;
#ifdef DIAG_PRINTFS 
            printf( "\r\nadd susp_tcb @ %p to list @ %p", new_entry,
                    nxt_entry );
#endif
        }
        else
        {
            *list_head = new_entry;