syscall.c

xenomai 很好的linux实时补丁

/**
 * @file
 * This file is part of the Xenomai project.
 *
 * Copyright (C) 2005 Philippe Gerum <[e-mail]> 
 * Copyright (C) 2005 Gilles Chanteperdrix <[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.
 */

#include <posix/syscall.h>
#include <posix/posix.h>
#include <posix/thread.h>
#include <posix/mutex.h>
#include <posix/cond.h>
#include <posix/jhash.h>
#include <posix/mq.h>
#include <posix/intr.h>
#include <posix/registry.h>     /* For PSE51_MAXNAME. */
#include <posix/sem.h>
#include <posix/shm.h>

static int __muxid;

int __pse51_errptd;

struct pthread_jhash {

#define PTHREAD_HASHBITS 8

    pthread_t k_tid;
    struct pse51_hkey hkey;
    struct pthread_jhash *next;
};

static struct pthread_jhash *__jhash_buckets[1<<PTHREAD_HASHBITS]; /* Guaranteed zero */

/* We want to keep the native pthread_t token unmodified for
   Xenomai mapped threads, and keep it pointing at a genuine
   NPTL/LinuxThreads descriptor, so that portions of the POSIX
   interface which are not overriden by Xenomai fall back to the
   original Linux services. If the latter invoke Linux system calls,
   the associated shadow thread will simply switch to secondary exec
   mode to perform them. For this reason, we need an external index to
   map regular pthread_t values to Xenomai's internal thread ids used
   in syscalling the POSIX skin, so that the outer interface can keep
   on using the former transparently. Semaphores and mutexes do not
   have this constraint, since we fully override their respective
   interfaces with Xenomai-based replacements. */

static inline struct pthread_jhash *__pthread_hash (const struct pse51_hkey *hkey,
                                                    pthread_t k_tid)
{
    struct pthread_jhash **bucketp;
    struct pthread_jhash *slot;
    u32 hash;
    spl_t s;

    slot = (struct pthread_jhash *)xnmalloc(sizeof(*slot));

    if (!slot)
        return NULL;

    slot->hkey = *hkey;
    slot->k_tid = k_tid;

    hash = jhash2((u32 *)&slot->hkey,
                  sizeof(slot->hkey)/sizeof(u32),
                  0);

    bucketp = &__jhash_buckets[hash&((1<<PTHREAD_HASHBITS)-1)];

    xnlock_get_irqsave(&nklock,s);
    slot->next = *bucketp;
    *bucketp = slot;
    xnlock_put_irqrestore(&nklock,s);

    return slot;
}

static inline void __pthread_unhash (const struct pse51_hkey *hkey)

{
    struct pthread_jhash **tail, *slot;
    u32 hash;
    spl_t s;

    hash = jhash2((u32 *)hkey,
                  sizeof(*hkey)/sizeof(u32),
                  0);

    tail = &__jhash_buckets[hash&((1<<PTHREAD_HASHBITS)-1)];

    xnlock_get_irqsave(&nklock,s);

    slot = *tail;

    while (slot != NULL &&
           (slot->hkey.u_tid != hkey->u_tid ||
            slot->hkey.mm != hkey->mm))
        {
        tail = &slot->next;
        slot = *tail;
        }

    if (slot)
        *tail = slot->next;

    xnlock_put_irqrestore(&nklock,s);

    if (slot)
        xnfree(slot);
}

static pthread_t __pthread_find (const struct pse51_hkey *hkey)

{
    struct pthread_jhash *slot;
    pthread_t k_tid;
    u32 hash;
    spl_t s;

    hash = jhash2((u32 *)hkey,
                  sizeof(*hkey)/sizeof(u32),
                  0);

    xnlock_get_irqsave(&nklock,s);

    slot = __jhash_buckets[hash&((1<<PTHREAD_HASHBITS)-1)];

    while (slot != NULL &&
           (slot->hkey.u_tid != hkey->u_tid ||
            slot->hkey.mm != hkey->mm))
        slot = slot->next;

    k_tid = slot ? slot->k_tid : NULL;

    xnlock_put_irqrestore(&nklock,s);

    return k_tid;
}

