Module: xenomai-3
Branch: master
Commit: 4f92282712b36c53ab6591011cb0448f40badfe2
URL:    
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=4f92282712b36c53ab6591011cb0448f40badfe2

Author: Philippe Gerum <r...@xenomai.org>
Date:   Thu Oct 16 14:40:56 2014 +0200

cobalt/posix/mq: prepare for 32bit syscall emulation

---

 kernel/cobalt/posix/mqueue.c |  299 ++++++++++++++++++++++++------------------
 kernel/cobalt/posix/mqueue.h |   22 ++++
 lib/cobalt/mq.c              |    6 +
 3 files changed, 196 insertions(+), 131 deletions(-)

diff --git a/kernel/cobalt/posix/mqueue.c b/kernel/cobalt/posix/mqueue.c
index b1ce1d2..f5a69f9 100644
--- a/kernel/cobalt/posix/mqueue.c
+++ b/kernel/cobalt/posix/mqueue.c
@@ -464,16 +464,22 @@ mq_tryrcv(struct cobalt_mqd *mqd, size_t len)
 
 static struct cobalt_msg *
 mq_timedsend_inner(struct cobalt_mqd *mqd,
-                  size_t len, const struct timespec *abs_timeoutp)
+                  size_t len, const void __user *u_ts,
+                  int (*fetch_timeout)(struct timespec *ts,
+                                       const void __user *u_ts))
 {
        struct cobalt_mqwait_context mwc;
        struct cobalt_msg *msg;
        struct cobalt_mq *mq;
+       struct timespec ts;
        xntmode_t tmode;
        xnticks_t to;
        spl_t s;
        int ret;
 
+       to = XN_INFINITE;
+       tmode = XN_RELATIVE;
+redo:
        xnlock_get_irqsave(&nklock, s);
        msg = mq_trysend(mqd, len);
        if (msg != ERR_PTR(-EAGAIN))
@@ -482,15 +488,17 @@ mq_timedsend_inner(struct cobalt_mqd *mqd,
        if (mqd->flags & O_NONBLOCK)
                goto out;
 
-       to = XN_INFINITE;
-       tmode = XN_RELATIVE;
-       if (abs_timeoutp) {
-               if ((unsigned long)abs_timeoutp->tv_nsec >= ONE_BILLION) {
-                       msg = ERR_PTR(-EINVAL);
-                       goto out;
-               }
-               to = ts2ns(abs_timeoutp) + 1;
+       if (fetch_timeout) {
+               xnlock_put_irqrestore(&nklock, s);
+               ret = fetch_timeout(&ts, u_ts);
+               if (ret)
+                       return ERR_PTR(ret);
+               if ((unsigned long)ts.tv_nsec >= ONE_BILLION)
+                       return ERR_PTR(-EINVAL);
+               to = ts2ns(&ts) + 1;
                tmode = XN_REALTIME;
+               fetch_timeout = NULL;
+               goto redo;
        }
 
        mq = mqd->mq;
@@ -584,16 +592,23 @@ mq_finish_send(struct cobalt_mqd *mqd, struct cobalt_msg 
*msg)
 
 static struct cobalt_msg *
 mq_timedrcv_inner(struct cobalt_mqd *mqd,
-                 size_t len, const struct timespec *abs_timeoutp)
+                 size_t len,
+                 const void __user *u_ts,
+                 int (*fetch_timeout)(struct timespec *ts,
+                                      const void __user *u_ts))
 {
        struct cobalt_mqwait_context mwc;
        struct cobalt_msg *msg;
        struct cobalt_mq *mq;
+       struct timespec ts;
        xntmode_t tmode;
        xnticks_t to;
        spl_t s;
        int ret;
 
+       to = XN_INFINITE;
+       tmode = XN_RELATIVE;
+redo:
        xnlock_get_irqsave(&nklock, s);
        msg = mq_tryrcv(mqd, len);
        if (msg != ERR_PTR(-EAGAIN))
@@ -602,15 +617,17 @@ mq_timedrcv_inner(struct cobalt_mqd *mqd,
        if (mqd->flags & O_NONBLOCK)
                goto out;
 
-       to = XN_INFINITE;
-       tmode = XN_RELATIVE;
-       if (abs_timeoutp) {
-               if (abs_timeoutp->tv_nsec >= ONE_BILLION) {
-                       msg = ERR_PTR(-EINVAL);
-                       goto out;
-               }
-               to = ts2ns(abs_timeoutp) + 1;
+       if (fetch_timeout) {
+               xnlock_put_irqrestore(&nklock, s);
+               ret = fetch_timeout(&ts, u_ts);
+               if (ret)
+                       return ERR_PTR(ret);
+               if (ts.tv_nsec >= ONE_BILLION)
+                       return ERR_PTR(-EINVAL);
+               to = ts2ns(&ts) + 1;
                tmode = XN_REALTIME;
+               fetch_timeout = NULL;
+               goto redo;
        }
 
        mq = mqd->mq;
@@ -752,39 +769,37 @@ static inline void cobalt_mqd_put(struct cobalt_mqd *mqd)
        rtdm_fd_put(&mqd->fd);
 }
 
-COBALT_SYSCALL(mq_notify, primary,
-              int, (mqd_t fd, const struct sigevent *__user evp))
+int __cobalt_mq_notify(mqd_t fd, const struct sigevent *evp)
 {
        struct cobalt_mqd *mqd;
-       struct sigevent sev;
-       int err;
+       int ret;
 
        mqd = cobalt_mqd_get(fd);
-       if (IS_ERR(mqd)) {
-               err = PTR_ERR(mqd);
-               goto out;
-       }
-
-       if (evp && __xn_safe_copy_from_user(&sev, evp, sizeof(sev))) {
-               err = -EFAULT;
-               goto out;
+       if (IS_ERR(mqd))
+               ret = PTR_ERR(mqd);
+       else {
+               trace_cobalt_mq_notify(fd, evp);
+               ret = mq_notify(mqd, fd, evp);
+               cobalt_mqd_put(mqd);
        }
 
-       trace_cobalt_mq_notify(fd, evp ? &sev : NULL);
+       return ret;
+}
 
-       err = mq_notify(mqd, fd, evp ? &sev : NULL);
+COBALT_SYSCALL(mq_notify, primary,
+              int, (mqd_t fd, const struct sigevent *__user evp))
+{
+       struct sigevent sev;
 
-  out:
-       cobalt_mqd_put(mqd);
+       if (evp && __xn_safe_copy_from_user(&sev, evp, sizeof(sev)))
+               return -EFAULT;
 
-       return err;
+       return __cobalt_mq_notify(fd, evp ? &sev : NULL);
 }
 
-COBALT_SYSCALL(mq_open, lostage,
-              int, (const char __user *u_name, int oflags,
-                    mode_t mode, struct mq_attr __user *u_attr))
+int __cobalt_mq_open(const char __user *u_name, int oflags,
+                    mode_t mode, struct mq_attr *attr)
 {
-       struct mq_attr locattr, *attr;
        char name[COBALT_MAXNAME];
        unsigned int len;
        mqd_t uqd;
@@ -800,14 +815,6 @@ COBALT_SYSCALL(mq_open, lostage,
        if (len == 0)
                return -EINVAL;
 
-       if ((oflags & O_CREAT) && u_attr) {
-               if (__xn_safe_copy_from_user(&locattr, u_attr, sizeof(locattr)))
-                       return -EFAULT;
-
-               attr = &locattr;
-       } else
-               attr = NULL;
-
        trace_cobalt_mq_open(name, oflags, mode);
 
        uqd = __rtdm_anon_getfd("[cobalt-mq]", oflags);
@@ -823,6 +830,21 @@ COBALT_SYSCALL(mq_open, lostage,
        return uqd;
 }
 
+COBALT_SYSCALL(mq_open, lostage,
+              int, (const char __user *u_name, int oflags,
+                    mode_t mode, struct mq_attr __user *u_attr))
+{
+       struct mq_attr _attr, *attr = &_attr;
+
+       if ((oflags & O_CREAT) && u_attr) {
+               if (__xn_safe_copy_from_user(&_attr, u_attr, sizeof(_attr)))
+                       return -EFAULT;
+       } else
+               attr = NULL;
+
+       return __cobalt_mq_open(u_name, oflags, mode, attr);
+}
+
 COBALT_SYSCALL(mq_close, lostage, int, (mqd_t uqd))
 {
        trace_cobalt_mq_close(uqd);
@@ -847,171 +869,177 @@ COBALT_SYSCALL(mq_unlink, lostage,
        return mq_unlink(name);
 }
 
-COBALT_SYSCALL(mq_getattr, current,
-              int, (mqd_t uqd, struct mq_attr __user *u_attr))
+int __cobalt_mq_getattr(mqd_t uqd, struct mq_attr *attr)
 {
        struct cobalt_mqd *mqd;
-       struct mq_attr attr;
-       int err;
+       int ret;
 
        mqd = cobalt_mqd_get(uqd);
        if (IS_ERR(mqd))
                return PTR_ERR(mqd);
 
-       err = mq_getattr(mqd, &attr);
-
+       ret = mq_getattr(mqd, attr);
        cobalt_mqd_put(mqd);
-       if (err)
-               return err;
+       if (ret)
+               return ret;
+
+       trace_cobalt_mq_getattr(uqd, attr);
+
+       return 0;
+}
 
-       trace_cobalt_mq_getattr(uqd, &attr);
+COBALT_SYSCALL(mq_getattr, current,
+              int, (mqd_t uqd, struct mq_attr __user *u_attr))
+{
+       struct mq_attr attr;
+       int ret;
+
+       ret = __cobalt_mq_getattr(uqd, &attr);
+       if (ret)
+               return ret;
 
        return __xn_safe_copy_to_user(u_attr, &attr, sizeof(attr));
 }
 
-COBALT_SYSCALL(mq_setattr, current,
-              int, (mqd_t uqd, const struct mq_attr __user *u_attr,
-                    struct mq_attr __user *u_oattr))
+int __cobalt_mq_setattr(mqd_t uqd, const struct mq_attr *attr,
+                       struct mq_attr *oattr)
 {
-       struct mq_attr attr, oattr;
        struct cobalt_mqd *mqd;
-       int err;
+       int ret;
 
        mqd = cobalt_mqd_get(uqd);
        if (IS_ERR(mqd))
                return PTR_ERR(mqd);
 
-       if (__xn_safe_copy_from_user(&attr, u_attr, sizeof(attr))) {
-               err = -EFAULT;
-               goto out;
-       }
+       trace_cobalt_mq_setattr(uqd, attr);
+       ret = mq_setattr(mqd, attr, oattr);
+       cobalt_mqd_put(mqd);
 
-       trace_cobalt_mq_setattr(uqd, &attr);
+       return ret;
+}
 
-       err = mq_setattr(mqd, &attr, &oattr);
-  out:
-       cobalt_mqd_put(mqd);
-       if (err)
-               return err;
+COBALT_SYSCALL(mq_setattr, current,
+              int, (mqd_t uqd, const struct mq_attr __user *u_attr,
+                    struct mq_attr __user *u_oattr))
+{
+       struct mq_attr attr, oattr;
+       int ret;
 
-       if (u_oattr)
-               return __xn_safe_copy_to_user(u_oattr, &oattr, sizeof(oattr));
+       if (__xn_safe_copy_from_user(&attr, u_attr, sizeof(attr)))
+               return -EFAULT;
 
-       return 0;
+       ret = __cobalt_mq_setattr(uqd, &attr, &oattr);
+       if (ret)
+               return ret;
+
+       if (u_oattr == NULL)
+               return 0;
+
+       return __xn_safe_copy_to_user(u_oattr, &oattr, sizeof(oattr));
 }
 
-COBALT_SYSCALL(mq_timedsend, primary,
-              int, (mqd_t uqd, const void __user *u_buf, size_t len,
-                    unsigned int prio, const struct timespec __user *u_ts))
+static inline int mq_fetch_timeout(struct timespec *ts,
+                                  const void __user *u_ts)
+{
+       return u_ts == NULL ? -EFAULT :
+               __xn_safe_copy_from_user(ts, u_ts, sizeof(*ts));
+}
+
+int __cobalt_mq_timedsend(mqd_t uqd, const void __user *u_buf, size_t len,
+                         unsigned int prio, const void __user *u_ts,
+                         int (*fetch_timeout)(struct timespec *ts,
+                                              const void __user *u_ts))
 {
-       struct timespec timeout, *timeoutp;
        struct cobalt_msg *msg;
        struct cobalt_mqd *mqd;
-       int err;
+       int ret;
 
        mqd = cobalt_mqd_get(uqd);
        if (IS_ERR(mqd))
                return PTR_ERR(mqd);
 
        if (prio >= COBALT_MSGPRIOMAX) {
-               err = -EINVAL;
+               ret = -EINVAL;
                goto out;
        }
 
        if (len > 0 && !access_rok(u_buf, len)) {
-               err = -EFAULT;
+               ret = -EFAULT;
                goto out;
        }
 
-       if (u_ts) {
-               if (__xn_safe_copy_from_user(&timeout, u_ts, sizeof(timeout))) {
-                       err = -EFAULT;
-                       goto out;
-               }
-               timeoutp = &timeout;
-               trace_cobalt_mq_timedsend(uqd, u_buf, len, prio, &timeout);
-       } else {
-               timeoutp = NULL;
-               trace_cobalt_mq_send(uqd, u_buf, len, prio);
-       }
-
-       msg = mq_timedsend_inner(mqd, len, timeoutp);
+       trace_cobalt_mq_send(uqd, u_buf, len, prio);
+       msg = mq_timedsend_inner(mqd, len, u_ts, fetch_timeout);
        if (IS_ERR(msg)) {
-               err = PTR_ERR(msg);
+               ret = PTR_ERR(msg);
                goto out;
        }
 
-       if(__xn_copy_from_user(msg->data, u_buf, len)) {
+       ret = __xn_safe_copy_from_user(msg->data, u_buf, len);
+       if (ret) {
                mq_finish_rcv(mqd, msg);
-               err = -EFAULT;
                goto out;
        }
        msg->len = len;
        msg->prio = prio;
-
-       err = mq_finish_send(mqd, msg);
-  out:
+       ret = mq_finish_send(mqd, msg);
+out:
        cobalt_mqd_put(mqd);
 
-       return err;
+       return ret;
 }
 
-COBALT_SYSCALL(mq_timedreceive, primary,
-              int, (mqd_t uqd, void __user *u_buf,
-                    ssize_t __user *u_len,
-                    unsigned int __user *u_prio,
-                    const struct timespec __user *u_ts))
+COBALT_SYSCALL(mq_timedsend, primary,
+              int, (mqd_t uqd, const void __user *u_buf, size_t len,
+                    unsigned int prio, const struct timespec __user *u_ts))
+{
+       return __cobalt_mq_timedsend(uqd, u_buf, len, prio,
+                                    u_ts, u_ts ? mq_fetch_timeout : NULL);
+}
+
+int __cobalt_mq_timedreceive(mqd_t uqd, void __user *u_buf,
+                            ssize_t __user *u_len,
+                            unsigned int __user *u_prio,
+                            const void __user *u_ts,
+                            int (*fetch_timeout)(struct timespec *ts,
+                                                 const void __user *u_ts))
 {
-       struct timespec timeout, *timeoutp;
        struct cobalt_mqd *mqd;
        struct cobalt_msg *msg;
        unsigned int prio;
        ssize_t len;
-       int err;
+       int ret;
 
        mqd = cobalt_mqd_get(uqd);
        if (IS_ERR(mqd))
                return PTR_ERR(mqd);
 
        if (__xn_get_user(len, u_len)) {
-               err = -EFAULT;
+               ret = -EFAULT;
                goto fail;
        }
 
        if (len > 0 && !access_wok(u_buf, len)) {
-               err = -EFAULT;
+               ret = -EFAULT;
                goto fail;
        }
 
-       if (u_ts) {
-               if (__xn_safe_copy_from_user(&timeout, u_ts, sizeof(timeout))) {
-                       err = -EFAULT;
-                       goto fail;
-               }
-
-               timeoutp = &timeout;
-               trace_cobalt_mq_timedreceive(uqd, u_buf, len, &timeout);
-       } else {
-               timeoutp = NULL;
-               trace_cobalt_mq_receive(uqd, u_buf, len);
-       }
-
-       msg = mq_timedrcv_inner(mqd, len, timeoutp);
+       msg = mq_timedrcv_inner(mqd, len, u_ts, fetch_timeout);
        if (IS_ERR(msg)) {
-               err = PTR_ERR(msg);
+               ret = PTR_ERR(msg);
                goto fail;
        }
 
-       if (__xn_copy_to_user(u_buf, msg->data, msg->len)) {
+       ret = __xn_safe_copy_to_user(u_buf, msg->data, msg->len);
+       if (ret) {
                mq_finish_rcv(mqd, msg);
-               err = -EFAULT;
                goto fail;
        }
 
        len = msg->len;
        prio = msg->prio;
-       err = mq_finish_rcv(mqd, msg);
-       if (err)
+       ret = mq_finish_rcv(mqd, msg);
+       if (ret)
                goto fail;
 
        cobalt_mqd_put(mqd);
@@ -1023,11 +1051,20 @@ COBALT_SYSCALL(mq_timedreceive, primary,
                return -EFAULT;
 
        return 0;
-
 fail:
        cobalt_mqd_put(mqd);
 
-       return err;
+       return ret;
+}
+
+COBALT_SYSCALL(mq_timedreceive, primary,
+              int, (mqd_t uqd, void __user *u_buf,
+                    ssize_t __user *u_len,
+                    unsigned int __user *u_prio,
+                    const struct timespec __user *u_ts))
+{
+       return __cobalt_mq_timedreceive(uqd, u_buf, u_len, u_prio,
+                                       u_ts, u_ts ? mq_fetch_timeout : NULL);
 }
 
 int cobalt_mq_pkg_init(void)
diff --git a/kernel/cobalt/posix/mqueue.h b/kernel/cobalt/posix/mqueue.h
index 9b48333..8bdccd4 100644
--- a/kernel/cobalt/posix/mqueue.h
+++ b/kernel/cobalt/posix/mqueue.h
@@ -30,6 +30,28 @@ struct mq_attr {
        long mq_curmsgs;
 };
 
+int __cobalt_mq_open(const char __user *u_name, int oflags,
+                    mode_t mode, struct mq_attr *attr);
+
+int __cobalt_mq_getattr(mqd_t uqd, struct mq_attr *attr);
+
+int __cobalt_mq_setattr(mqd_t uqd, const struct mq_attr *attr,
+                       struct mq_attr *oattr);
+
+int __cobalt_mq_timedsend(mqd_t uqd, const void __user *u_buf, size_t len,
+                         unsigned int prio, const void __user *u_ts,
+                         int (*fetch_timeout)(struct timespec *ts,
+                                              const void __user *u_ts));
+
+int __cobalt_mq_timedreceive(mqd_t uqd, void __user *u_buf,
+                            ssize_t __user *u_len,
+                            unsigned int __user *u_prio,
+                            const void __user *u_ts,
+                            int (*fetch_timeout)(struct timespec *ts,
+                                                 const void __user *u_ts));
+
+int __cobalt_mq_notify(mqd_t fd, const struct sigevent *evp);
+
 COBALT_SYSCALL_DECL(mq_open,
                    int, (const char __user *u_name, int oflags,
                          mode_t mode, struct mq_attr __user *u_attr));
diff --git a/lib/cobalt/mq.c b/lib/cobalt/mq.c
index 87b1b73..49e9db3 100644
--- a/lib/cobalt/mq.c
+++ b/lib/cobalt/mq.c
@@ -364,6 +364,9 @@ COBALT_IMPL(int, mq_timedsend, (mqd_t q,
 {
        int err, oldtype;
 
+       if (timeout == NULL)
+               return -EFAULT;
+
        pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
 
        err = XENOMAI_SYSCALL5(sc_cobalt_mq_timedsend,
@@ -483,6 +486,9 @@ COBALT_IMPL(ssize_t, mq_timedreceive, (mqd_t q,
        ssize_t rlen = (ssize_t) len;
        int err, oldtype;
 
+       if (timeout == NULL)
+               return -EFAULT;
+
        pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
 
        err = XENOMAI_SYSCALL5(sc_cobalt_mq_timedreceive,


_______________________________________________
Xenomai-git mailing list
Xenomai-git@xenomai.org
http://www.xenomai.org/mailman/listinfo/xenomai-git

Reply via email to