syscall.c

xenomai 很好的linux实时补丁

}

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

{
    union __xeno_cond cnd, *ucnd;

    ucnd = (union __xeno_cond *) __xn_reg_arg1(regs);
    
    if (!__xn_access_ok(curr,VERIFY_READ,(void __user *) ucnd,sizeof(*ucnd)))
        return -EFAULT;

    __xn_copy_from_user(curr,
                        &cnd.shadow_cond,
                        (void __user *) &ucnd->shadow_cond,
                        sizeof(cnd.shadow_cond));

    return -pthread_cond_broadcast(&cnd.native_cond);
}

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

{
    char name[PSE51_MAXNAME];
    struct mq_attr attr;
    unsigned len;
    mode_t mode;
    int oflags;
    mqd_t q;

    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;
    
    oflags = __xn_reg_arg2(regs);
    mode = __xn_reg_arg3(regs);

    if (__xn_reg_arg4(regs))
        {
        if (!__xn_access_ok(curr,VERIFY_READ,__xn_reg_arg4(regs),sizeof(attr)))
            return -EFAULT;

        __xn_copy_from_user(curr,&attr,(struct mq_attr *)__xn_reg_arg4(regs),sizeof(attr));
        }
    else
        {
        /* Won't be used, but still, we make sure that it can't be
           used. */
        attr.mq_flags = 0;
        attr.mq_maxmsg = 0;
        attr.mq_msgsize = 0;
        attr.mq_curmsgs = 0;
        }

    q = mq_open(name,oflags,mode,&attr);

    return q == (mqd_t)-1 ? -thread_get_errno() : q;
}

int __mq_close (struct task_struct *curr, struct pt_regs *regs)
{
    mqd_t q;
    int err;
    
    q = (mqd_t) __xn_reg_arg1(regs);
    err = mq_close(q);

    return err ? -thread_get_errno() : 0;
}

int __mq_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 = mq_unlink(name);

    return err ? -thread_get_errno() : 0;
}

int __mq_getattr (struct task_struct *curr, struct pt_regs *regs)
{
    struct mq_attr attr;
    mqd_t q;
    int err;

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

    q = (mqd_t) __xn_reg_arg1(regs);

    err = mq_getattr(q,&attr);

    if (err)
        return -thread_get_errno();

    __xn_copy_to_user(curr,
                      (void __user *)__xn_reg_arg2(regs),
                      &attr,
                      sizeof(attr));
    return 0;
}

int __mq_setattr (struct task_struct *curr, struct pt_regs *regs)
{
    struct mq_attr attr, oattr;
    mqd_t q;
    int err;

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

    q = (mqd_t) __xn_reg_arg1(regs);

    __xn_copy_from_user(curr,&attr,(struct mq_attr *)__xn_reg_arg2(regs),sizeof(attr));

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

    err = mq_setattr(q,&attr,&oattr);

    if (err)
        return -thread_get_errno();

    if (__xn_reg_arg3(regs))
        __xn_copy_to_user(curr,
                          (void __user *)__xn_reg_arg3(regs),
                          &oattr,
                          sizeof(oattr));
    return 0;
}

int __mq_send (struct task_struct *curr, struct pt_regs *regs)
{
    char tmp_buf[PSE51_MQ_FSTORE_LIMIT];
    caddr_t tmp_area;
    unsigned prio;
    size_t len;
    int err;
    mqd_t q;

    q = (mqd_t) __xn_reg_arg1(regs);
    len = (size_t) __xn_reg_arg3(regs);
    prio = __xn_reg_arg4(regs);

    if (len > 0)
        {
        if (!__xn_access_ok(curr,VERIFY_READ,__xn_reg_arg2(regs),len))
            return -EFAULT;

        /* Try optimizing a bit here: if the message size can fit into
           our local buffer, use the latter; otherwise, take the slow
           path and fetch a larger buffer from the system heap. Most
           messages are expected to be short enough to fit on the
           stack anyway. */

        if (len <= sizeof(tmp_buf))
            tmp_area = tmp_buf;
        else
            {
            tmp_area = xnmalloc(len);

            if (!tmp_area)
                return -ENOMEM;
            }

        __xn_copy_from_user(curr,tmp_area,(void __user *)__xn_reg_arg2(regs),len);
        }
    else
        tmp_area = NULL;

    err = mq_send(q,tmp_area,len,prio);

    if (tmp_area && tmp_area != tmp_buf)
        xnfree(tmp_area);

    return err ? -thread_get_errno() : 0;
}

