source: svn/trunk/zas_dstar/hal/os/src/v2lin/lmsgQLib.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.
 * 
 ****************************************************************************/

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

#define SEND  0
#define URGNT 1
#define KILLD 2

typedef struct q_msg
{
        uint msglen;
        char *msgbuf;
} q_msg_t;

/*****************************************************************************
**  Control block for v2pthread queue
**
**  The message list for a queue is organized into an array called an extent.
**  Actual send and fetch operations are done using a queue_head and
**  queue_tail pointer.  These pointers must 'rotate' through the extent to
**  create a logical circular buffer.  A single extra location is added
**  to ensure room for urgent messages even when the queue is 'full' for
**  normal messages.
**
*****************************************************************************/
typedef struct v2pt_mqueue
{
        // Mutex and Condition variable for queue send/pend
        pthread_mutex_t queue_lock;
        pthread_cond_t queue_send;

        // Mutex and Condition variable for queue delete
        pthread_mutex_t qdlet_lock;
        pthread_cond_t qdlet_cmplt;

        // Mutex and Condition variable for queue-full pend 
        pthread_mutex_t qfull_lock;
        pthread_cond_t queue_space;

        //  Pointer to next message pointer to be fetched from queue
        q_msg_t *queue_head;

        // Pointer to last message pointer sent to queue
        q_msg_t *queue_tail;

        int send_type; // Type of send operation last performed on queue

        q_msg_t *first_msg_in_queue;
        q_msg_t *last_msg_in_queue;

        struct v2pt_mqueue *nxt_queue;

        // First task control block in list of tasks waiting to receive a message from queue
        DS_TASK_T *first_susp;

        // First task control block in list of tasks waiting for space to post messages to queue
        DS_TASK_T *first_write_susp;

        int msg_count; // Total number of messages currently sent to queue
        int msgs_per_queue; // Total (max) messages per queue
        uint msg_len; // Maximum size of messages sent to queue
        size_t vmsg_len; // sizeof( each element in queue ) used for subscript incr/decr.
        int order; // Task pend order (FIFO or Priority) for queue
} v2pt_mqueue_t;

static v2pt_mqueue_t *mqueue_list;
static pthread_mutex_t mqueue_list_lock = PTHREAD_MUTEX_INITIALIZER;

int msgQShow(v2pt_mqueue_t * q , FILE * out, int mem)
{
        DS_TASK_T *t; 
        int i;
        
        fprintf(out,"%x num=%i len=%i: ", (int)q, (int)q->msgs_per_queue, (int)q->msg_len);
        fprintf(out,"readers: ");
        i=0;
        for ( t = q->first_susp; t; t = t->nxt_susp)   {
                fprintf(out,"%x %s ", (int)t, t->name);
                i++;
                if ( i > 10 )  break;
        }
        i=0;
        fprintf(out,"writers: ");
        for ( t = q->first_write_susp; t; t = t->nxt_susp)  {
                fprintf(out,"%x %s ", (int)t, t->name);
                i++;
                if ( i > 10 )  break;
        }
        if (mem) {
                int * w;
                for ( w = (int*)q; w < (int*) (q+1);w++) 
                        fprintf(out,"%x ",*w);
        }
        fprintf(out,"\n");
        q=q->nxt_queue;
        return 0;
}

int msgQList(FILE * out,int mem)
{
        TRACEF();
        int c=0;
        v2pt_mqueue_t * q = mqueue_list;
        while (q){
                c++;
                fprintf(out,"%i ",c);
                msgQShow(q,out,mem);
                q=q->nxt_queue;
        }
        return c;
}


