syscall.c

xenomai 很好的linux实时补丁

    
    xnlock_put_irqrestore(&nklock,s);

    return err;
}

int __intr_control (struct task_struct *curr, struct pt_regs *regs)
{
    struct pse51_interrupt *intr = (struct pse51_interrupt *)__xn_reg_arg1(regs);
    int cmd = (int)__xn_reg_arg2(regs);

    return pse51_intr_control(intr,cmd);
}

int __timer_create (struct task_struct *curr, struct pt_regs *regs)
{
    struct sigevent sev;
    timer_t tm;
    int rc;

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

    __xn_copy_from_user(curr, &sev, (char *) __xn_reg_arg2(regs), sizeof(sev));

    rc = timer_create((clockid_t) __xn_reg_arg1(regs), &sev, &tm);

    if(!rc)
        {
        if (!__xn_access_ok(curr, VERIFY_WRITE, __xn_reg_arg3(regs), sizeof(tm)))
            {
            timer_delete(tm);
            return -EFAULT;
            }

        __xn_copy_to_user(curr, (char *) __xn_reg_arg3(regs), &tm, sizeof(tm));
        }

    return rc == 0 ? 0 : -thread_get_errno();
}

int __timer_delete (struct task_struct *curr, struct pt_regs *regs)
{
    int rc;

    rc = timer_delete((timer_t) __xn_reg_arg1(regs));

    return rc == 0 ? 0 : -thread_get_errno();
}

int __timer_settime (struct task_struct *curr, struct pt_regs *regs)
{
    struct itimerspec newv, oldv, *oldvp;
    int rc;

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

    oldvp = __xn_reg_arg4(regs) == 0 ? NULL : &oldv;

    __xn_copy_from_user(curr, &newv, (char *) __xn_reg_arg3(regs), sizeof(newv));

    rc = timer_settime((timer_t) __xn_reg_arg1(regs),
                       (int) __xn_reg_arg2(regs),
                       &newv,
                       oldvp);

    if (!rc && oldvp)
        {
        if(!__xn_access_ok(curr,VERIFY_WRITE,__xn_reg_arg4(regs),sizeof(oldv)))
            {
            timer_settime((timer_t) __xn_reg_arg1(regs),
                          (int) __xn_reg_arg2(regs),
                          oldvp,
                          NULL);

            return -EFAULT;
            }

        __xn_copy_to_user(curr,
                          (char *) __xn_reg_arg4(regs),
                          oldvp,
                          sizeof(oldv));
        }

    return rc == 0 ? 0 : -thread_get_errno();
}

int __timer_gettime (struct task_struct *curr, struct pt_regs *regs)
{
    struct itimerspec val;
    int rc;

    rc = timer_gettime((timer_t) __xn_reg_arg1(regs), &val);

    if (!rc)
        {
        if(!__xn_access_ok(curr,VERIFY_WRITE,__xn_reg_arg2(regs),sizeof(val)))
            return -EFAULT;

        __xn_copy_to_user(curr, (char *) __xn_reg_arg2(regs), &val, sizeof(val));
        }
    
    return rc == 0 ? 0 : -thread_get_errno();
}

int __timer_getoverrun (struct task_struct *curr, struct pt_regs *regs)
{
    int rc;

    rc = timer_getoverrun((timer_t) __xn_reg_arg1(regs));

    return rc >= 0 ? rc : -thread_get_errno();
}

