queue.c

xenomai 很好的linux实时补丁

 * service, or the caller did not get ownership of the message through
 * a successful return from rt_queue_recv().
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Kernel module initialization/cleanup code
 * - Interrupt service routine
 * - Kernel-based task
 * - User-space task
 *
 * Rescheduling: never.
 */

int rt_queue_free (RT_QUEUE *q,
		   void *buf)
{
    int err;
    spl_t s;

    if (buf == NULL)
	return -EINVAL;

    xnlock_get_irqsave(&nklock,s);

    q = xeno_h2obj_validate(q,XENO_QUEUE_MAGIC,RT_QUEUE);

    if (!q)
        {
        err = xeno_handle_error(q,XENO_QUEUE_MAGIC,RT_QUEUE);
        goto unlock_and_exit;
        }
    
    err = xnheap_test_and_free(&q->bufpool,
			       ((rt_queue_msg_t *)buf) - 1,
			       &__queue_check_msg);
    if (err == -EBUSY)
	/* Release failed due to non-zero refcount; this is not an
	 * error from the interface POV. */
	err = 0;

 unlock_and_exit:

    xnlock_put_irqrestore(&nklock,s);

    return err;
}

/**
 * @fn int rt_queue_send(RT_QUEUE *q,void *buf,size_t size,int mode)
 *
 * @brief Send a message to a queue.
 *
 * This service sends a complete message to a given queue.
 *
 * @param q The descriptor address of the message queue to send to.
 *
 * @param buf The address of the message to be sent.  The message
 * space must have been allocated using the rt_queue_alloc() service.
 * Once passed to rt_queue_send(), the memory pointed to by @a buf is
 * no more under the control of the sender and thus should not be
 * referenced by it anymore; deallocation of this memory must be
 * handled on the receiving side.
 *
 * @param size The size in bytes of the message. Zero is a valid
 * value, in which case an empty message will be sent.
 *
 * @param mode A set of flags affecting the operation:
 *
 * - Q_URGENT causes the message to be prepended to the message queue,
 * ensuring a LIFO ordering.
 *
 * - Q_NORMAL causes the message to be appended to the message queue,
 * ensuring a FIFO ordering.
 *
 * - Q_BROADCAST causes the message to be sent to all tasks currently
 * waiting for messages. The message is not copied; a reference count
 * is maintained instead so that the message will remain valid until
 * the last receiver releases its own reference using rt_queue_free(),
 * after which the message space will be returned to the queue's
 * internal pool.
 *
 * @return Upon success, this service returns the number of receivers
 * which got awaken as a result of the operation. If zero is returned,
 * no task was waiting on the receiving side of the queue, and the
 * message has been enqueued. Upon error, one of the following error
 * codes is returned:
 *
 * - -EINVAL is returned if @a q is not a message queue descriptor.
 *
 * - -EIDRM is returned if @a q is a deleted queue descriptor.
 *
 * - -ENOMEM is returned if queuing the message would exceed the limit
 * defined for the queue at creation.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Kernel module initialization/cleanup code
 * - Interrupt service routine
 * - Kernel-based task
 * - User-space task
 *
 * Rescheduling: possible.
 */

int rt_queue_send (RT_QUEUE *q,
		   void *buf,
		   size_t size,
		   int mode)
{
    xnthread_t *sleeper;
    rt_queue_msg_t *msg;
    int err, nrecv = 0;
    spl_t s;

    if (buf == NULL)
	return -EINVAL;

    xnlock_get_irqsave(&nklock,s);

    q = xeno_h2obj_validate(q,XENO_QUEUE_MAGIC,RT_QUEUE);

    if (!q)
        {
        err = xeno_handle_error(q,XENO_QUEUE_MAGIC,RT_QUEUE);
        goto unlock_and_exit;
        }

    if (q->qlimit != Q_UNLIMITED && countq(&q->pendq) >= q->qlimit)
	{
	err = -ENOMEM;
	goto unlock_and_exit;
	}

    msg = ((rt_queue_msg_t *)buf) - 1;

    if (msg->refcount == 0)
	{
	/* Cheap test: the sender cannot own the message. Bail out. */
	err = -EINVAL;
	goto unlock_and_exit;
	}

    /* Message buffer ownership is being transferred from the sender to
       the receiver(s) here; so we need to update the reference count
       appropriately. */
    msg->refcount--;
    msg->size = size;

    do
	{
	sleeper = xnsynch_wakeup_one_sleeper(&q->synch_base);
    
	if (!sleeper)
	    break;

	thread2rtask(sleeper)->wait_args.qmsg = msg;
	msg->refcount++;
	nrecv++;
	}
    while (mode & Q_BROADCAST);

    if (nrecv > 0)
	xnpod_schedule();
    else if (!(mode & Q_BROADCAST))
	{
	/* Messages are never queued in broadcast mode. Otherwise we
	   need to queue the message if no task is waiting for it. */

	if (mode & Q_URGENT)
	    prependq(&q->pendq,&msg->link);
	else
	    appendq(&q->pendq,&msg->link);
	}

    err = nrecv;

 unlock_and_exit:

    xnlock_put_irqrestore(&nklock,s);

    return err;
}