int __pthread_create (struct task_struct *curr, struct pt_regs *regs)

{
    struct sched_param param;
    struct pse51_hkey hkey;
    pthread_attr_t attr;
    pthread_t k_tid;
    int err;

    if (curr->policy != SCHED_FIFO) /* Only allow FIFO for now. */
        return -EINVAL;

    /* We have been passed the pthread_t identifier the user-space
       POSIX library has assigned to our caller; we'll index our
       internal pthread_t descriptor in kernel space on it. */
    hkey.u_tid = __xn_reg_arg1(regs);
    hkey.mm = curr->mm;

    /* Build a default thread attribute, then make sure that a few
       critical fields are set in a compatible fashion wrt to the
       calling context. */

    pthread_attr_init(&attr);
    attr.policy = curr->policy;
    param.sched_priority = curr->rt_priority;
    attr.detachstate = PTHREAD_CREATE_DETACHED;
    attr.schedparam = param;
    attr.fp = 1;
    attr.name = curr->comm;

    err = pthread_create(&k_tid,&attr,NULL,NULL);

    if (err)
        return -err; /* Conventionally, our error codes are negative. */

    err = xnshadow_map(&k_tid->threadbase,NULL);

    if (!err && !__pthread_hash(&hkey,k_tid))
        err = -ENOMEM;

    if (err)
        pse51_thread_abort(k_tid, NULL);
    else
        k_tid->hkey = hkey;
        
    return err;
}

int __pthread_detach (struct task_struct *curr, struct pt_regs *regs)

{ 
    struct pse51_hkey hkey;
    pthread_t k_tid;

    hkey.u_tid = __xn_reg_arg1(regs);
    hkey.mm = curr->mm;
    k_tid = __pthread_find(&hkey);

    return -pthread_detach(k_tid);
}

static int __pthread_shadow (struct task_struct *curr,
                             struct pse51_hkey *hkey,
                             struct sched_param *param)
{
    pthread_attr_t attr;
    pthread_t k_tid;
    int err;

    pthread_attr_init(&attr);
    attr.policy = SCHED_FIFO;
    attr.schedparam = *param;
    attr.fp = 1;
    attr.name = curr->comm;

    err = pthread_create(&k_tid,&attr,NULL,NULL);

    if (err)
        return -err;

    err = xnshadow_map(&k_tid->threadbase,NULL);

    if (!err && !__pthread_hash(hkey,k_tid))
        err = -ENOMEM;

    if (err)
        pse51_thread_abort(k_tid, NULL);
    else
        k_tid->hkey = *hkey;
        
    return err;
}

int __pthread_setschedparam (struct task_struct *curr, struct pt_regs *regs)

{ 
    int policy, err, promoted = 0;
    struct sched_param param;
    struct pse51_hkey hkey;
    pthread_t k_tid;

    policy = __xn_reg_arg2(regs);

    if (policy != SCHED_FIFO)
        /* User-space POSIX shadows only support SCHED_FIFO for now. */
        return -EINVAL;

    if (!__xn_access_ok(curr,VERIFY_READ,__xn_reg_arg3(regs),sizeof(param)))
        return -EFAULT;

    __xn_copy_from_user(curr,
                        &param,
                        (void __user *)__xn_reg_arg3(regs),
                        sizeof(param));

    hkey.u_tid = __xn_reg_arg1(regs);
    hkey.mm = curr->mm;
    k_tid = __pthread_find(&hkey);

    if (!k_tid && __xn_reg_arg1(regs) == __xn_reg_arg4(regs))
        {
        /* If the syscall applies to "current", and the latter is not
           a Xenomai thread already, then shadow it. */
        err = __pthread_shadow(curr,&hkey,&param);
        promoted = 1;
        }
    else
        err = -pthread_setschedparam(k_tid,policy,&param);

    if (!err)
        __xn_put_user(curr,promoted,(int __user *)__xn_reg_arg5(regs));

    return err;
}

int __sched_yield (struct task_struct *curr, struct pt_regs *regs)

{
    return -sched_yield();
}

int __pthread_make_periodic_np (struct task_struct *curr, struct pt_regs *regs)

