shm.c

xenomai 很好的linux实时补丁

    else if (len != xnheap_size(&shm->heapbase))
        err = EINVAL;

    up(&shm->maplock);

  err_shm_put:
    pse51_shm_put(shm, 1);

    if (!err)
        return 0;

  error:    
    thread_set_errno(err == ENOMEM ? EFBIG : err);
    return -1;
}

/**
 * Map pages of memory.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/mmap.html
 * 
 */
void *mmap(void *addr, size_t len, int prot, int flags, int fd, off_t off)
{
    pse51_shm_map_t *map;
    unsigned desc_flags;
    pse51_desc_t *desc;
    pse51_shm_t *shm;
    void *result;
    int err;
    spl_t s;

    if (xnpod_asynch_p() || !xnpod_root_p())
        {
        err = EPERM;
        goto error;
        }    

    if (!len)
        {
        err = EINVAL;
        goto error;
        }

    if (flags != MAP_SHARED)
        {
        err = ENOTSUP;
        goto error;
        }

    if (((unsigned long) addr) % PAGE_SIZE)
        {
        err = EINVAL;
        goto error;
        }

    xnlock_get_irqsave(&nklock, s);

    shm = pse51_shm_get(&desc, fd, 1);

    if(IS_ERR(shm))
        {
        xnlock_put_irqrestore(&nklock, s);
        err = -PTR_ERR(shm);
        goto error;
        }

    desc_flags = pse51_desc_getflags(desc);
    xnlock_put_irqrestore(&nklock, s);
    
    if ((desc_flags != O_RDWR && desc_flags != O_RDONLY) ||
        ((prot & PROT_WRITE) && desc_flags == O_RDONLY))
        {
        err = EACCES;
        goto err_shm_put;
        }    

    if (down_interruptible(&shm->maplock))
        {
        err = EINTR;
        goto err_shm_put;
        }

    if (!shm->addr || off + len > shm->size)
        {
        err = ENXIO;
        goto err_put_lock;
        }

    map = (pse51_shm_map_t *) xnmalloc(sizeof(*map));

    if (!map)
        {
        err = EAGAIN;
        goto err_put_lock;
        }

    /* Align the heap address on a page boundary. */
    result = (void *) PAGE_ALIGN((u_long)shm->addr);
    map->addr = result = (void *)((char *) result + off);
    map->size = len;
    inith(&map->link);
    prependq(&shm->mappings, &map->link);
    up(&shm->maplock);

    return result;


  err_put_lock:
    up(&shm->maplock);
  err_shm_put:
    pse51_shm_put(shm, 1);
  error:
    thread_set_errno(err);
    return MAP_FAILED;
}    

static pse51_shm_t *pse51_shm_lookup(void *addr)
{
    xnholder_t *holder;
    pse51_shm_t *shm = NULL;
    off_t off;
    spl_t s;

    xnlock_get_irqsave(&nklock, s);
    for (holder = getheadq(&pse51_shmq);
         holder;
         holder = nextq(&pse51_shmq, holder))
        {
        shm = link2shm(holder);

        if (!shm->addr)
            continue;

        off = (off_t)(addr - shm->addr);
        if (off >= 0 && off < shm->size)
            break;
        }

    if (!holder)
        {
        xnlock_put_irqrestore(&nklock, s);
        return NULL;
        }

    xnlock_put_irqrestore(&nklock, s);

    return shm;
}

/**
 * Unmap pages of memory.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/munmap.html
 * 
 */
int munmap(void *addr, size_t len)
{
    pse51_shm_map_t *mapping = NULL;
    xnholder_t *holder;
    pse51_shm_t *shm;
    int err;
    spl_t s;

    if (xnpod_asynch_p() || !xnpod_root_p())
        {
        err = EPERM;
        goto error;
        }    

    if (!len)
        {
        err = EINVAL;
        goto error;
        }

    if (((unsigned long) addr) % PAGE_SIZE)
        {
        err = EINVAL;
        goto error;
        }

    xnlock_get_irqsave(&nklock, s);
    shm = pse51_shm_lookup(addr);
    
    if (!shm)
        {
        xnlock_put_irqrestore(&nklock, s);
        err = EINVAL;
        goto error;
        }

    ++shm->nodebase.refcount;
    xnlock_put_irqrestore(&nklock, s);

    if (down_interruptible(&shm->maplock))
        {
        err = EINTR;
        goto err_shm_put;
        }

    for (holder = getheadq(&shm->mappings);
         holder;
         holder = nextq(&shm->mappings, holder))
        {
        mapping = link2map(holder);

        if (mapping->addr == addr && mapping->size == len)
            break;
        }

    if (!holder)
        {
        xnlock_put_irqrestore(&nklock, s);
        err = EINVAL;
        goto err_up;
        }

    removeq(&shm->mappings, holder);    
    up(&shm->maplock);

    xnfree(mapping);
    pse51_shm_put(shm, 2);
    return 0;

  err_up:
    up (&shm->maplock);
  err_shm_put:
    pse51_shm_put(shm, 1);
  error:
    thread_set_errno(err);
    return -1;
}

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
typedef struct {
    unsigned long uobj;
    struct mm_struct *mm;
    unsigned long kobj;

    xnholder_t link;
#define link2assoc(laddr) \
    (pse51_assoc_t *) ((char *)(laddr) - offsetof(pse51_assoc_t, link))

} pse51_assoc_t;