/**
 * @fn ssize_t rt_queue_recv(RT_QUEUE *q,void **bufp,RTIME timeout)
 *
 * @brief Receive a message from a queue.
 *
 * This service retrieves the next message available from the given
 * queue. Unless otherwise specified, the caller is blocked for a
 * given amount of time if no message is immediately available on
 * entry.
 *
 * @param q The descriptor address of the message queue to receive
 * from.
 *
 * @param bufp A pointer to a memory location which will be written
 * upon success with the address of the received message. Once
 * consumed, the message space should be freed using rt_queue_free().
 *
 * @param timeout The number of clock ticks to wait for some message
 * to arrive (see note). Passing TM_INFINITE causes the caller to
 * block indefinitely until some message is eventually
 * available. Passing TM_NONBLOCK causes the service to return
 * immediately without waiting if no message is available on entry.
 *
 * @return The number of bytes available from the received message is
 * returned upon success. Zero is a possible value corresponding to a
 * zero-sized message passed to rt_queue_send(). Otherwise:
 *
 * - -EINVAL is returned if @a q is not a message queue descriptor.
 *
 * - -EIDRM is returned if @a q is a deleted queue descriptor.
 *
 * - -ETIMEDOUT is returned if @a timeout is different from
 * TM_NONBLOCK and no message is available within the specified amount
 * of time.
 *
 * - -EWOULDBLOCK is returned if @a timeout is equal to TM_NONBLOCK
 * and no message is immediately available on entry.
 *
 * - -EINTR is returned if rt_task_unblock() has been called for the
 * waiting task before any data was available.
 *
 * - -EPERM is returned if this service should block, but was called
 * from a context which cannot sleep (e.g. interrupt, non-realtime or
 * scheduler locked).
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Kernel module initialization/cleanup code
 * - Interrupt service routine
 *   only if @a timeout is equal to TM_NONBLOCK.
 *
 * - Kernel-based task
 * - User-space task (switches to primary mode)
 *
 * Rescheduling: always unless the request is immediately satisfied or
 * @a timeout specifies a non-blocking operation.
 *
 * @note This service is sensitive to the current operation mode of
 * the system timer, as defined by the rt_timer_start() service. In
 * periodic mode, clock ticks are interpreted as periodic jiffies. In
 * oneshot mode, clock ticks are interpreted as nanoseconds.
 */

ssize_t rt_queue_recv (RT_QUEUE *q,
		       void **bufp,
		       RTIME timeout)
{
    rt_queue_msg_t *msg = NULL;
    xnholder_t *holder;
    ssize_t err = 0;
    RT_TASK *task;
    spl_t s;

    xnlock_get_irqsave(&nklock,s);

    q = xeno_h2obj_validate(q,XENO_QUEUE_MAGIC,RT_QUEUE);

    if (!q)
        {
        err = xeno_handle_error(q,XENO_QUEUE_MAGIC,RT_QUEUE);
        goto unlock_and_exit;
        }

    holder = getq(&q->pendq);

    if (holder)
        {
	msg = link2rtmsg(holder);
	msg->refcount++;
	}
    else
	{
	if (timeout == TM_NONBLOCK)
	    {
	    err = -EWOULDBLOCK;;
	    goto unlock_and_exit;
	    }

	if (xnpod_unblockable_p())
	    {
	    err = -EPERM;
	    goto unlock_and_exit;
	    }

	xnsynch_sleep_on(&q->synch_base,timeout);

	task = xeno_current_task();

	if (xnthread_test_flags(&task->thread_base,XNRMID))
	    err = -EIDRM; /* Queue deleted while pending. */
	else if (xnthread_test_flags(&task->thread_base,XNTIMEO))
	    err = -ETIMEDOUT; /* Timeout.*/
	else if (xnthread_test_flags(&task->thread_base,XNBREAK))
	    err = -EINTR; /* Unblocked.*/
	else
	    {
	    msg = task->wait_args.qmsg;
	    task->wait_args.qmsg = NULL;
	    }
	}

    if (msg)
	{
	*bufp = msg + 1;
	err = (ssize_t)msg->size;
	}

 unlock_and_exit:

    xnlock_put_irqrestore(&nklock,s);

    return err;
}