/* shm_open(name, oflag, mode, ufd) */
int __shm_open (struct task_struct *curr, struct pt_regs *regs)
{
    int ufd, kfd, oflag, err;
    char name[PSE51_MAXNAME];
    unsigned len;
    mode_t mode;

    len = __xn_strncpy_from_user(curr,
                                 name,
                                 (const char __user *)__xn_reg_arg1(regs),
                                 sizeof(name));
    if (len <= 0)
        return -EFAULT;

    if (len >= sizeof(name))
        return -ENAMETOOLONG;

    oflag = (int) __xn_reg_arg2(regs);
    mode = (mode_t) __xn_reg_arg3(regs);
    
    kfd = shm_open(name, oflag, mode);

    if (kfd == -1)
        return -thread_get_errno();

    ufd = (int) __xn_reg_arg4(regs);

    err = pse51_assoc_create(&pse51_ufds, (u_long) kfd, curr->mm, (u_long) ufd);
    /* pse51_assoc_create returning an error means that the same mm and user
       file descriptor are already registered. That is impossible. */
    BUG_ON(err);
    return 0;
}

/* shm_unlink(name) */
int __shm_unlink (struct task_struct *curr, struct pt_regs *regs)
{
    char name[PSE51_MAXNAME];
    unsigned len;
    int err;

    len = __xn_strncpy_from_user(curr,
                                 name,
                                 (const char __user *)__xn_reg_arg1(regs),
                                 sizeof(name));
    if (len <= 0)
        return -EFAULT;

    if (len >= sizeof(name))
        return -ENAMETOOLONG;

    err = shm_unlink(name);

    return !err ? 0 : -thread_get_errno();
}

/* shm_close(ufd) */
int __shm_close (struct task_struct *curr, struct pt_regs *regs)
{
    unsigned long kfd;
    int ufd, err;

    ufd = (int) __xn_reg_arg1(regs);

    err = pse51_assoc_lookup(&pse51_ufds, &kfd, curr->mm, (u_long) ufd, 1);

    if (err)
        return err;

    err = close(kfd);

    return !err ? 0 : -thread_get_errno();
}

/* ftruncate(ufd, len) */
int __ftruncate (struct task_struct *curr, struct pt_regs *regs)
{
    unsigned long kfd;
    int ufd, err;
    off_t len;

    ufd = (int) __xn_reg_arg1(regs);
    len = (off_t) __xn_reg_arg2(regs);

    err = pse51_assoc_lookup(&pse51_ufds, &kfd, curr->mm, (u_long) ufd, 0);

    if (err)
        return err;

    err = ftruncate(kfd, len);

    return !err ? 0 : -thread_get_errno();
}

typedef struct {
    void *kaddr;
    unsigned long len;
    xnheap_t *ioctl_cookie;
    unsigned long heapsize;
    unsigned long offset;
} pse51_umap_t;

/* mmap_prologue(len, ufd, off, pse51_umap_t *umap) */
int __mmap_prologue (struct task_struct *curr, struct pt_regs *regs)
{
    unsigned long kfd;
    pse51_umap_t umap;
    int ufd, err;
    size_t len;
    off_t off;

    len = (size_t) __xn_reg_arg1(regs);
    ufd = (int) __xn_reg_arg2(regs);
    off = (off_t) __xn_reg_arg3(regs);

    if(!__xn_access_ok(curr,VERIFY_WRITE,__xn_reg_arg4(regs),sizeof(umap)))
        return -EFAULT;    

    err = pse51_assoc_lookup(&pse51_ufds, &kfd, curr->mm, (u_long) ufd, 0);

    if (err)
        return err;

    /* We do not care for the real flags and protection, this mapping is a
       placeholder. */  
    umap.kaddr = mmap(NULL,len,PROT_READ | PROT_WRITE,MAP_SHARED,kfd,off);

    if (umap.kaddr == MAP_FAILED)
        return -thread_get_errno();

    if ((err = pse51_xnheap_get(&umap.ioctl_cookie, umap.kaddr)))
        {
        munmap(umap.kaddr, len);
        return err;
        }

    umap.len = len;
    umap.heapsize = xnheap_size(umap.ioctl_cookie);
    umap.offset = xnheap_mapped_offset(umap.ioctl_cookie, umap.kaddr);

    __xn_copy_to_user(curr,
                      (void __user *)__xn_reg_arg4(regs),
                      &umap,
                      sizeof(umap));

    return 0;
}

