vqueue.c

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

            */
            if ( error == ERR_NO_ERROR )
            {
                link_qcb( queue );
            }
            else
            {
                /*
                **  Oops!  Problem somewhere above.  Release control block
                **  and data memory and return.
                */
                ts_free( (void *)queue->first_msg_in_queue );
                ts_free( (void *)queue );
            }
        }
        else
        {
            /*
            **  No memory for queue data... free queue control block & return
            */
            ts_free( (void *)queue );
            error = ERR_NOMGB;
        }
    }
    else
    {
        error = ERR_NOQCB;
    }

    return( error );
}

/*****************************************************************************
** q_vurgent - sends a message to the front of a p2pthread queue and awakens the
**           highest priority task pended on the queue.
*****************************************************************************/
ULONG
   q_vurgent( ULONG qid, void *msgbuf, ULONG msglen )
{
#ifdef DIAG_PRINTFS 
    p2pthread_cb_t *our_tcb;
#endif
    p2pt_vqueue_t *queue;
    ULONG error;

    error = ERR_NO_ERROR;

    if ( (queue = qcb_for( qid )) != (p2pt_vqueue_t *)NULL )
    {
        /*
        **  Return with error if caller's message is larger than max message
        **  size specified for queue.
        */
        if ( msglen > queue->msg_len )
        {
           error = ERR_MSGSIZ;
           return( error );
        }

#ifdef DIAG_PRINTFS 
        our_tcb = my_tcb();
        printf( "\r\ntask @ %p urgent send to queue list @ %p", our_tcb,
                &(queue->first_susp) );
#endif

        /*
        **  'Lock the p2pthread scheduler' to defer any context switch to a
        **  higher priority task until after this call has completed its work.
        */
        sched_lock();

        /*
        ** Lock mutex for queue send
        */
        pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                              (void *)&(queue->queue_lock));
        pthread_mutex_lock( &(queue->queue_lock) );

        /*
        **  See how many messages are already sent into the queue
        */
        if ( queue->msg_count > queue->msgs_per_queue )
        {
            /*
            **  Queue is already full... return QUEUE FULL error
            */
            error = ERR_QFULL;
        }
        else
        {
            /*
            **  Stuff the new message onto the front of the queue.
            */
            urgent_msg_to( queue, (char *)msgbuf, msglen );

            /*
            **  Signal the condition variable for the queue
            */
            pthread_cond_broadcast( &(queue->queue_send) );
        }

        /*
        **  Unlock the queue mutex. 
        */
        pthread_mutex_unlock( &(queue->queue_lock) );
        pthread_cleanup_pop( 0 );

        /*
        **  'Unlock the p2pthread scheduler' to enable a possible context switch
        **  to a task made runnable by this call.
        */
        sched_unlock();
    }
    else
    {
        error = ERR_OBJDEL;
    }

    return( error );
}

/*****************************************************************************
** q_vsend - posts a message to the tail of a p2pthread queue and awakens the
**           highest priority task pended on the queue.
*****************************************************************************/
ULONG
   q_vsend( ULONG qid, void *msgbuf, ULONG msglen )
{
#ifdef DIAG_PRINTFS 
    p2pthread_cb_t *our_tcb;
#endif
    p2pt_vqueue_t *queue;
    ULONG error;

    error = ERR_NO_ERROR;

    if ( (queue = qcb_for( qid )) != (p2pt_vqueue_t *)NULL )
    {
        /*
        **  Return with error if caller's message is larger than max message
        **  size specified for queue.
        */
        if ( msglen > queue->msg_len )
        {
           error = ERR_MSGSIZ;
           return( error );
        }

#ifdef DIAG_PRINTFS 
        our_tcb = my_tcb();
        printf( "\r\ntask @ %p send to queue list @ %p", our_tcb,
                &(queue->first_susp) );
#endif

        /*
        **  'Lock the p2pthread scheduler' to defer any context switch to a
        **  higher priority task until after this call has completed its work.
        */
        sched_lock();

        /*
        ** Lock mutex for queue send
        */
        pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                              (void *)&(queue->queue_lock));
        pthread_mutex_lock( &(queue->queue_lock) );

        /*
        **  See how many messages are already sent into the queue
        */
        if ( queue->msg_count > queue->msgs_per_queue )
        {
            /*
            **  Queue is already full... return QUEUE FULL error
            */
            error = ERR_QFULL;
        }
        else
        {
            if ( queue->msg_count == queue->msgs_per_queue )
            {
                if ( (queue->msgs_per_queue == 0) &&
                     (queue->first_susp != (p2pthread_cb_t *)NULL) )
                {
                    /*
                    **  Special case... Send the new message.
                    */
                    send_msg_to( queue, (char *)msgbuf, msglen );
                }
                else
                {
                    /*
                    **  Queue is already full... return QUEUE FULL error
                    */
                    error = ERR_QFULL;
                }
            }
            else
            {
                /*
                **  Send the new message.
                */
                send_msg_to( queue, (char *)msgbuf, msglen );
            }
        }

        /*
        **  Unlock the queue mutex. 
        */
        pthread_mutex_unlock( &(queue->queue_lock) );
        pthread_cleanup_pop( 0 );

        /*
        **  'Unlock the p2pthread scheduler' to enable a possible context switch
        **  to a task made runnable by this call.
        */
        sched_unlock();
    }
    else
    {
        error = ERR_OBJDEL;
    }

    return( error );
}