/**
 * @fn int rt_queue_inquire(RT_QUEUE *q, RT_QUEUE_INFO *info)
 *
 * @brief Inquire about a message queue.
 *
 * Return various information about the status of a given queue.
 *
 * @param q The descriptor address of the inquired queue.
 *
 * @param info The address of a structure the queue information will
 * be written to.

 * @return 0 is returned and status information is written to the
 * structure pointed at by @a info upon success. Otherwise:
 *
 * - -EINVAL is returned if @a q is not a message queue descriptor.
 *
 * - -EIDRM is returned if @a q is a deleted queue descriptor.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Kernel module initialization/cleanup code
 * - Interrupt service routine
 * - Kernel-based task
 * - User-space task
 *
 * Rescheduling: never.
 */

int rt_queue_inquire (RT_QUEUE *q,
                      RT_QUEUE_INFO *info)
{
    int err = 0;
    spl_t s;

    xnlock_get_irqsave(&nklock,s);

    q = xeno_h2obj_validate(q,XENO_QUEUE_MAGIC,RT_QUEUE);

    if (!q)
        {
        err = xeno_handle_error(q,XENO_QUEUE_MAGIC,RT_QUEUE);
        goto unlock_and_exit;
        }
    
    strcpy(info->name,q->name);
    info->nwaiters = xnsynch_nsleepers(&q->synch_base);
    info->nmessages = countq(&q->pendq);
    info->qlimit = q->qlimit;
    info->poolsize = xnheap_size(&q->bufpool);
    info->mode = q->mode;

 unlock_and_exit:

    xnlock_put_irqrestore(&nklock,s);

    return err;
}

/**
 * @fn int rt_queue_bind(RT_QUEUE *q,const char *name,RTIME timeout)
 *
 * @brief Bind to a shared message queue.
 *
 * This user-space only service retrieves the uniform descriptor of a
 * given shared Xenomai message queue identified by its symbolic name. If
 * the queue does not exist on entry, this service blocks the caller
 * until a queue of the given name is created.
 *
 * @param name A valid NULL-terminated name which identifies the
 * queue to bind to.
 *
 * @param q The address of a queue descriptor retrieved by the
 * operation. Contents of this memory is undefined upon failure.
 *
 * @param timeout The number of clock ticks to wait for the
 * registration to occur (see note). Passing TM_INFINITE causes the
 * caller to block indefinitely until the object is
 * registered. Passing TM_NONBLOCK causes the service to return
 * immediately without waiting if the object is not registered on
 * entry.
 *
 * @return 0 is returned upon success. Otherwise:
 *
 * - -EFAULT is returned if @a q or @a name is referencing invalid
 * memory.
 *
 * - -EINTR is returned if rt_task_unblock() has been called for the
 * waiting task before the retrieval has completed.
 *
 * - -EWOULDBLOCK is returned if @a timeout is equal to TM_NONBLOCK
 * and the searched object is not registered on entry.
 *
 * - -ETIMEDOUT is returned if the object cannot be retrieved within
 * the specified amount of time.
 *
 * - -EPERM is returned if this service should block, but was called
 * from a context which cannot sleep (e.g. interrupt, non-realtime or
 * scheduler locked).
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - User-space task (switches to primary mode)
 *
 * Rescheduling: always unless the request is immediately satisfied or
 * @a timeout specifies a non-blocking operation.
 *
 * @note This service is sensitive to the current operation mode of
 * the system timer, as defined by the rt_timer_start() service. In
 * periodic mode, clock ticks are interpreted as periodic jiffies. In
 * oneshot mode, clock ticks are interpreted as nanoseconds.
 */

/**
 * @fn int rt_queue_unbind(RT_QUEUE *q)
 *
 * @brief Unbind from a shared message queue.
 *
 * This user-space only service unbinds the calling task from the
 * message queue object previously retrieved by a call to
 * rt_queue_bind().
 *
 * Unbinding from a message queue when it is no more needed is
 * especially important in order to properly release the mapping
 * resources used to attach the shared queue memory to the caller's
 * address space.
 *
 * @param q The address of a queue descriptor to unbind from.
 *
 * @return 0 is always returned.
 *
 * This service can be called from:
 *
 * - User-space task.
 *
 * Rescheduling: never.
 */

int __native_queue_pkg_init (void)

{
    return 0;
}

void __native_queue_pkg_cleanup (void)

{
}

/*@}*/

EXPORT_SYMBOL(rt_queue_create);
EXPORT_SYMBOL(rt_queue_delete);
EXPORT_SYMBOL(rt_queue_alloc);
EXPORT_SYMBOL(rt_queue_free);
EXPORT_SYMBOL(rt_queue_send);
EXPORT_SYMBOL(rt_queue_recv);
EXPORT_SYMBOL(rt_queue_inquire);