/* mmap_epilogue(uaddr, pse51_umap_t *umap) */
int __mmap_epilogue (struct task_struct *curr, struct pt_regs *regs)
{
    pse51_umap_t umap;
    void *uaddr;
    int err;

    uaddr = (void *) __xn_reg_arg1(regs);
 
    if(!__xn_access_ok(curr,VERIFY_READ,__xn_reg_arg2(regs),sizeof(umap)))
        return -EFAULT;

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

    if (uaddr == MAP_FAILED)
        {
        munmap(umap.kaddr, umap.len);
        return 0;
        }
    
    err = pse51_assoc_create(&pse51_umaps,
                             (u_long) umap.kaddr,
                             curr->mm,
                             (u_long) uaddr);
    BUG_ON(err);
    return 0;
}

/* munmap_prologue(uaddr, len, &unmap) */
int __munmap_prologue (struct task_struct *curr, struct pt_regs *regs)
{
    struct {
        unsigned long mapsize;
        unsigned long offset;
    } uunmap;
    unsigned long uaddr;
    xnheap_t *heap;
    void *kaddr;
    size_t len;
    int err;

    uaddr = (unsigned long) __xn_reg_arg1(regs);
    len = (size_t) __xn_reg_arg2(regs);
    if (!__xn_access_ok(curr,VERIFY_WRITE,__xn_reg_arg3(regs),sizeof(uunmap)))
        return -EFAULT;

    err = pse51_assoc_lookup(&pse51_umaps, (u_long *)&kaddr, curr->mm, uaddr, 0);

    if (err)
        return err;

    err = pse51_xnheap_get(&heap, kaddr);

    if (err)
        return err;
   
    uunmap.mapsize = xnheap_size(heap);
    uunmap.offset = xnheap_mapped_offset(heap, kaddr);
    __xn_copy_to_user(curr,
                      (void __user *)__xn_reg_arg3(regs),
                      &uunmap,
                      sizeof(uunmap));

    return 0;
}

/* munmap_epilogue(uaddr, len) */
int __munmap_epilogue (struct task_struct *curr, struct pt_regs *regs)
{
    unsigned long uaddr;
    void *kaddr;
    size_t len;
    int err;

    uaddr = (unsigned long) __xn_reg_arg1(regs);
    len = (size_t) __xn_reg_arg2(regs);

    err = pse51_assoc_lookup(&pse51_umaps, (u_long *)&kaddr, curr->mm, uaddr, 1);

    if (err)
        return err;

    err = munmap(kaddr, len);

    return !err ? 0 : -thread_get_errno();
}

#if 0
int __itimer_set (struct task_struct *curr, struct pt_regs *regs)
{
    pthread_t thread = pse51_current_thread();
    xnticks_t delay, interval;
    struct itimerval itv;

    if (__xn_reg_arg1(regs))
        {
        if (!__xn_access_ok(curr,VERIFY_READ,(void *)__xn_reg_arg1(regs),sizeof(itv)))
            return -EFAULT;

        __xn_copy_from_user(curr,&itv,(void *)__xn_reg_arg1(regs),sizeof(itv));
        }
    else
        memset(&itv,0,sizeof(itv));

    if (__xn_reg_arg2(regs) &&
        !__xn_access_ok(curr,VERIFY_WRITE,(void *)__xn_reg_arg2(regs),sizeof(itv)))
        return -EFAULT;

    xntimer_stop(&thread->itimer);

    delay = xnshadow_tv2ticks(&itv.it_value);
    interval = xnshadow_tv2ticks(&itv.it_interval);

    if (delay > 0)
        xntimer_start(&thread->itimer,delay,interval);

    if (__xn_reg_arg2(regs))
        {
        interval = xntimer_interval(&thread->itimer);

        if (xntimer_running_p(&thread->itimer))
            {
            delay = xntimer_get_timeout(&thread->itimer);
            
            if (delay == 0)
                delay = 1;
            }
        else
            delay = 0;

        xnshadow_ticks2tv(delay,&itv.it_value);
        xnshadow_ticks2tv(interval,&itv.it_interval);
        __xn_copy_to_user(curr,(void *)__xn_reg_arg2(regs),&itv,sizeof(itv));
        }

    return 0;
}