int __mq_timedsend (struct task_struct *curr, struct pt_regs *regs)
{
    struct timespec timeout, *timeoutp;
    char tmp_buf[PSE51_MQ_FSTORE_LIMIT];
    caddr_t tmp_area;
    unsigned prio;
    size_t len;
    int err;
    mqd_t q;

    if (__xn_reg_arg5(regs) &&
        !__xn_access_ok(curr,VERIFY_READ,__xn_reg_arg5(regs),sizeof(timeout)))
        return -EFAULT;

    q = (mqd_t) __xn_reg_arg1(regs);
    len = (size_t)__xn_reg_arg3(regs);
    prio = __xn_reg_arg4(regs);

    if (len > 0)
        {
        if (!__xn_access_ok(curr,VERIFY_READ,__xn_reg_arg2(regs),len))
            return -EFAULT;

        if (len <= sizeof(tmp_buf))
            tmp_area = tmp_buf;
        else
            {
            tmp_area = xnmalloc(len);

            if (!tmp_area)
                return -ENOMEM;
            }

        __xn_copy_from_user(curr,tmp_area,(void __user *)__xn_reg_arg2(regs),len);
        }
    else
        tmp_area = NULL;

    if (__xn_reg_arg5(regs))
        {
        __xn_copy_from_user(curr,
                            &timeout,
                            (struct timespec __user *)__xn_reg_arg5(regs),
                            sizeof(timeout));
        timeoutp = &timeout;
        }
    else
        timeoutp = NULL;

    err = mq_timedsend(q,tmp_area,len,prio,timeoutp);

    if (tmp_area && tmp_area != tmp_buf)
        xnfree(tmp_area);

    return err ? -thread_get_errno() : 0;
}

int __mq_receive (struct task_struct *curr, struct pt_regs *regs)
{
    char tmp_buf[PSE51_MQ_FSTORE_LIMIT];
    caddr_t tmp_area;
    unsigned prio;
    ssize_t len;
    mqd_t q;

    if (!__xn_access_ok(curr,VERIFY_WRITE,__xn_reg_arg3(regs),sizeof(len)))
        return -EFAULT;

    __xn_copy_from_user(curr,&len,(ssize_t *)__xn_reg_arg3(regs),sizeof(len));

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

    q = (mqd_t) __xn_reg_arg1(regs);

    if (len > 0)
        {
        if (!__xn_access_ok(curr,VERIFY_WRITE,__xn_reg_arg2(regs),len))
            return -EFAULT;

        if (len <= sizeof(tmp_buf))
            tmp_area = tmp_buf;
        else
            {
            tmp_area = xnmalloc(len);

            if (!tmp_area)
                return -ENOMEM;
            }
        }
    else
        tmp_area = NULL;

    len = mq_receive(q,tmp_area,len,&prio);

    if (len == -1)
        {
        if (tmp_area && tmp_area != tmp_buf)
            xnfree(tmp_area);

        return -thread_get_errno();
        }

    __xn_copy_to_user(curr,
                      (void __user *)__xn_reg_arg3(regs),
                      &len,
                      sizeof(len));

    if (len > 0)
        __xn_copy_to_user(curr,
                          (void __user *)__xn_reg_arg2(regs),
                          tmp_area,
                          len);

    if (tmp_area && tmp_area != tmp_buf)
        xnfree(tmp_area);

    return 0;
}

int __mq_timedreceive (struct task_struct *curr, struct pt_regs *regs)
{
    struct timespec timeout, *timeoutp;
    char tmp_buf[PSE51_MQ_FSTORE_LIMIT];
    caddr_t tmp_area;
    unsigned prio;
    ssize_t len;
    mqd_t q;

    q = (mqd_t) __xn_reg_arg1(regs);

    if (!__xn_access_ok(curr,VERIFY_WRITE,__xn_reg_arg3(regs),sizeof(len)))
        return -EFAULT;

    __xn_copy_from_user(curr,&len,(ssize_t *)__xn_reg_arg3(regs),sizeof(len));

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

    if (__xn_reg_arg5(regs) &&
        !__xn_access_ok(curr,VERIFY_READ,__xn_reg_arg5(regs),sizeof(timeout)))
        return -EFAULT;

    if (len > 0)
        {
        if (!__xn_access_ok(curr,VERIFY_WRITE,__xn_reg_arg2(regs),len))
            return -EFAULT;

        if (len <= sizeof(tmp_buf))
            tmp_area = tmp_buf;
        else
            {
            tmp_area = xnmalloc(len);

            if (!tmp_area)
                return -ENOMEM;
            }
        }
    else
        tmp_area = NULL;

    if (__xn_reg_arg5(regs))
        {
        __xn_copy_from_user(curr,
                            &timeout,
                            (struct timespec __user *)__xn_reg_arg5(regs),
                            sizeof(timeout));
        timeoutp = &timeout;
        }
    else
        timeoutp = NULL;

    len = mq_timedreceive(q,tmp_area,len,&prio,timeoutp);

    if (len == -1)
        {
        if (tmp_area && tmp_area != tmp_buf)
            xnfree(tmp_area);
        
        return -thread_get_errno();
        }

    __xn_copy_to_user(curr,
                      (void __user *)__xn_reg_arg3(regs),
                      &len,
                      sizeof(len));

    if (len > 0)
        __xn_copy_to_user(curr,
                          (void __user *)__xn_reg_arg2(regs),
                          tmp_area,
                          len);

    if (tmp_area && tmp_area != tmp_buf)
        xnfree(tmp_area);

    return 0;
}

