mqpxlib.c

VXWORKS源代码

    struct mq_attr *	pAttr;		/* pointer to attr passed in */
    void *		pQMem;		/* memory queue will be placed in */
    mode_t		mode;		/* not used by vxWorks */
    int			nMsgs;		/* number of messages in queue */
    int			msgSize;	/* size of each message in queue */
    int			nBytes;		/* amount of mem needed for queue */
    SYM_TYPE		dummy;		/* dummy var for symFindByName */

    if (INT_RESTRICT () != OK)	/* restrict ISR from calling */
	return ((mqd_t) -1);

    if ((!mqLibInstalled) && (mqPxLibInit (0) != OK))
	return ((mqd_t) -1);	/* package init problem */

    if ((oflags & 3) == 3)
	{
	errno = EINVAL;
	return ((mqd_t) -1);
	}

    semTake (&mqNameTbl->symMutex, WAIT_FOREVER);

    if (symFindByName (mqNameTbl, (char *) mqName, (char **)&pQMem, 
                       &dummy) == OK)
	{
	/*
	 * Found a mq, see if we should use it
	 */
	if (O_EXCL & oflags)
	    {
	    semGive (&mqNameTbl->symMutex);
	    errno = EEXIST;
	    return ((mqd_t) -1);
	    }

	if ((pMqDesc = (mqd_t) objAlloc (mqClassId)) == NULL)
	    {
	    semGive (&mqNameTbl->symMutex);
	    errno = ENOSPC;
	    return ((mqd_t) -1);
	    }
	}
    else
	{
	/*
	 * There was no mq, create one if asked
	 */
	if (!(O_CREAT & oflags))
	    {
	    semGive (&mqNameTbl->symMutex);
	    errno = ENOENT;
	    return ((mqd_t) -1);		/* queue does not exist */
	    }

	va_start (vaList, oflags);
	mode = va_arg (vaList, mode_t);
	pAttr = va_arg (vaList, struct mq_attr *);
	va_end (vaList);

	if (pAttr != NULL)
	    {
	    nMsgs = pAttr->mq_maxmsg;
	    msgSize = pAttr->mq_msgsize;

	    if ((nMsgs <= 0) || (msgSize <= 0))
		{
	    	semGive (&mqNameTbl->symMutex);
	    	errno = EINVAL;
	    	return ((mqd_t) -1);		/* invalid size specified */
	    	}

	    oflags |= (pAttr->mq_flags & O_NONBLOCK);
	    }
	else
	    {
	    nMsgs =   MQ_NUM_MSGS_DEFAULT;
	    msgSize = MQ_MSG_SIZE_DEFAULT;
	    }

	nBytes = MEM_ROUND_UP (sizeof(struct msg_que)) + 
		 nMsgs * MEM_ROUND_UP (msgSize + sizeof (struct sll_node));
	pQMem = malloc (nBytes + strlen (mqName) + 1);

	if (pQMem == 0)
	    {
	    semGive (&mqNameTbl->symMutex);
	    errno = ENOSPC;
	    return ((mqd_t) -1);
	    }

	mq_init (pQMem, nMsgs, msgSize);

	((struct msg_que *) pQMem)->msgq_sym.name = (char *) pQMem + nBytes;
	((struct msg_que *) pQMem)->msgq_sym.value = (char *) pQMem;
	strcpy ((char *)pQMem + nBytes, mqName);


	if ((pMqDesc = (mqd_t) objAlloc (mqClassId)) == NULL)
	    {
	    free (pQMem);
	    semGive (&mqNameTbl->symMutex);
	    errno = ENOSPC;
	    return ((mqd_t) -1);
	    }

	symTblAdd (mqNameTbl, &((struct msg_que *) pQMem)->msgq_sym);
	}

    /*
     * (oflags & 3) + 1 is magic for transposing O_RDONLY, O_RDWR or
     * O_WRONLY into the bit field flags FREAD and FWRITE
     */
    pMqDesc->f_flag = (oflags & O_NONBLOCK) | ((oflags & 3) + 1);
    pMqDesc->f_data = pQMem;
    pMqDesc->f_data->msgq_links++;
    objCoreInit (&pMqDesc->f_objCore, mqClassId);	/* valid file obj */

    semGive (&mqNameTbl->symMutex);

    return (pMqDesc);
    }