int __itimer_get (struct task_struct *curr, struct pt_regs *regs)
{
    pthread_t thread = pse51_current_thread();
    xnticks_t delay, interval;
    struct itimerval itv;

    if (!__xn_access_ok(curr,VERIFY_WRITE,(void *)__xn_reg_arg1(regs),sizeof(itv)))
        return -EFAULT;

    interval = xntimer_interval(&thread->itimer);

    if (xntimer_running_p(&thread->itimer))
        {
        delay = xntimer_get_timeout(&thread->itimer);
        
        if (delay == 0) /* Cannot be negative in this context. */
            delay = 1;
        }
    else
        delay = 0;

    xnshadow_ticks2tv(delay,&itv.it_value);
    xnshadow_ticks2tv(interval,&itv.it_interval);
    __xn_copy_to_user(curr,(void *)__xn_reg_arg1(regs),&itv,sizeof(itv));

    return 0;
}
#endif

static xnsysent_t __systab[] = {
    [__pse51_thread_create ] = { &__pthread_create, __xn_exec_init },
    [__pse51_thread_detach ] = { &__pthread_detach, __xn_exec_any },
    [__pse51_thread_setschedparam ] = { &__pthread_setschedparam, __xn_exec_conforming },
    [__pse51_sched_yield ] = { &__sched_yield, __xn_exec_primary },
    [__pse51_thread_make_periodic ] = { &__pthread_make_periodic_np, __xn_exec_primary },
    [__pse51_thread_wait] = { &__pthread_wait_np, __xn_exec_primary },
    [__pse51_thread_set_mode] = { &__pthread_set_mode_np, __xn_exec_primary },
    [__pse51_thread_set_name] = { &__pthread_set_name_np, __xn_exec_any },
    [__pse51_sem_init] = { &__sem_init, __xn_exec_any },
    [__pse51_sem_destroy] = { &__sem_destroy, __xn_exec_any },
    [__pse51_sem_post] = { &__sem_post, __xn_exec_any },
    [__pse51_sem_wait] = { &__sem_wait, __xn_exec_primary },
    [__pse51_sem_timedwait] = { &__sem_timedwait, __xn_exec_primary },
    [__pse51_sem_trywait] = { &__sem_trywait, __xn_exec_primary },
    [__pse51_sem_getvalue] = { &__sem_getvalue, __xn_exec_any },
    [__pse51_sem_open] = { &__sem_open, __xn_exec_any },
    [__pse51_sem_close] = { &__sem_close, __xn_exec_any },
    [__pse51_sem_unlink] = { &__sem_unlink, __xn_exec_any },
    [__pse51_clock_getres] = { &__clock_getres, __xn_exec_any },
    [__pse51_clock_gettime] = { &__clock_gettime, __xn_exec_any },
    [__pse51_clock_settime] = { &__clock_settime, __xn_exec_any },
    [__pse51_clock_nanosleep] = { &__clock_nanosleep, __xn_exec_primary },
    [__pse51_mutex_init] = { &__mutex_init, __xn_exec_any },
    [__pse51_mutex_destroy] = { &__mutex_destroy, __xn_exec_any },
    [__pse51_mutex_lock] = { &__mutex_lock, __xn_exec_primary },
    [__pse51_mutex_timedlock] = { &__mutex_timedlock, __xn_exec_primary },
    [__pse51_mutex_trylock] = { &__mutex_trylock, __xn_exec_primary },
    [__pse51_mutex_unlock] = { &__mutex_unlock, __xn_exec_primary },
    [__pse51_cond_init] = { &__cond_init, __xn_exec_any },
    [__pse51_cond_destroy] = { &__cond_destroy, __xn_exec_any },
    [__pse51_cond_wait] = { &__cond_wait, __xn_exec_primary },
    [__pse51_cond_timedwait] = { &__cond_timedwait, __xn_exec_primary },
    [__pse51_cond_signal] = { &__cond_signal, __xn_exec_any },
    [__pse51_cond_broadcast] = { &__cond_broadcast, __xn_exec_any },
    [__pse51_mq_open] = { &__mq_open, __xn_exec_lostage },
    [__pse51_mq_close] = { &__mq_close, __xn_exec_lostage },
    [__pse51_mq_unlink] = { &__mq_unlink, __xn_exec_lostage },
    [__pse51_mq_getattr] = { &__mq_getattr, __xn_exec_any },
    [__pse51_mq_setattr] = { &__mq_setattr, __xn_exec_any },
    [__pse51_mq_send] = { &__mq_send, __xn_exec_primary },
    [__pse51_mq_timedsend] = { &__mq_timedsend, __xn_exec_primary },
    [__pse51_mq_receive] = { &__mq_receive, __xn_exec_primary },
    [__pse51_mq_timedreceive] = { &__mq_timedreceive, __xn_exec_primary },
    [__pse51_mq_notify] = { &__mq_notify, __xn_exec_any },
    [__pse51_intr_attach] = { &__intr_attach, __xn_exec_any },
    [__pse51_intr_detach] = { &__intr_detach, __xn_exec_any },
    [__pse51_intr_wait] = { &__intr_wait, __xn_exec_primary },
    [__pse51_intr_control] = { &__intr_control, __xn_exec_any },
    [__pse51_timer_create] = { &__timer_create, __xn_exec_any },
    [__pse51_timer_delete] = { &__timer_delete, __xn_exec_any },
    [__pse51_timer_settime] = { &__timer_settime, __xn_exec_primary },
    [__pse51_timer_gettime] = { &__timer_gettime, __xn_exec_any },
    [__pse51_timer_getoverrun] = { &__timer_getoverrun, __xn_exec_any },
    [__pse51_shm_open] = {&__shm_open, __xn_exec_lostage },
    [__pse51_shm_unlink] = {&__shm_unlink, __xn_exec_lostage },
    [__pse51_shm_close] = {&__shm_close, __xn_exec_lostage },
    [__pse51_ftruncate] = {&__ftruncate, __xn_exec_lostage },
    [__pse51_mmap_prologue] = {&__mmap_prologue, __xn_exec_lostage },
    [__pse51_mmap_epilogue] = {&__mmap_epilogue, __xn_exec_lostage },
    [__pse51_munmap_prologue] = {&__munmap_prologue, __xn_exec_lostage },
    [__pse51_munmap_epilogue] = {&__munmap_epilogue, __xn_exec_lostage },
};

static void __shadow_delete_hook (xnthread_t *thread)

{
    if (xnthread_get_magic(thread) == PSE51_SKIN_MAGIC &&
        testbits(thread->status,XNSHADOW))
        {
        pthread_t k_tid = thread2pthread(thread);
        __pthread_unhash(&k_tid->hkey);
        xnshadow_unmap(thread);
        }
}

int pse51_syscall_init (void)

{
    __muxid =
        xnshadow_register_interface("posix",
                                    PSE51_SKIN_MAGIC,
                                    sizeof(__systab) / sizeof(__systab[0]),
                                    __systab,
                                    NULL);
    if (__muxid < 0)
        return -ENOSYS;

    xnpod_add_hook(XNHOOK_THREAD_DELETE,&__shadow_delete_hook);

    __pse51_errptd = rthal_alloc_ptdkey();
    
    return 0;
}

void pse51_syscall_cleanup (void)

{
    xnpod_remove_hook(XNHOOK_THREAD_DELETE,&__shadow_delete_hook);
    xnshadow_unregister_interface(__muxid);
    rthal_free_ptdkey(__pse51_errptd);
}