/*****************************************************************************
** q_vbroadcast - sends the specified message to all tasks pending on the
**               specified p2pthread queue and awakens the tasks.
*****************************************************************************/
ULONG
   q_vbroadcast( ULONG qid, void *msgbuf, ULONG msglen, ULONG *tasks )
{
    p2pt_vqueue_t *queue;
    ULONG error;

    error = ERR_NO_ERROR;

    if ( (queue = qcb_for( qid )) != (p2pt_vqueue_t *)NULL )
    {
        /*
        ** Lock mutex for urgent queue send
        */
        pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                              (void *)&(queue->queue_lock));
        pthread_mutex_lock( &(queue->queue_lock) );

        if ( queue->first_susp != (p2pthread_cb_t *)NULL )
        {
            /*
            **  See how many messages are already sent into the queue
            */
            if ( queue->msg_count > queue->msgs_per_queue )
            {
                /*
                **  Queue is already full... return QUEUE FULL error
                */
                error = ERR_QFULL;
            }
            else
            {
                /*
                **  Stuff the new message onto the front of the queue.
                */
                urgent_msg_to( queue, (char *)msgbuf, msglen );

                /*
                **  Declare the send type
                */
                queue->send_type = BCAST;
                queue->bcst_tasks_awakened = 0;
            }
        }

        /*
        **  Unlock the queue mutex. 
        */
        pthread_mutex_unlock( &(queue->queue_lock) );
        pthread_cleanup_pop( 0 );

        /*
        **  Send msg and block while any tasks are still pended on the queue
        */
        sched_lock();
        if ( (error == ERR_NO_ERROR) &&
             (queue->first_susp != (p2pthread_cb_t *)NULL) )
        {
            /*
            ** Lock mutex for queue broadcast completion
            */
            pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                                  (void *)&(queue->qbcst_lock) );
            pthread_mutex_lock( &(queue->qbcst_lock) );

            /*
            ** Lock mutex for urgent queue send
            */
            pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                                  (void *)&(queue->queue_lock));
            pthread_mutex_lock( &(queue->queue_lock) );

            /*
            **  Signal the condition variable for the queue
            */
            pthread_cond_broadcast( &(queue->queue_send) );

            /*
            **  Unlock the queue mutex. 
            */
            pthread_mutex_unlock( &(queue->queue_lock) );
            pthread_cleanup_pop( 0 );

            /*
            **  Wait for all pended tasks to receive broadcast message.
            **  The last task to receive the message will signal the
            **  broadcast-complete condition variable.
            */
            while ( queue->first_susp != (p2pthread_cb_t *)NULL )
            {
                pthread_cond_wait( &(queue->qbcst_cmplt),
                                   &(queue->qbcst_lock) );
            }

            /*
            **  Unlock the queue broadcast completion mutex. 
            */
            pthread_mutex_unlock( &(queue->qbcst_lock) );
            pthread_cleanup_pop( 0 );

            if ( tasks != (ULONG *)NULL )
                *tasks = queue->bcst_tasks_awakened;
        }

        *tasks = queue->bcst_tasks_awakened;

        sched_unlock();
    }
    else
    {
        error = ERR_OBJDEL;
    }

    return( error );
}

/*****************************************************************************
** delete_vqueue - takes care of destroying the specified queue and freeing
**                any resources allocated for that queue
*****************************************************************************/
static void
   delete_vqueue( p2pt_vqueue_t *queue )
{
    /*
    **  First remove the queue from the queue list
    */
    unlink_qcb( queue->qid );

    /*
    **  Next delete extent allocated for queue data.
    */
    ts_free( (void *)queue->first_msg_in_queue );

    /*
    **  Finally delete the queue control block itself;
    */
    ts_free( (void *)queue );

}

/*****************************************************************************
** q_vdelete - removes the specified queue from the queue list and frees
**              the memory allocated for the queue control block and extents.
*****************************************************************************/
ULONG
   q_vdelete( ULONG qid )
{
    p2pt_vqueue_t *queue;
    ULONG error;
    static char deleted_msg[16] = "Queue Deleted!\n";

    error = ERR_NO_ERROR;

    if ( (queue = qcb_for( qid )) != (p2pt_vqueue_t *)NULL )
    {
        /*
        ** Lock mutex for queue delete
        */
        pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                              (void *)&(queue->queue_lock));
        pthread_mutex_lock( &(queue->queue_lock) );

        if ( queue->msg_count )
            error = ERR_MATQDEL;

        if ( queue->first_susp != (p2pthread_cb_t *)NULL )
        {
            /*
            **  Stuff the new message onto the front of the queue.
            **  Overwrite any message previously there.
            */
            urgent_msg_to( queue, deleted_msg, 16 );

            /*
            **  Declare the send type
            */
            queue->send_type = KILLD;

            error = ERR_TATQDEL;
        }

        /*
        **  Unlock the queue mutex. 
        */
        pthread_mutex_unlock( &(queue->queue_lock) );
        pthread_cleanup_pop( 0 );

        /*