queue.c

xenomai 很好的linux实时补丁

/**
 * @file
 * This file is part of the Xenomai project.
 *
 * @note Copyright (C) 2004 Philippe Gerum <[e-mail]> 
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * \ingroup native_queue
 */

/*!
 * \ingroup native
 * \defgroup native_queue Message queue services.
 *
 * Queue services.
 *
 * Message queueing is a method by which real-time tasks can exchange
 * or pass data through a xeno-managed queue of messages. Messages can
 * vary in length and be assigned different types or usages. A message
 * queue can be created by one task and used by multiple tasks that
 * send and/or receive messages to the queue.
 *
 * This implementation is based on a zero-copy scheme for message
 * buffers. Message buffer pools are built over the nucleus's heap
 * objects, which in turn provide the needed support for exchanging
 * messages between kernel and user-space using direct memory mapping.
 *
 *@{*/

#include <nucleus/pod.h>
#include <nucleus/registry.h>
#include <native/task.h>
#include <native/queue.h>

#ifdef CONFIG_XENO_EXPORT_REGISTRY

static int __queue_read_proc (char *page,
			      char **start,
			      off_t off,
			      int count,
			      int *eof,
			      void *data)
{
    RT_QUEUE *q = (RT_QUEUE *)data;
    char *p = page;
    int len;
    spl_t s;

    p += sprintf(p,"type=%s:poolsz=%lu:limit=%d:mcount=%d\n",
		 q->mode & Q_SHARED ? "shared" : "local",
		 xnheap_size(&q->bufpool),
		 q->qlimit,
		 countq(&q->pendq));

    xnlock_get_irqsave(&nklock,s);

    if (xnsynch_nsleepers(&q->synch_base) > 0)
	{
	xnpholder_t *holder;
	
	/* Pended queue -- dump waiters. */

	holder = getheadpq(xnsynch_wait_queue(&q->synch_base));

	while (holder)
	    {
	    xnthread_t *sleeper = link2thread(holder,plink);
	    p += sprintf(p,"+%s\n",xnthread_name(sleeper));
	    holder = nextpq(xnsynch_wait_queue(&q->synch_base),holder);
	    }
	}

    xnlock_put_irqrestore(&nklock,s);

    len = (p - page) - off;
    if (len <= off + count) *eof = 1;
    *start = page + off;
    if(len > count) len = count;
    if(len < 0) len = 0;

    return len;
}

extern xnptree_t __native_ptree;

static xnpnode_t __queue_pnode = {

    .dir = NULL,
    .type = "queues",
    .entries = 0,
    .read_proc = &__queue_read_proc,
    .write_proc = NULL,
    .root = &__native_ptree,
};

#elif defined(CONFIG_XENO_OPT_REGISTRY)

static xnpnode_t __queue_pnode = {

    .type = "queues"
};

#endif /* CONFIG_XENO_EXPORT_REGISTRY */

static void __queue_flush_private (xnheap_t *heap,
				   void *poolmem,
				   u_long poolsize,
				   void *cookie)
{
    xnarch_sysfree(poolmem,poolsize);
}