/*******************************************************************************
*
* mq_receive - receive a message from a message queue (POSIX)
*
* This routine receives the oldest of the highest priority message from
* the message queue specified by <mqdes>.  If the size of the buffer in
* bytes, specified by the <msgLen> argument, is less than the `mq_msgsize'
* attribute of the message queue, mq_receive() will fail and return an
* error.  Otherwise, the selected message is removed from the queue and
* copied to <pMsg>.
*
* If <pMsgPrio> is not NULL, the priority of the selected message
* will be stored in <pMsgPrio>.
*
* If the message queue is empty and O_NONBLOCK is not set in the message
* queue's description, mq_receive() will block until a message is added to
* the message queue, or until it is interrupted by a signal.  If more than
* one task is waiting to receive a message when a message arrives at an
* empty queue, the task of highest priority that has been waiting the
* longest will be selected to receive the message.  If the specified message
* queue is empty and O_NONBLOCK is set in the message queue's description,
* no message is removed from the queue, and mq_receive() returns an error.
*
* RETURNS: The length of the selected message in bytes, otherwise -1 (ERROR). 
*
* ERRNO: EAGAIN, EBADF, EMSGSIZE, EINTR
*
* SEE ALSO: mq_send()
*/

ssize_t mq_receive
    (
    mqd_t	mqdes,		/* message queue descriptor */
    void	*pMsg,		/* buffer to receive message */
    size_t	msgLen,		/* size of buffer, in bytes */
    int		*pMsgPrio	/* if not NULL, priority of message */
    )
    {
    struct sll_node *pNode;
    struct msg_que *pMq;
    int status;
    int level;
    int prio;
    int error = 0;
    int savtype;

    if (INT_RESTRICT () != OK)		/* restrict ISR from calling */
	return (-1);

    TASK_LOCK ();			/* TASK LOCK */

    if ((OBJ_VERIFY (mqdes, mqClassId) != OK) ||
	((mqdes->f_flag & FREAD) == 0))
	{
	error = EBADF;
	goto bad;
	}

    pMq = mqdes->f_data;

    /* Link into pthreads support code */

    if (_func_pthread_setcanceltype != NULL)
        {
        _func_pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &savtype);
        }

    if (msgLen < pMq->msgq_attr.mq_msgsize)
	{
	error = EMSGSIZE;
	goto bad;
	}

    level = intLock ();			/* LOCK INTERRUPTS */

    if (pMq->msgq_bmap == 0)
	{
	if (mqdes->f_flag & O_NONBLOCK)
	    {
	    intUnlock (level);		/* UNLOCK INTERRUPTS */
	    error = EAGAIN;
	    goto bad;
	    }

	/*
	 * This needs to be in a loop, the reason is that someone
	 * can wake us up but we may not run.  During that time someone
	 * else can come in and steal our message.
	 */
	while (pMq->msgq_bmap == 0)
	    {
	    kernelState = TRUE;		/* ENTER KERNEL */
	    intUnlock (level);		/* UNLOCK INTERRUPTS */

	    /* windview - level 2 instrumentation */
            EVT_TASK_1 (EVENT_OBJ_MSGRECEIVE, pMq);

	    if (windPendQPut (&pMq->msgq_cond_data, WAIT_FOREVER) != OK)
		{
		windExit ();			/* EXIT KERNEL */
		error = EBADF;			/* what should ernno be??? */
		goto bad;
		}

	    status = windExit ();		/* EXIT KERNEL */

	    if (status != 0)
		{
		error = (status == RESTART) ? EINTR : EAGAIN;
		goto bad;
		}

	    level = intLock ();		/* LOCK INTERRUPTS */
	    }
	}

    prio = ffsMsb (pMq->msgq_bmap) - 1;

    pNode = sll_head (&pMq->msgq_data_list[prio]);
    --pMq->msgq_attr.mq_curmsgs;

    if (pMq->msgq_data_list[prio] == NULL)
	pMq->msgq_bmap &= ~(1 << prio);

    intUnlock (level);			/* UNLOCK INTERRUPTS */

    msgLen = pNode->sll_size;

    bcopy ((void *) (pNode + 1), pMsg, msgLen);

    if (pMsgPrio != 0)
	*pMsgPrio = prio;

    level = intLock ();			/* LOCK INTERRUPTS */

    sll_ins (pNode, &pMq->msgq_free_list);

    if (Q_FIRST (&pMq->msgq_cond_read) != 0)
	{
	kernelState = TRUE;		/* ENTER KERNEL */
	intUnlock (level);		/* UNLOCK INTERRUPTS */

	/* windview - level 2 instrumentation */
        EVT_TASK_1 (EVENT_OBJ_MSGRECEIVE, pMq);

	windPendQGet (&pMq->msgq_cond_read);
	windExit ();			/* EXIT KERNEL */
	}
    else
	intUnlock (level);		/* UNLOCK INTERRUPTS */