{ 
    struct timespec startt, periodt;
    struct pse51_hkey hkey;
    pthread_t k_tid;

    if (!__xn_access_ok(curr,VERIFY_READ,__xn_reg_arg2(regs),sizeof(startt)))
        return -EFAULT;

    if (!__xn_access_ok(curr,VERIFY_READ,__xn_reg_arg3(regs),sizeof(periodt)))
        return -EFAULT;

    hkey.u_tid = __xn_reg_arg1(regs);
    hkey.mm = curr->mm;
    k_tid = __pthread_find(&hkey);

    __xn_copy_from_user(curr,
                        &startt,
                        (void __user *)__xn_reg_arg2(regs),
                        sizeof(startt));

    __xn_copy_from_user(curr,
                        &periodt,
                        (void __user *)__xn_reg_arg3(regs),
                        sizeof(periodt));

    return -pthread_make_periodic_np(k_tid,&startt,&periodt);
}

int __pthread_wait_np (struct task_struct *curr, struct pt_regs *regs)

{
    unsigned long overruns;
    int err;

    if (__xn_reg_arg1(regs) &&
	!__xn_access_ok(curr,VERIFY_WRITE,__xn_reg_arg1(regs),sizeof(overruns)))
	return -EFAULT;

    err = -pthread_wait_np(&overruns);

    if (__xn_reg_arg1(regs) && (err == 0 || err == -ETIMEDOUT))
	__xn_copy_to_user(curr,(void __user *)__xn_reg_arg1(regs),&overruns,sizeof(overruns));

    return err;
}

int __pthread_set_mode_np (struct task_struct *curr, struct pt_regs *regs)

{
    xnflags_t clrmask, setmask;
    struct pse51_hkey hkey;
    pthread_t k_tid;
    int err;

    hkey.u_tid = __xn_reg_arg1(regs); /* always pthread_self() */
    hkey.mm = curr->mm;
    k_tid = __pthread_find(&hkey);
    clrmask = __xn_reg_arg2(regs);
    setmask = __xn_reg_arg3(regs);

    /* XNTHREAD_SPARE1 is used for primary mode switch. */

    if ((clrmask & ~(XNSHIELD|XNTRAPSW|XNTHREAD_SPARE1)) != 0 ||
        (setmask & ~(XNSHIELD|XNTRAPSW|XNTHREAD_SPARE1)) != 0)
        return -EINVAL;

    err = xnpod_set_thread_mode(&k_tid->threadbase,
                                clrmask & ~XNTHREAD_SPARE1,
                                setmask & ~XNTHREAD_SPARE1);

    if ((clrmask & XNTHREAD_SPARE1) != 0)
        xnshadow_relax(0);

    return err;
}

int __pthread_set_name_np (struct task_struct *curr, struct pt_regs *regs)

{ 
    char name[XNOBJECT_NAME_LEN];
    struct pse51_hkey hkey;
    pthread_t k_tid;
    spl_t s;

    if (!__xn_access_ok(curr,VERIFY_READ,__xn_reg_arg2(regs),sizeof(name)))
        return -EFAULT;

    __xn_strncpy_from_user(curr,name,(const char __user *)__xn_reg_arg2(regs),sizeof(name) - 1);
    name[sizeof(name) - 1] = '\0';

    hkey.u_tid = __xn_reg_arg1(regs);
    hkey.mm = curr->mm;
    k_tid = __pthread_find(&hkey);

    xnlock_get_irqsave(&nklock, s);

    if (!pse51_obj_active(k_tid, PSE51_THREAD_MAGIC, struct pse51_thread))
        {
        xnlock_put_irqrestore(&nklock, s);
        return -ESRCH;
        }

    strcpy(xnthread_name(&k_tid->threadbase),name);

    xnlock_put_irqrestore(&nklock, s);

    return 0;
}

int __sem_init (struct task_struct *curr, struct pt_regs *regs)