/**
 * @fn int rt_queue_create(RT_QUEUE *q,const char *name,size_t poolsize,size_t qlimit,int mode)
 *
 * @brief Create a message queue.
 *
 * Create a message queue object that allows multiple tasks to
 * exchange data through the use of variable-sized messages. A message
 * queue is created empty. Message queues can be local to the kernel
 * space, or shared between kernel and user-space.
 *
 * This service needs the special character device /dev/rtheap
 * (10,254) when called from user-space tasks.
 *
 * @param q The address of a queue descriptor Xenomai will use to store
 * the queue-related data.  This descriptor must always be valid while
 * the message queue is active therefore it must be allocated in
 * permanent memory.
 *
 * @param name An ASCII string standing for the symbolic name of the
 * queue. When non-NULL and non-empty, this string is copied to a safe
 * place into the descriptor, and passed to the registry package if
 * enabled for indexing the created queue. Shared queues must be given
 * a valid name.
 *
 * @param poolsize The size (in bytes) of the message buffer pool
 * which is going to be pre-allocated to the queue. Message buffers
 * will be claimed and released to this pool.  The buffer pool memory
 * is not extensible, so this value must be compatible with the
 * highest message pressure that could be expected.
 *
 * @param qlimit This parameter allows to limit the maximum number of
 * messages which can be queued at any point in time. Sending to a
 * full queue begets an error. The special value Q_UNLIMITED can be
 * passed to specify an unlimited amount.
 *
 * @param mode The queue creation mode. The following flags can be
 * OR'ed into this bitmask, each of them affecting the new queue:
 *
 * - Q_FIFO makes tasks pend in FIFO order on the queue for consuming
 * messages.
 *
 * - Q_PRIO makes tasks pend in priority order on the queue.
 *
 * - Q_SHARED causes the queue to be sharable between kernel and
 * user-space tasks. Otherwise, the new queue is only available for
 * kernel-based usage. This flag is implicitely set when the caller is
 * running in user-space. This feature requires the real-time support
 * in user-space to be configured in (CONFIG_XENO_OPT_PERVASIVE).
 *
 * - Q_DMA causes the buffer pool associated to the queue to be
 * allocated in physically contiguous memory, suitable for DMA
 * operations with I/O devices. A 128Kb limit exists for @a poolsize
 * when this flag is passed.
 *
 * @return 0 is returned upon success. Otherwise:
 *
 * - -EEXIST is returned if the @a name is already in use by some
 * registered object.
 *
 * - -EINVAL is returned if @a poolsize is null, greater than the
 * system limit, or @a name is null or empty for a shared queue.
 *
 * - -ENOMEM is returned if not enough system memory is available to
 * create or register the queue. Additionally, and if Q_SHARED has
 * been passed in @a mode, errors while mapping the buffer pool in the
 * caller's address space might beget this return code too.
 *
 * - -EPERM is returned if this service was called from an invalid
 * context.
 *
 * - -ENOSYS is returned if @a mode specifies Q_SHARED, but the
 * real-time support in user-space is unavailable.
 *
 * - -ENOENT is returned if /dev/rtheap can't be opened.
 *   
 * Environments:
 *
 * This service can be called from:
 *
 * - Kernel module initialization/cleanup code
 * - User-space task (switches to secondary mode)
 *
 * Rescheduling: possible.
 */

int rt_queue_create (RT_QUEUE *q,
		     const char *name,
		     size_t poolsize,
		     size_t qlimit,
		     int mode)
{
    int err;

    if (!xnpod_root_p())
	return -EPERM;

    if (poolsize == 0)
	return -EINVAL;

    /* Make sure we won't hit trivial argument errors when calling
       xnheap_init(). */

    if (poolsize < 2 * PAGE_SIZE)
	poolsize = 2 * PAGE_SIZE;

    /* Account for the overhead so that the actual free space is large
       enough to match the requested size. */

    poolsize += xnheap_overhead(poolsize,PAGE_SIZE);
    poolsize = PAGE_ALIGN(poolsize);

#ifdef __KERNEL__
    if (mode & Q_SHARED)
	{
	if (!name || !*name)
	    return -EINVAL;

#ifdef CONFIG_XENO_OPT_PERVASIVE
	err = xnheap_init_mapped(&q->bufpool,
				 poolsize,
				 (mode & Q_DMA) ? GFP_DMA : 0);
	if (err)
	    return err;

	q->cpid = 0;
#else /* !CONFIG_XENO_OPT_PERVASIVE */
	return -ENOSYS;
#endif /* CONFIG_XENO_OPT_PERVASIVE */
	}
    else
#endif /* __KERNEL__ */
	{
	void *poolmem = xnarch_sysalloc(poolsize);

	if (!poolmem)
	    return -ENOMEM;

	err = xnheap_init(&q->bufpool,
			  poolmem,
			  poolsize,
			  PAGE_SIZE); /* Use natural page size */
	if (err)
	    {
	    xnarch_sysfree(poolmem,poolsize);
	    return err;
	    }
	}

    xnsynch_init(&q->synch_base,mode & (Q_PRIO|Q_FIFO));
    initq(&q->pendq);
    q->handle = 0;  /* i.e. (still) unregistered queue. */
    q->magic = XENO_QUEUE_MAGIC;
    q->qlimit = qlimit;
    q->mode = mode;
    xnobject_copy_name(q->name,name);

#ifdef CONFIG_XENO_OPT_REGISTRY
    /* <!> Since xnregister_enter() may reschedule, only register
       complete objects, so that the registry cannot return handles to
       half-baked objects... */

    if (name)
        {
	xnpnode_t *pnode = &__queue_pnode;
	
	if (!*name)
	    {
	    /* Since this is an anonymous object (empty name on entry)
	       from user-space, it gets registered under an unique
	       internal name but is not exported through /proc. */
	    xnobject_create_name(q->name,sizeof(q->name),(void*)q);
	    pnode = NULL;
	    }
	    
        err = xnregistry_enter(q->name,q,&q->handle,pnode);

        if (err)
            rt_queue_delete(q);
        }
#endif /* CONFIG_XENO_OPT_REGISTRY */

    return err;
}