bad:
    TASK_UNLOCK();			/* TASK UNLOCK */

    /* Link into pthreads support code */

    if (_func_pthread_setcanceltype != NULL)
        {
        _func_pthread_setcanceltype(savtype, NULL);
        }

    if (error != 0)
	{
	errno = error;
	return (-1);
	}

    return (msgLen);
    }

/*******************************************************************************
*
* mq_work - handle mq work
*
* The routine is always done as work. It is needed because windPendQGet
* does not check it the queue is empty.
*/

LOCAL void mq_work
    (
    struct msg_que *pMq,
    int flag
    )
    {
    if (Q_FIRST (&pMq->msgq_cond_data) != NULL)
	{
	/* windview - level 2 instrumentation */
        EVT_TASK_1 (EVENT_OBJ_MSGSEND, pMq);

	windPendQGet (&pMq->msgq_cond_data);
	}
    else if ((flag == 0) && (pMq->msgq_sigTask != -1))
	{
        /* SPR #32033
         * An interrupt context is faked while executing sigPendKill().  
         * Since the work queue is drained with kernelState set to TRUE,
         * sigPendKill() will perform an excJobAdd() of itself.  Without
         * faking an interrupt context, the ensuing msgQSend() will perform
         * taskLock/taskUnlock to protect it's critical section.  The 
         * taskUnlock() would then re-enter the kernel via windExit().
         * The kernel cannot be re-entered while in the process of draining
         * the work queue.
         */
   
	++intCnt;     /* no need to lock interrupts around increment */
	sigPendKill (pMq->msgq_sigTask, &pMq->msgq_sigPend);
	--intCnt;     /* no need to lock interrupts around decrement */
	pMq->msgq_sigTask = -1;
	}
    }

/*******************************************************************************
*
* mq_send - send a message to a message queue (POSIX)
*
* This routine adds the message <pMsg> to the message queue
* <mqdes>.  The <msgLen> parameter specifies the length of the message in
* bytes pointed to by <pMsg>.  The value of <pMsg> must be less than or
* equal to the `mq_msgsize' attribute of the message queue, or mq_send()
* will fail.
*
* If the message queue is not full, mq_send() will behave as if the message
* is inserted into the message queue at the position indicated by the 
* <msgPrio> argument.  A message with a higher numeric value for <msgPrio>
* is inserted before messages with a lower value.  The value
* of <msgPrio> must be less than or equal to 31.
*
* If the specified message queue is full and O_NONBLOCK is not set in the
* message queue's, mq_send() will block until space becomes available to
* queue the message, or until it is interrupted by a signal.  The priority
* scheduling option is supported in the event that there is more than one
* task waiting on space becoming available.  If the message queue is full
* and O_NONBLOCK is set in the message queue's description, the message is
* not queued, and mq_send() returns an error.
*
* USE BY INTERRUPT SERVICE ROUTINES
* This routine can be called by interrupt service routines as well as
* by tasks.  This is one of the primary means of communication
* between an interrupt service routine and a task.  If mq_send()
* is called from an interrupt service routine, it will behave as if
* the O_NONBLOCK flag were set.
*
* RETURNS: 0 (OK), otherwise -1 (ERROR).
*
* ERRNO: EAGAIN, EBADF, EINTR, EINVAL, EMSGSIZE
*
* SEE ALSO: mq_receive()
*/

int mq_send
    (
    mqd_t	mqdes,		/* message queue descriptor */
    const void	*pMsg,		/* message to send */
    size_t	msgLen,		/* size of message, in bytes */
    int		msgPrio		/* priority of message */
    )
    {
    struct sll_node *pNode;
    struct msg_que *pMq;
    int status;
    int level;
    int flag;
    int sigTaskSave;		/* saved task id */
    int error = 0;
    int savtype;		/* saved cancellation type */

    if (msgPrio > MQ_PRIORITY_MAX)
	{
	errno = EINVAL;
	return (-1);
	}

    /* Link into pthreads support code */
    
    if (_func_pthread_setcanceltype != NULL)
        {
        _func_pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &savtype);
        }

     if (!INT_CONTEXT ())
	TASK_LOCK ();			/* TASK LOCK */

    if ((OBJ_VERIFY (mqdes, mqClassId) != OK) ||
	((mqdes->f_flag & FWRITE) == 0))
	{
	error = EBADF;
	goto bad;
	}

    pMq = mqdes->f_data;

    if (msgLen > pMq->msgq_attr.mq_msgsize)
	{
	error = EMSGSIZE;
	goto bad;
	}

    level = intLock ();			/* LOCK INTERRUPTS */

    if (pMq->msgq_free_list == NULL)
	{
	if (mqdes->f_flag & O_NONBLOCK)
	    {
	    intUnlock (level);		/* UNLOCK INTERRUPTS */
	    error = EAGAIN;
	    goto bad;
	    }