int __mq_notify (struct task_struct *curr, struct pt_regs *regs)
{
    struct sigevent sev;
    mqd_t q;

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

    q = (mqd_t) __xn_reg_arg1(regs);

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

    if (mq_notify(q, &sev))
        return -thread_get_errno();

    return 0;
}

static int __pse51_intr_handler (xnintr_t *cookie)
{
    struct pse51_interrupt *intr = PTHREAD_IDESC(cookie);

    ++intr->pending;

    if (xnsynch_nsleepers(&intr->synch_base) > 0)
        xnsynch_flush(&intr->synch_base,0);

    if (intr->mode & XN_ISR_PROPAGATE)
        return XN_ISR_PROPAGATE|(intr->mode & XN_ISR_NOENABLE);

    return XN_ISR_HANDLED|(intr->mode & XN_ISR_NOENABLE);
}

int __intr_attach (struct task_struct *curr, struct pt_regs *regs)
{
    struct pse51_interrupt *intr;
    unsigned long handle;
    int err, mode;
    unsigned irq;

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

    /* Interrupt line number. */
    irq = (unsigned)__xn_reg_arg2(regs);

    /* Interrupt control mode. */
    mode = (int)__xn_reg_arg3(regs);

    if (mode & ~(XN_ISR_NOENABLE|XN_ISR_PROPAGATE))
        return -EINVAL;

    intr = (struct pse51_interrupt *)xnmalloc(sizeof(*intr));

    if (!intr)
        return -ENOMEM;

    err = pse51_intr_attach(intr,irq,&__pse51_intr_handler,NULL);

    if (err == 0)
        {
        intr->mode = mode;
        handle = (unsigned long)intr;
        __xn_copy_to_user(curr,
                          (void __user *)__xn_reg_arg1(regs),
                          &handle,
                          sizeof(handle));
        }
    else
        xnfree(intr);

    return -err;
}

int __intr_detach (struct task_struct *curr, struct pt_regs *regs)
{
    struct pse51_interrupt *intr = (struct pse51_interrupt *)__xn_reg_arg1(regs);
    int err = pse51_intr_detach(intr);

    if (!err)
        xnfree(intr);

    return -err;
}

int __intr_wait (struct task_struct *curr, struct pt_regs *regs)
{
    struct pse51_interrupt *intr = (struct pse51_interrupt *)__xn_reg_arg1(regs);
    struct timespec ts;
    xnthread_t *thread;
    xnticks_t timeout;
    int err = 0;
    spl_t s;

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

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

        if (ts.tv_sec == 0 && ts.tv_nsec == 0)
            return -EINVAL;

        timeout = ts2ticks_ceil(&ts)+1;
        }
    else
        timeout = XN_INFINITE;

    xnlock_get_irqsave(&nklock,s);

    if (!pse51_obj_active(intr, PSE51_INTR_MAGIC, struct pse51_interrupt))
        {
        xnlock_put_irqrestore(&nklock, s);
        return -EINVAL;
        }

    if (!intr->pending)
        {
        thread = xnpod_current_thread();

        if (xnthread_base_priority(thread) != XNCORE_IRQ_PRIO)
            /* Renice the waiter above all regular threads if needed. */
            xnpod_renice_thread(thread,XNCORE_IRQ_PRIO);

        xnsynch_sleep_on(&intr->synch_base,timeout);
        
        if (xnthread_test_flags(thread,XNRMID))
            err = -EIDRM; /* Interrupt object deleted while pending. */
        else if (xnthread_test_flags(thread,XNTIMEO))
            err = -ETIMEDOUT; /* Timeout.*/
        else if (xnthread_test_flags(thread,XNBREAK))
            err = -EINTR; /* Unblocked.*/
        else
            err = intr->pending;
        }
    else
        err = intr->pending;

    intr->pending = 0;