#ifdef CONFIG_SMP
static xnlock_t pse51_assoc_lock;
#endif /* CONIG_SMP */
pse51_assocq_t pse51_umaps;     /* List of user-space mappings. */
pse51_assocq_t pse51_ufds;      /* List of user-space descriptors. */

int pse51_xnheap_get(xnheap_t **pheap, void *addr)
{
    pse51_shm_t *shm;

    shm = pse51_shm_lookup(addr);

    if (!shm)
        return -EBADF;

    *pheap = &shm->heapbase;
    return 0;
}

static int pse51_assoc_lookup_inner(pse51_assocq_t *q,
                                    pse51_assoc_t **passoc,
                                    struct mm_struct *mm,
                                    u_long uobj)
{
    pse51_assoc_t *assoc;
    xnholder_t *holder;
    spl_t s;

    xnlock_get_irqsave(&pse51_assoc_lock, s);

    holder = getheadq(q);

    if (holder)
        {
        do 
            {
            assoc = link2assoc(holder);
            holder = nextq(q, holder);
            }
        while (holder && (assoc->uobj < uobj ||
                          (assoc->uobj == uobj && assoc->mm < mm)));

        if (assoc->mm == mm && assoc->uobj == uobj)
            {
            /* found */
            *passoc = assoc;
            xnlock_put_irqrestore(&pse51_assoc_lock, s);
            return 1;
            }
        }

    /* not found. */
    xnlock_put_irqrestore(&pse51_assoc_lock, s);
    *passoc = holder ? link2assoc(holder) : NULL;
    return 0;
}

int pse51_assoc_create(pse51_assocq_t *q,
                       u_long kobj,
                       struct mm_struct *mm,
                       u_long uobj)
{
    pse51_assoc_t *assoc, *next;
    spl_t s;

    xnlock_get_irqsave(&pse51_assoc_lock, s);

    if (pse51_assoc_lookup_inner(q, &next, mm, uobj))
        {
        xnlock_put_irqrestore(&pse51_assoc_lock, s);
        return -EBUSY;
        }

    assoc = (pse51_assoc_t *) xnmalloc(sizeof(*assoc));
    assoc->mm = mm;
    assoc->uobj = uobj;
    assoc->kobj = kobj;
    inith(&assoc->link);
    if (next)
        insertq(q, &next->link, &assoc->link);
    else
        appendq(q, &assoc->link);

    xnlock_put_irqrestore(&pse51_assoc_lock, s);

    return 0;
}

int pse51_assoc_lookup(pse51_assocq_t *q,
                       u_long *kobj,
                       struct mm_struct *mm,
                       u_long uobj,
                       int destroy)
{
    pse51_assoc_t *assoc;
    spl_t s;

    xnlock_get_irqsave(&pse51_assoc_lock, s);

    if (!pse51_assoc_lookup_inner(q, &assoc, mm, uobj))
        {
        xnlock_put_irqrestore(&pse51_assoc_lock, s);
        return -EBADF;
        }

    *kobj = assoc->kobj;

    if (destroy)
        {
        removeq(q, &assoc->link);
        xnfree(assoc);
        }

    xnlock_put_irqrestore(&pse51_assoc_lock, s);

    return 0;
}

void pse51_assocq_destroy(pse51_assocq_t *q, void (*destroy)(u_long kobj))
{
    pse51_assoc_t *assoc;
    xnholder_t *holder;
    spl_t s;

    xnlock_get_irqsave(&pse51_assoc_lock, s);

    while ((holder = getq(q)))
        {
        assoc = link2assoc(holder);
        if (destroy)
            destroy(assoc->kobj);
        xnfree(assoc);
        }

    xnlock_put_irqrestore(&pse51_assoc_lock, s);
}

#endif /* __KERNEL__ && CONFIG_XENO_OPT_PERVASIVE */    

int pse51_shm_pkg_init(void)
{
    initq(&pse51_shmq);
#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
    xnlock_init(&pse51_assoc_lock);
    pse51_assocq_init(&pse51_umaps);
    pse51_assocq_init(&pse51_ufds);
#endif /* __KERNEL__ && CONFIG_XENO_OPT_PERVASIVE */    

    return 0;
}

void pse51_shm_pkg_cleanup(void)
{
    xnholder_t *holder;

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
    pse51_assocq_destroy(&pse51_umaps, NULL);
    pse51_assocq_destroy(&pse51_ufds, NULL);
#endif /* __KERNEL__ && CONFIG_XENO_OPT_PERVASIVE */

    while ((holder = getheadq(&pse51_shmq)))
        {
        pse51_shm_t *shm = link2shm(holder);
#ifdef CONFIG_XENO_OPT_DEBUG
        xnprintf("POSIX shared memory \"%s\" discarded.\n", shm->nodebase.name);
#endif /* CONFIG_XENO_OPT_DEBUG */
        pse51_shm_destroy(shm, 1);
        }
}

/*@}*/

EXPORT_SYMBOL(shm_open);
EXPORT_SYMBOL(shm_unlink);
EXPORT_SYMBOL(pse51_shm_close);
EXPORT_SYMBOL(ftruncate);
EXPORT_SYMBOL(mmap);
EXPORT_SYMBOL(munmap);