{
    union __xeno_sem sm, *usm;
    unsigned value;
    int pshared;

    usm = (union __xeno_sem *) __xn_reg_arg1(regs);
    
    if (!__xn_access_ok(curr,VERIFY_WRITE,(void __user *) usm,sizeof(*usm)))
        return -EFAULT;

    pshared = (int)__xn_reg_arg2(regs);
    value = (unsigned)__xn_reg_arg3(regs);

    if (sem_init(&sm.native_sem,pshared,value) == -1)
        return -thread_get_errno();

    __xn_copy_to_user(curr,
                      (void __user *)&usm->shadow_sem,
                      &sm.shadow_sem,
                      sizeof(usm->shadow_sem));

    return 0;
}

int __sem_post (struct task_struct *curr, struct pt_regs *regs)

{
    union __xeno_sem sm, *usm;

    usm = (union __xeno_sem *) __xn_reg_arg1(regs);

    if (!__xn_access_ok(curr,VERIFY_READ,(void __user *) usm,sizeof(*usm)))
        return -EFAULT;

    __xn_copy_from_user(curr,
                        &sm.shadow_sem,
                        (void __user*)&usm->shadow_sem,
                        sizeof(sm.shadow_sem));

    return sem_post(&sm.native_sem) == 0 ? 0 : -thread_get_errno();
}

int __sem_wait (struct task_struct *curr, struct pt_regs *regs)

{
    union __xeno_sem sm, *usm;

    usm = (union __xeno_sem *) __xn_reg_arg1(regs);

    if (!__xn_access_ok(curr,VERIFY_READ,(void __user *) usm,sizeof(*usm)))
        return -EFAULT;

    __xn_copy_from_user(curr,
                        &sm.shadow_sem,
                        (void __user*)&usm->shadow_sem,
                        sizeof(sm.shadow_sem));

    return sem_wait(&sm.native_sem) == 0 ? 0 : -thread_get_errno();
}

int __sem_timedwait (struct task_struct *curr, struct pt_regs *regs)

{
    union __xeno_sem sm, *usm;
    struct timespec ts;

    usm = (union __xeno_sem *) __xn_reg_arg1(regs);

    if (!__xn_access_ok(curr,VERIFY_READ,(void __user *) usm,sizeof(*usm)))
        return -EFAULT;

    if (!__xn_access_ok(curr,VERIFY_READ,__xn_reg_arg2(regs),sizeof(ts)))
        return -EFAULT;

    __xn_copy_from_user(curr,
                        &sm.shadow_sem,
                        (void __user*)&usm->shadow_sem,
                        sizeof(sm.shadow_sem));

    __xn_copy_from_user(curr,
                        &ts,
                        (void __user *)__xn_reg_arg2(regs),
                        sizeof(ts));

    return sem_timedwait(&sm.native_sem, &ts) == 0 ? : -thread_get_errno();
}

int __sem_trywait (struct task_struct *curr, struct pt_regs *regs)

{
    union __xeno_sem sm, *usm;

    usm = (union __xeno_sem *) __xn_reg_arg1(regs);

    if (!__xn_access_ok(curr,VERIFY_READ,(void __user *) usm,sizeof(*usm)))
        return -EFAULT;

    __xn_copy_from_user(curr,
                        &sm.shadow_sem,
                        (void __user*)&usm->shadow_sem,
                        sizeof(sm.shadow_sem));

    return sem_trywait(&sm.native_sem) == 0 ? 0 : -thread_get_errno();
}

int __sem_getvalue (struct task_struct *curr, struct pt_regs *regs)

{
    union __xeno_sem sm, *usm;
    int err, sval;

    usm = (union __xeno_sem *) __xn_reg_arg1(regs);

    if (!__xn_access_ok(curr,VERIFY_READ,(void __user *) usm,sizeof(*usm)))
        return -EFAULT;

    if (!__xn_access_ok(curr,VERIFY_WRITE,__xn_reg_arg2(regs),sizeof(sval)))
        return -EFAULT;

    __xn_copy_from_user(curr,
                        &sm.shadow_sem,
                        (void __user*)&usm->shadow_sem,
                        sizeof(sm.shadow_sem));

    err = sem_getvalue(&sm.native_sem,&sval);

    if (err)
        return -thread_get_errno();

    __xn_copy_to_user(curr,
                      (void __user *)__xn_reg_arg2(regs),