/*****************************************************************************
**  mqueue_find_lock - verifies whether the specified queue still exists, and if
**                so, locks exclusive access to the queue for the caller.
*****************************************************************************/
static int mqueue_find_lock(v2pt_mqueue_t * queue)
{
        v2pt_mqueue_t *current_qcb;
        int found_queue;
        found_queue = FALSE;
        pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, (void *) &mqueue_list_lock);
        pthread_mutex_lock(&mqueue_list_lock);
        for (current_qcb = mqueue_list;
                        current_qcb != NULL; current_qcb = current_qcb->nxt_queue) {
                if (current_qcb == queue) {
                        /*
                         ** Lock mutex for queue 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(&queue->queue_lock);
                        found_queue = TRUE;
                        break;
                }
        }
        pthread_cleanup_pop(1);
        return found_queue;
}

/*****************************************************************************
** link_qcb - appends a new queue control block pointer to the mqueue_list
*****************************************************************************/
static void link_qcb(v2pt_mqueue_t * new_mqueue)
{
        v2pt_mqueue_t **i;

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

        i = & mqueue_list; 
        while (*i) i = & (*i)->nxt_queue; // find tail
        new_mqueue->nxt_queue = NULL;
        *i = new_mqueue;
        TRACEF("add queue cb @ %p ",new_mqueue);
        pthread_mutex_unlock(&mqueue_list_lock);
        pthread_cleanup_pop(0);
}

/*****************************************************************************
** unlink_qcb - removes a queue control block pointer from the mqueue_list
*****************************************************************************/
static v2pt_mqueue_t *unlink_qcb(v2pt_mqueue_t * qid)
{
        v2pt_mqueue_t **i = &mqueue_list;
        pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, (void *) &mqueue_list_lock);
        pthread_mutex_lock(&mqueue_list_lock);

        while (*i) {
                if ( *i == qid ) {
                        TRACEF("%p", qid);
                        *i = (*i)->nxt_queue;   // remove
                        break;
                }
                i = &((*i)->nxt_queue);
        }
        pthread_cleanup_pop(1);
        return qid;
}

/*****************************************************************************
** urgent_msg_to - sends a message to the front of the specified queue
*****************************************************************************/
static void urgent_msg_to(v2pt_mqueue_t * queue, char *msg, uint msglen)
{
        uint i;
        char *element;
        TRACEF();
        /*
         **  It is assumed when we enter this function that the queue has space
         **  to accept the message about to be sent. 
         **  Pre-decrement the queue_head (fetch) pointer, adjusting for
         **  possible wrap to the end of the queue;
         **  (Urgent messages are placed at the queue head so they will be the
         **  next message fetched from the queue - ahead of any
         **  previously-queued messages.)
         */
        element = (char *) queue->queue_head;
        element -= queue->vmsg_len;
        queue->queue_head = (q_msg_t *) element;

        if (queue->queue_head < queue->first_msg_in_queue) {
                /*
                 **  New queue_head pointer underflowed beginning of the extent...
                 **  Wrap the queue_head pointer to the last message address
                 **  in the extent allocated for the queue.
                 */
                queue->queue_head = queue->last_msg_in_queue;
        }
        TRACEF(" new queue_head @ %p", queue->queue_head);

        if (msg != (char *) NULL) {
                element = (char *) &((queue->queue_head)->msgbuf);
                for (i = 0; i < msglen; i++) {
                        *(element + i) = *(msg + i);
                }
        }
        (queue->queue_head)->msglen = msglen;

        TRACEF("nsent urgent msg %p len %x to queue_head @ %p", msg, msglen, queue->queue_head);
        queue->msg_count++;

        queue->send_type = URGNT;
}

/*****************************************************************************
** send_msg_to - sends the specified message to the tail of the specified queue
*****************************************************************************/
static void send_msg_to(v2pt_mqueue_t * queue, char *msg, uint msglen)
{
        uint i;
        char *element;
        TRACEF();
        /*
         **  It is assumed when we enter this function that the queue has space
         **  to accept the message about to be sent.  Start by sending the
         **  message.
         */
        if (msg != (char *) NULL) {
                element = (char *) &((queue->queue_tail)->msgbuf);
                for (i = 0; i < msglen; i++) {
                        *(element + i) = *(msg + i);
                }
        }
        queue->queue_tail->msglen = msglen;

        TRACEF("%x len %x to queue_tail @ %p", msg, msglen, queue->queue_tail);

        /*
         **  Now increment the queue_tail (send) pointer, adjusting for
         **  possible wrap to the beginning of the queue.
         */
        element = (char *) queue->queue_tail;
        element += queue->vmsg_len;
        queue->queue_tail = (q_msg_t *) element;

        if (queue->queue_tail > queue->last_msg_in_queue) {
                /*
                 **  Wrap the queue_tail pointer to the first message address
                 **  in the queue.
                 */
                queue->queue_tail = queue->first_msg_in_queue;
        }
        TRACEF(" new queue_tail @ %p", queue->queue_tail);

        queue->msg_count++;
        queue->send_type = SEND;
        
        if ( queue->first_susp )
            pthread_cond_broadcast(&(queue->queue_send));
}

/*****************************************************************************
** notify_if_delete_complete - indicates if all tasks waiting on specified
**                             queue have successfully been awakened. 
*****************************************************************************/
static void notify_if_delete_complete(v2pt_mqueue_t * queue)
{
        /*
         **  All tasks pending on the specified queue are being awakened...
         **  If the calling task was the last task pending on the queue,
         **  signal the deletion-complete condition variable.
         */
        if (( ! queue->first_susp ) && ( ! queue->first_write_susp )) {
                // Lock mutex for queue delete completion
                pthread_mutex_clean_lock(&(queue->qdlet_lock));

                // Signal the deletion-complete condition variable for the queue
                pthread_cond_broadcast(&queue->qdlet_cmplt);

                // Unlock the queue delete completion mutex. 
                pthread_mutex_unlock(&queue->qdlet_lock);
                pthread_cleanup_pop(0);
        }
}


/*****************************************************************************
** fetch_msg_from - fetches the next message from the specified queue
*****************************************************************************/
static uint fetch_msg_from(v2pt_mqueue_t * queue, char *msg)
{
        char *element;
        uint i;
        uint msglen;

        /*
         **  It is assumed when we enter this function that the queue contains
         **  one or more messages to be fetched.
         **  Fetch the message from the queue_head message location.
         */
        if (msg != (char *) NULL) {
                element = (char *) &((queue->queue_head)->msgbuf);
                msglen = (queue->queue_head)->msglen;
                for (i = 0; i < msglen; i++) {
                        *(msg + i) = *(element + i);
                }
        } else
                msglen = 0;

        TRACEF("fetched msg of len %x from queue_head @ %p", msglen, queue->queue_head);

        /*
         **  Clear the message from the queue
         */
        element = (char *) &((queue->queue_head)->msgbuf);
        *element = (char) 0;
        (queue->queue_head)->msglen = 0;

        /*
         **  Now increment the queue_head (send) pointer, adjusting for
         **  possible wrap to the beginning of the queue.
         */
        element = (char *) queue->queue_head;
        element += queue->vmsg_len;
        queue->queue_head = (q_msg_t *) element;

        if (queue->queue_head > queue->last_msg_in_queue) {
                /*
                 **  New queue_head pointer overflowed end of queue...
                 **  Wrap the queue_head pointer to the first message address
                 **  in the queue.
                 */
                queue->queue_head = queue->first_msg_in_queue;
        }
        TRACEF(" new queue_head @ %p", queue->queue_head);

        queue->msg_count--;

        /*
         **  Now see if adequate space was freed in the queue and alert any tasks
         **  waiting for message space if adequate space now exists.
         */
        if (queue->first_write_susp != (DS_TASK_T *) NULL) {
                if (queue->msg_count <= (queue->msgs_per_queue - 1)) {

                        TRACEF("\r\nqueue @ %p freed msg space for queue list @ %p",
                                   queue, &(queue->first_write_susp));
                        /*
                         **  Lock mutex for queue space
                         */
                        pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock,
                                                                 (void *) &(queue->qfull_lock));
                        pthread_mutex_lock(&(queue->qfull_lock));

                        /*
                         **  Alert the waiting tasks that message space is available.
                         */
                        pthread_cond_broadcast(&(queue->queue_space));

                        /*
                         **  Unlock the queue space mutex. 
                         */
                        pthread_cleanup_pop(1);
                }
        }
        return (msglen);
}