/**
 * @fn int rt_queue_delete(RT_QUEUE *q)
 *
 * @brief Delete a message queue.
 *
 * Destroy a message queue and release all the tasks currently pending
 * on it.  A queue exists in the system since rt_queue_create() has
 * been called to create it, so this service must be called in order
 * to destroy it afterwards.
 *
 * @param q The descriptor address of the affected queue.
 *
 * @return 0 is returned 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.
 *
 * - -EPERM is returned if this service was called from an
 * asynchronous context.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Kernel module initialization/cleanup code
 * - User-space task (switches to secondary mode).
 *
 * Rescheduling: possible.
 */

int rt_queue_delete (RT_QUEUE *q)

{
    int err = 0, rc;
    spl_t s;

    if (xnpod_asynch_p())
	return -EPERM;

    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);
	xnlock_put_irqrestore(&nklock,s);
        return err;
        }

    rc = xnsynch_destroy(&q->synch_base);

#ifdef CONFIG_XENO_OPT_REGISTRY
    if (q->handle)
        xnregistry_remove(q->handle);
#endif /* CONFIG_XENO_OPT_REGISTRY */

    xeno_mark_deleted(q);

    /* Get out of the nklocked section before releasing the heap
       memory, since we are about to invoke Linux kernel services. */

    xnlock_put_irqrestore(&nklock,s);

    /* The queue descriptor has been marked as deleted before we
       released the superlock thus preventing any sucessful subsequent
       calls of rt_queue_delete(), so now we can actually destroy the
       associated heap safely. */

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
    if (q->mode & Q_SHARED)
	err = xnheap_destroy_mapped(&q->bufpool);
    else
#endif /* __KERNEL__ && CONFIG_XENO_OPT_PERVASIVE */
	err = xnheap_destroy(&q->bufpool,&__queue_flush_private,NULL);

    if (rc == XNSYNCH_RESCHED)
        /* Some task has been woken up as a result of the deletion:
           reschedule now. */
        xnpod_schedule();

    return err;
}

/**
 * @fn void *rt_queue_alloc(RT_QUEUE *q,size_t size)
 *
 * @brief Allocate a message queue buffer.
 *
 * This service allocates a message buffer from the queue's internal
 * pool which can be subsequently filled by the caller then passed to
 * rt_queue_send() for sending.
 *
 * @param q The descriptor address of the affected queue.
 *
 * @param size The requested size in bytes of the buffer. Zero is an
 * acceptable value, meaning that the message will not carry any
 * payload data; the receiver will thus receive a zero-sized message.
 *
 * @return The address of the allocated message buffer upon success,
 * or NULL if the allocation fails.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Kernel module initialization/cleanup code
 * - Interrupt service routine
 * - Kernel-based task
 * - User-space task
 *
 * Rescheduling: never.
 */

void *rt_queue_alloc (RT_QUEUE *q,
		      size_t size)
{
    rt_queue_msg_t *msg;
    spl_t s;

    xnlock_get_irqsave(&nklock,s);

    q = xeno_h2obj_validate(q,XENO_QUEUE_MAGIC,RT_QUEUE);

    if (!q)
        {
	xnlock_put_irqrestore(&nklock,s);
	return NULL;
        }
    
    msg = (rt_queue_msg_t *)xnheap_alloc(&q->bufpool,size + sizeof(rt_queue_msg_t));

    if (msg)
	{
	inith(&msg->link);
	msg->size = size;	/* Zero is ok. */
	msg->refcount = 1;
	++msg;
	}

    xnlock_put_irqrestore(&nklock,s);

    return msg;
}

static int __queue_check_msg (void *p)

{
    rt_queue_msg_t *msg = (rt_queue_msg_t *)p;

    if (msg->refcount == 0)
	return -EINVAL;

    if (--msg->refcount > 0)
	return -EBUSY;

    return 0;
}

/**
 * @fn int rt_queue_free(RT_QUEUE *q,void *buf)
 *
 * @brief Free a message queue buffer.
 *
 * This service releases a message buffer returned by rt_queue_recv()
 * to the queue's internal pool.
 *
 * @param q The descriptor address of the affected queue.
 *
 * @param buf The address of the message buffer to free. Even
 * zero-sized messages carrying no payload data must be freed, since
 * they are assigned a valid memory space to store internal
 * information.
 *
 * @return 0 is returned upon success, or -EINVAL if @a buf is not a
 * valid message buffer previously allocated by the rt_queue_alloc()