/*****************************************************************************
** data_extent_for - allocates space for queue data.  Data is allocated in
**                  a block large enough to hold (max_msgs + 1) messages.
*****************************************************************************/
static q_msg_t *data_extent_for(v2pt_mqueue_t * queue)
{
        char *new_extent;
        char *last_msg;
        size_t alloc_size;

        /*
         **  Calculate the number of bytes of memory needed for this extent.
         **  Start by calculating the size of each element of the extent array.
         **  Each (q_msg_t) element will contain an unsigned int byte length followed
         **  by a character array of queue->msg_len bytes.  First get the size
         **  of the q_msg_t 'header' excluding the start of the data array.
         **  Then add the size of the maximum-length message data.
         */
        queue->vmsg_len = sizeof(q_msg_t) - sizeof(char *);
        queue->vmsg_len += (sizeof(char) * queue->msg_len);

        /*
         **  The size of each array element is now known...
         **  Multiply it by the number of elements to get allocation size.
         */
        alloc_size = queue->vmsg_len * (queue->msgs_per_queue + 1);

        /*
         **  Now allocate a block of memory to contain the extent.
         */
        if ((new_extent = (char *) malloc(alloc_size)) != (char *) NULL) {
                /*
                 **  Clear the memory block.  Note that this creates a NULL pointer
                 **  for the nxt_extent link as well as zeroing the message array.
                 */
                bzero((void *) new_extent, (int) alloc_size);

                /*
                 **  Link new data extent into the queue control block
                 */
                last_msg = new_extent + (queue->vmsg_len * queue->msgs_per_queue);
                queue->first_msg_in_queue = (q_msg_t *) new_extent;
                queue->last_msg_in_queue = (q_msg_t *) last_msg;
        }
        TRACEF("new extent @ %p for queue @ %p vmsg_len %x", new_extent, queue, queue->vmsg_len);
        return ((q_msg_t *) new_extent);
}

/*****************************************************************************
** msgQCreate - creates a v2pthread message queue
*****************************************************************************/
MSG_Q_ID msgQCreate(int max_msgs, int msglen, int opt)
{
        v2pt_mqueue_t *queue;
        STATUS error;
        TRACEF("%i %i %x",max_msgs,msglen,opt);
        error = OK;
        queue = (v2pt_mqueue_t *) malloc(sizeof(v2pt_mqueue_t));
        if (!queue) {

                error = S_memLib_NOT_ENOUGH_MEMORY;
                goto exit;
        }
        memset(queue,0,sizeof(*queue));
        queue->msgs_per_queue = max_msgs;

        queue->msg_len = msglen;

        if ( data_extent_for(queue) ) {

                /*
                 ** Mutex and Condition variable for queue send/pend
                 */
                pthread_mutex_init(&(queue->queue_lock), (pthread_mutexattr_t *) NULL);
                pthread_cond_init(&(queue->queue_send), (pthread_condattr_t *) NULL);

                /*
                 ** Mutex and Condition variable for queue delete
                 */
                pthread_mutex_init(&(queue->qdlet_lock), (pthread_mutexattr_t *) NULL);
                pthread_cond_init(&(queue->qdlet_cmplt), (pthread_condattr_t *) NULL);

                /*
                 ** Mutex and Condition variable for queue-full pend
                 */
                pthread_mutex_init(&(queue->qfull_lock), (pthread_mutexattr_t *) NULL);
                pthread_cond_init(&(queue->queue_space), (pthread_condattr_t *) NULL);

                /*
                 ** Pointer to next message pointer to be fetched from queue
                 */
                queue->queue_head = queue->first_msg_in_queue;

                /*
                 ** Pointer to last message pointer sent to queue
                 */
                queue->queue_tail = queue->first_msg_in_queue;

                /*
                 ** Type of send operation last performed on queue
                 */
                queue->send_type = SEND;

                queue->first_susp = NULL;

                /*
                 ** First task control block in list of tasks waiting for space to
                 ** post messages to queue
                 */
                queue->first_write_susp = (DS_TASK_T *) NULL;

                /*
                 ** Total number of messages currently sent to queue
                 */
                queue->msg_count = 0;

                /*
                 ** Task pend order (FIFO or Priority) for queue
                 */
                if (opt & MSG_Q_PRIORITY)
                        queue->order = 1;
                else
                        queue->order = 0;

                /*
                 **  If no errors thus far, we have a new queue ready to link into
                 **  the queue list.
                 */
                if (error == OK) {
                        link_qcb(queue);
                } else {
                        /*
                         **  Oops!  Problem somewhere above.  Release control block
                         **  and data memory and return.
                         */
                        free((void *) queue->first_msg_in_queue);
                        free((void *) queue);
                }
        } else {
                /*
                 **  No memory for queue data... free queue control block & return
                 */
                free((void *) queue);
                error = S_memLib_NOT_ENOUGH_MEMORY;
        }
exit:
        if (error != OK) {
                errno = (int) error;
                queue = (v2pt_mqueue_t *) NULL;
        }

        return (MSG_Q_ID)queue;
}

/*****************************************************************************
** waiting_on_q_space - returns a nonzero result unless a qualifying event
**                      occurs on the specified queue which should cause the
**                      pended task to be awakened.  The qualifying events
**                      are:
**                          1. message space is freed in the queue and the 
**                              current task is selected to receive it
**                          2. the queue is deleted
*****************************************************************************/
static int waiting_on_q_space(v2pt_mqueue_t * queue, struct timespec *timeout, int *retcode)
{
        int result;
        struct timeval now;
        unsigned long usec;
        TRACEF();
        if (queue->send_type & KILLD) {
                result = 0;
                *retcode = 0;
        } else {
                /*
                 **  Queue still in service... check for message space availability.
                 **  Initially assume no message space available for our task
                 */
                result = 1;

                /*
                 **  Multiple messages removed from the queue may be represented by
                 **  only a single signal to the condition variable, so continue
                 **  checking for a message slot for our task as long as more space
                 **  is available.  Also note that for a 'zero-length' queue, the
                 **  presence of a task waiting on the queue for our message will
                 **  allow our message to be posted to the queue.
                 */
                while ((queue->msg_count <= (queue->msgs_per_queue - 1)) ||
                           ((queue->msgs_per_queue == 0) && queue->first_susp )) {
                        // Message slot available... see if it's for our task.
                        if (signal_for_my_task(&queue->first_write_susp, queue->order)) {
                                /*
                                 **  Message slot was destined for our task... waiting is over.
                                 */
                                result = 0;
                                *retcode = 0;
                                break;
                        } else {
                                /*
                                 **  Message slot isn't for our task... continue waiting.
                                 **  Sleep awhile to allow other tasks ahead of ours in the
                                 **  list of tasks waiting on the queue to get their
                                 **  messages, bringing our task to the head of the list.
                                 */
                                pthread_mutex_unlock(&(queue->qfull_lock));
                                OS_Delay(1);
                                pthread_mutex_lock(&(queue->qfull_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;
}

/*****************************************************************************
** waitToSend - sends the queue message if sufficient space becomes available
**              within the allotted waiting interval.
*****************************************************************************/
STATUS waitToSend(v2pt_mqueue_t * queue, char *msg, uint msglen, int wait, int pri)
{
        DS_TASK_T *our_task;
        struct timeval now;
        struct timespec timeout;
        int retcode;
        long sec, usec;
        STATUS error = OK;
        TRACEF();

        if (wait != NO_WAIT) {
                //  Add task for task to list of tasks waiting on queue
                our_task = taskFind(0, 1);
                TRACEV("%x", queue->first_write_susp);

                link_susp_task(&queue->first_write_susp, our_task);

                retcode = 0;

                //  Unlock the queue mutex so other tasks can receive messages. 
                pthread_mutex_unlock(&queue->queue_lock);

                if (wait == WAIT_FOREVER) {
                        while (waiting_on_q_space(queue, 0, &retcode)) {
                                pthread_cond_wait(&queue->queue_space, &queue->qfull_lock);
                        }
                } else {
                        /*
                         **  Wait on queue message space with timeout...
                         **  Calculate timeout delay in seconds and microseconds.
                         */
                        sec = 0;
                        usec = 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 queue message space for the current task or for the
                         **  timeout to expire.  The loop is required since the task
                         **  may be awakened by signals for messages which are
                         **  not ours, or for signals other than from a message send.
                         */
                        while ((waiting_on_q_space(queue, &timeout, &retcode)) && (retcode != ETIMEDOUT)) {
                                retcode = pthread_cond_timedwait(&queue->queue_space,
                                                &queue->qfull_lock, &timeout);
                        }
                }

                /*
                 **  Re-lock the queue mutex before manipulating its control block. 
                 */
                pthread_mutex_lock(&(queue->queue_lock));

                /*
                 **  Remove the calling task's task from the pended task list
                 **  for the queue.  Clear our TCB's suspend list pointer in
                 **  case the queue was killed & its ctrl blk deallocated.
                 */
                unlink_susp_task(&(queue->first_write_susp), our_task);
                //our_task->suspend_list = NULL;

                /*
                 **  See if we were awakened due to a msgQDelete on the queue.
                 */
                if (queue->send_type & KILLD) {
                        notify_if_delete_complete(queue);
                        error = S_objLib_OBJ_DELETED;
                        TRACEF("...queue deleted");
                } else {
                        /*
                         **  See if we timed out or if we got a message slot
                         */
                        if (retcode == ETIMEDOUT) {
                                /*
                                 **  Timed out without obtaining a message slot
                                 */
                                error = S_objLib_OBJ_TIMEOUT;
                                TRACEF("...timed out");
                        } else {
                                /*
                                 **  A message slot was freed on the queue for this task...
                                 */
                                TRACEF("...rcvd queue msg space");

                                if (pri == MSG_PRI_URGENT) {
                                        /*
                                         **  Stuff the new message onto the front of the queue.
                                         */
                                        urgent_msg_to(queue, msg, msglen);

                                        /*
                                         **  Signal the condition variable for the queue
                                         */
                                        pthread_cond_broadcast(&(queue->queue_send));
                                } else
                                        /*
                                         **  Send the new message to the back of the queue.
                                         */
                                        send_msg_to(queue, msg, msglen);
                        }
                }
        } else {
                /*
                 **  Queue is full and no waiting allowed... return QUEUE FULL error
                 */
                error = S_objLib_OBJ_UNAVAILABLE;
                TRACEF("WARNING: queue is full");
        }
        return (error);
}

/*****************************************************************************
** msgQSend - posts a message to the tail of a v2pthread queue and awakens the
**            first selected task pended on the queue.
*****************************************************************************/
STATUS msgQSend(v2pt_mqueue_t * queue, char *msg, uint msglen, int wait, int pri)
{
        STATUS error = OK;
        TRACEF("%x %x %x",my_task(),queue,queue->first_susp);

        pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, &queue->queue_lock);
        if (!mqueue_find_lock(queue)) {
                error = S_objLib_OBJ_ID_ERROR;
                goto exit;
        }
        if (msglen > queue->msg_len) {
                error = S_msgQLib_INVALID_MSG_LENGTH;
                goto unlock;
        } 

        if (queue->msg_count > queue->msgs_per_queue) { // BUG > ?
                //  Queue is full
                error = waitToSend(queue, msg, msglen, wait, pri);
        } else {
                if (queue->msg_count == queue->msgs_per_queue) {
                        // Full
                        if ((queue->msgs_per_queue == 0) && queue->first_susp ) {
                                //  Special case... Send the new message.
                                send_msg_to(queue, msg, msglen);
                        } else {
                                if (pri == MSG_PRI_URGENT) {
                                        //  Stuff the new message onto the queue.
                                        urgent_msg_to(queue, msg, msglen);
                                        pthread_cond_broadcast(&(queue->queue_send));
                                } else
                                        /*
                                         **  Queue is full... if waiting on space is
                                         **  allowed, wait until space becomes available
                                         **  or the timeout expires.  If space becomes
                                         **  available, send the caller's message.
                                         */
                                        error = waitToSend(queue, msg, msglen, wait, pri);
                        }
                } else {
                        if (pri == MSG_PRI_URGENT) {
                                //  Stuff the new message onto the front of the queue.
                                urgent_msg_to(queue, msg, msglen);

                                //  Signal the condition variable for the queue
                                pthread_cond_broadcast(&(queue->queue_send));
                        } else
                                //  Send the new message to the back of the queue.
                                send_msg_to(queue, msg, msglen);
                }
        }
unlock:
        pthread_mutex_unlock(&queue->queue_lock);
exit:   {}
        pthread_cleanup_pop(0);

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

        return (error);
}

/*****************************************************************************
** delete_mqueue - takes care of destroying the specified queue and freeing
**                any resources allocated for that queue
*****************************************************************************/
static void delete_mqueue(v2pt_mqueue_t * queue)
{
        TRACEF();
        unlink_qcb(queue);
        
        if (queue->first_msg_in_queue)
            free(queue->first_msg_in_queue);
        
        if (queue)
            free(queue);
}

//#define pthread_cond_broadcast(args) do { TRACEF("pthread_cond_broadcast %x",args);pthread_cond_broadcast(args); } while (0)

/*****************************************************************************
** msgQDelete - removes the specified queue from the queue list and frees
**              the memory allocated for the queue control block and extents.
*****************************************************************************/
STATUS msgQDelete(v2pt_mqueue_t * queue)
{
        STATUS error = OK;
        TRACEF();

        pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, &queue->queue_lock);
        if ( ! mqueue_find_lock(queue)) {
                error = S_objLib_OBJ_ID_ERROR;
                goto exit;
        }
        queue->send_type = KILLD;

        //taskLock();
        if ( queue->first_susp || queue->first_write_susp ) {
            printf("!!! Certain thread is waiting for this queue %x\n", (int) queue );
                pthread_mutex_clean_lock(&queue->qdlet_lock);

                TRACEF("%x %x", &queue->queue_send, &queue->queue_lock);
                // Signal the condition variable for tasks waiting on messages in the queue
                pthread_cond_broadcast(&queue->queue_send);

                // Unlock the queue send mutex. 
                pthread_mutex_unlock(&queue->queue_lock);

                // Lock mutex for queue space
                pthread_mutex_clean_lock(&queue->qfull_lock);

                // Signal the condition variable for tasks waiting on space to post messages into the queue
                pthread_cond_broadcast(&queue->queue_space);

                // Unlock the queue space mutex. 
                pthread_cleanup_pop(1); // queue->qfull_lock

                /*
                 **  Wait for all pended tasks to receive deletion signal.
                 **  The last task to receive the deletion signal will signal the
                 **  deletion-complete condition variable.
                 */
                // while ( queue->first_susp  && queue->first_write_susp ) { BUG
                while ( queue->first_susp  || queue->first_write_susp ) {
                        pthread_cond_wait(&queue->qdlet_cmplt, &queue->qdlet_lock);
                }

                // Unlock the queue delete completion mutex. 
                pthread_cleanup_pop(1);
        } else {
                // Unlock the queue mutex. 
                pthread_mutex_unlock(&(queue->queue_lock));
        }

        /*
         **  No other tasks are pending on the queue by this point...
         **  Now physically delete the queue.
         */
        delete_mqueue(queue);
        //taskUnlock();
exit:
        {}
        pthread_cleanup_pop(0);

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

        return error;
}

/*****************************************************************************
** waiting_on_q_msg - returns a nonzero result unless a qualifying event
**                    occurs on the specified queue which should cause the
**                    pended task to be awakened.  The qualifying events
**                    are:
**                        1. a message is sent to the queue and the current
**                            task is selected to receive it
**                        2. the queue is deleted
*****************************************************************************/
static int waiting_on_q_msg(v2pt_mqueue_t * queue, struct timespec *timeout, int *retcode)
{
        int result;
        struct timeval now;
        unsigned long usec;
        TRACEF();
        if (queue->send_type & KILLD) {
                // Queue has been killed... waiting is over.
                TRACEF("KILLED");
                result = 0;
                *retcode = 0;
        } else {
                result = 1;

                /*
                 **  Multiple messages sent to the queue may be represented by only
                 **  a single signal to the condition variable, so continue
                 **  checking for a message for our task as long as more messages
                 **  are available.
                 */
                while (queue->msg_count > 0) {
                        TRACEF("%i",queue->msg_count);
                        // Message arrived... see if it's for our task.
                        if (signal_for_my_task(&queue->first_susp, queue->order)) {
                                /*
                                 **  Message was  destined for our task... waiting is over.
                                 */
                                result = 0;
                                *retcode = 0;
                                break;
                        } else {
                                /*
                                 **  Message isn't for our task... continue waiting.
                                 **  Sleep awhile to allow other tasks ahead of ours in the
                                 **  list of tasks waiting on the queue to get their
                                 **  messages, bringing our task to the head of the list.
                                 */
                                pthread_mutex_unlock(&queue->queue_lock);
                                OS_Delay(1);
                                pthread_mutex_lock(&queue->queue_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;
}

/*****************************************************************************
** msgQReceive - blocks the calling task until a message is available in the
**               specified v2pthread queue.
*****************************************************************************/
int msgQReceive(v2pt_mqueue_t * queue, char *msgbuf, uint buflen, int max_wait)
{
        DS_TASK_T *our_task;
        struct timeval now;
        struct timespec timeout;
        int retcode;
        int msglen = ERROR;
        long sec, usec;
        STATUS error  = OK;

        pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, &queue->queue_lock);
        if (!mqueue_find_lock(queue)) 
        {
                TRACEF("S_objLib_OBJ_ID_ERROR");
                error = S_objLib_OBJ_ID_ERROR;  /* Invalid queue specified */
                goto exit;
        }

        if (buflen < queue->msg_len) {
                TRACEF("S_msgQLib_INVALID_MSG_LENGTH %i %i ", buflen, queue->msg_len);
                error = S_msgQLib_INVALID_MSG_LENGTH;
                goto unlock;
        } 
        // Add task for task to list of tasks waiting on queue
        our_task = taskFind(0, 1);
        TRACEF("%x %x", our_task, queue);
        TRACEF("wait on queue list @ %p", our_task, &queue->first_susp);

        link_susp_task(&queue->first_susp, our_task);
        //  If tasks waiting to write to a zero-length queue, notify
        //  waiting task that we're ready to receive a message.
        if ( !queue->msgs_per_queue && queue->first_write_susp ) {
                pthread_mutex_clean_lock(&queue->qfull_lock);
                TRACEF();
                //  Alert the waiting tasks that message space is available.
                pthread_cond_broadcast(&queue->queue_space);
                pthread_cleanup_pop(1);
        }

        retcode = 0;

        if (max_wait == NO_WAIT) {
                /*
                 **  Caller specified no wait on queue message...
                 **  Check the condition variable with an immediate timeout.
                 */
                gettimeofday(&now, NULL);
                timeout.tv_sec = now.tv_sec;
                timeout.tv_nsec = now.tv_usec * 1000;
                while ((waiting_on_q_msg(queue, &timeout, &retcode)) && (retcode != ETIMEDOUT)) {
                        retcode = pthread_cond_timedwait(&queue->queue_send,
                                        &queue->queue_lock, &timeout);
                }
        } else if (max_wait == WAIT_FOREVER) {
                //  Infinite wait was specified... wait without timeout.
                while (waiting_on_q_msg(queue, 0, &retcode)) {
                        pthread_cond_wait(&queue->queue_send, &queue->queue_lock);
                }
        } else {
                /*
                 **  Wait on queue 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 queue message for the current task or for the
                 **  timeout to expire.  The loop is required since the task
                 **  may be awakened by signals for messages which are
                 **  not ours, or for signals other than from a message send.
                 */
                while ((waiting_on_q_msg(queue, &timeout, &retcode)) && (retcode != ETIMEDOUT)) {
                        TRACEF("pthread_cond_timedwait { %x %x %i", &queue->queue_send, &queue->queue_lock,max_wait);
                        retcode = pthread_cond_timedwait(&queue->queue_send, &queue->queue_lock, &timeout);
                        TRACEF("pthread_cond_timedwait }");
                }
        }

        /*
         **  Remove the calling task's task from the waiting task list
         **  for the queue.  Clear our TCB's suspend list pointer in
         **  case the queue was killed & its ctrl blk deallocated.
         */
        unlink_susp_task(&(queue->first_susp), our_task);
        //our_task->suspend_list = NULL;

        /*
         **  See if we were awakened due to a msgQDelete on the queue.
         */
        if (queue->send_type & KILLD) {
                notify_if_delete_complete(queue);
                error = S_objLib_OBJ_DELETED;
                TRACEF("...queue deleted");
        } else if (retcode == ETIMEDOUT) {
                /*
                 **  Timed out without a message
                 */
                if (max_wait == NO_WAIT)
                        error = S_objLib_OBJ_UNAVAILABLE;
                else
                        error = S_objLib_OBJ_TIMEOUT;
                TRACEF("...timed out");
        } else {
                /*
                 **  A message was sent to the queue for this task...
                 **  Retrieve the message and clear the queue contents.
                 */
                msglen = (int) fetch_msg_from(queue, (char *) msgbuf);
                TRACEF("...rcvd queue msg @ %p", msgbuf);
        }

        /*
         **  Unlock the mutex for the condition variable.
         */
unlock:
        pthread_mutex_unlock(&queue->queue_lock);
exit: {}
        /*
         **  Clean up the opening pthread_cleanup_push()
         */
        pthread_cleanup_pop(0);

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

/*****************************************************************************
** msgQNumMsgs - returns the number of messages currently posted to the
**               specified queue.
*****************************************************************************/
int msgQNumMsgs(v2pt_mqueue_t * queue)
{
        int num_msgs;
        // copuld be just return queue->msg_count
        pthread_cleanup_push((void (*)(void *)) pthread_mutex_unlock, (void *) &(queue->queue_lock));
        if (mqueue_find_lock(queue)) {
                num_msgs = queue->msg_count;
                pthread_mutex_unlock(&(queue->queue_lock));
        } else {
                num_msgs = (int) ERROR;
        }

        pthread_cleanup_pop(0);

        return (num_msgs);
}