Module: xenomai-3
Branch: next
Commit: ba9af05589506e2499c7aea8bd875784e472b56a
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=ba9af05589506e2499c7aea8bd875784e472b56a
Author: Philippe Gerum <r...@xenomai.org>
Date: Thu Jan 25 16:06:29 2018 +0100
cobalt/rtdm: add sendmmsg(), recvmmsg() syscalls
---
include/cobalt/kernel/compat.h | 6 ++
include/cobalt/kernel/rtdm/fd.h | 11 +++
include/cobalt/uapi/syscall.h | 2 +
kernel/cobalt/posix/compat.c | 24 ++++++
kernel/cobalt/posix/io.c | 43 ++++++++++
kernel/cobalt/posix/io.h | 8 ++
kernel/cobalt/posix/syscall32.c | 44 ++++++++++
kernel/cobalt/posix/syscall32.h | 9 ++
kernel/cobalt/rtdm/Makefile | 2 +-
kernel/cobalt/rtdm/fd.c | 172 +++++++++++++++++++++++++++++++++++++
kernel/cobalt/trace/cobalt-rtdm.h | 28 ++++++
11 files changed, 348 insertions(+), 1 deletion(-)
diff --git a/include/cobalt/kernel/compat.h b/include/cobalt/kernel/compat.h
index 43378d9..bb6c754 100644
--- a/include/cobalt/kernel/compat.h
+++ b/include/cobalt/kernel/compat.h
@@ -139,9 +139,15 @@ int sys32_put_siginfo(void __user *u_si, const struct
siginfo *si,
int sys32_get_msghdr(struct user_msghdr *msg,
const struct compat_msghdr __user *u_cmsg);
+int sys32_get_mmsghdr(struct mmsghdr *mmsg,
+ const struct compat_mmsghdr __user *u_cmmsg);
+
int sys32_put_msghdr(struct compat_msghdr __user *u_cmsg,
const struct user_msghdr *msg);
+int sys32_put_mmsghdr(struct compat_mmsghdr __user *u_cmmsg,
+ const struct mmsghdr *mmsg);
+
int sys32_get_iovec(struct iovec *iov,
const struct compat_iovec __user *ciov,
int ciovlen);
diff --git a/include/cobalt/kernel/rtdm/fd.h b/include/cobalt/kernel/rtdm/fd.h
index e504c0c..58d3c4a 100644
--- a/include/cobalt/kernel/rtdm/fd.h
+++ b/include/cobalt/kernel/rtdm/fd.h
@@ -367,9 +367,20 @@ int rtdm_fd_close(int ufd, unsigned int magic);
ssize_t rtdm_fd_recvmsg(int ufd, struct user_msghdr *msg, int flags);
+int __rtdm_fd_recvmmsg(int ufd, void __user *u_msgvec, unsigned int vlen,
+ unsigned int flags, void __user *u_timeout,
+ int (*get_mmsg)(struct mmsghdr *mmsg, void __user
*u_mmsg),
+ int (*put_mmsg)(void __user **u_mmsg_p, const struct
mmsghdr *mmsg),
+ int (*get_timespec)(struct timespec *ts, const void
__user *u_ts));
+
ssize_t rtdm_fd_sendmsg(int ufd, const struct user_msghdr *msg,
int flags);
+int __rtdm_fd_sendmmsg(int ufd, void __user *u_msgvec, unsigned int vlen,
+ unsigned int flags,
+ int (*get_mmsg)(struct mmsghdr *mmsg, void __user
*u_mmsg),
+ int (*put_mmsg)(void __user **u_mmsg_p, const struct
mmsghdr *mmsg));
+
int rtdm_fd_mmap(int ufd, struct _rtdm_mmap_request *rma,
void **u_addrp);
diff --git a/include/cobalt/uapi/syscall.h b/include/cobalt/uapi/syscall.h
index cd7e820..d246f0b 100644
--- a/include/cobalt/uapi/syscall.h
+++ b/include/cobalt/uapi/syscall.h
@@ -118,6 +118,8 @@
#define sc_cobalt_serialdbg 95
#define sc_cobalt_extend 96
#define sc_cobalt_ftrace_puts 97
+#define sc_cobalt_recvmmsg 98
+#define sc_cobalt_sendmmsg 99
#define __NR_COBALT_SYSCALLS 128 /* Power of 2 */
diff --git a/kernel/cobalt/posix/compat.c b/kernel/cobalt/posix/compat.c
index 962465a..3921510 100644
--- a/kernel/cobalt/posix/compat.c
+++ b/kernel/cobalt/posix/compat.c
@@ -359,6 +359,18 @@ int sys32_get_msghdr(struct user_msghdr *msg,
}
EXPORT_SYMBOL_GPL(sys32_get_msghdr);
+int sys32_get_mmsghdr(struct mmsghdr *mmsg,
+ const struct compat_mmsghdr __user *u_cmmsg)
+{
+ if (u_cmmsg == NULL ||
+ !access_rok(u_cmmsg, sizeof(*u_cmmsg)) ||
+ __xn_get_user(mmsg->msg_len, &u_cmmsg->msg_len))
+ return -EFAULT;
+
+ return sys32_get_msghdr(&mmsg->msg_hdr, &u_cmmsg->msg_hdr);
+}
+EXPORT_SYMBOL_GPL(sys32_get_mmsghdr);
+
int sys32_put_msghdr(struct compat_msghdr __user *u_cmsg,
const struct user_msghdr *msg)
{
@@ -377,6 +389,18 @@ int sys32_put_msghdr(struct compat_msghdr __user *u_cmsg,
}
EXPORT_SYMBOL_GPL(sys32_put_msghdr);
+int sys32_put_mmsghdr(struct compat_mmsghdr __user *u_cmmsg,
+ const struct mmsghdr *mmsg)
+{
+ if (u_cmmsg == NULL ||
+ !access_wok(u_cmmsg, sizeof(*u_cmmsg)) ||
+ __xn_put_user(mmsg->msg_len, &u_cmmsg->msg_len))
+ return -EFAULT;
+
+ return sys32_put_msghdr(&u_cmmsg->msg_hdr, &mmsg->msg_hdr);
+}
+EXPORT_SYMBOL_GPL(sys32_put_mmsghdr);
+
int sys32_get_iovec(struct iovec *iov,
const struct compat_iovec __user *u_ciov,
int ciovlen)
diff --git a/kernel/cobalt/posix/io.c b/kernel/cobalt/posix/io.c
index ece7877..ef4a0fe 100644
--- a/kernel/cobalt/posix/io.c
+++ b/kernel/cobalt/posix/io.c
@@ -93,6 +93,33 @@ COBALT_SYSCALL(recvmsg, handover,
return cobalt_copy_to_user(umsg, &m, sizeof(*umsg)) ?: ret;
}
+static int get_timespec(struct timespec *ts,
+ const void __user *u_ts)
+{
+ return cobalt_copy_from_user(ts, u_ts, sizeof(*ts));
+}
+
+static int get_mmsg(struct mmsghdr *mmsg, void __user *u_mmsg)
+{
+ return cobalt_copy_from_user(mmsg, u_mmsg, sizeof(*mmsg));
+}
+
+static int put_mmsg(void __user **u_mmsg_p, const struct mmsghdr *mmsg)
+{
+ struct mmsghdr __user **p = (struct mmsghdr **)u_mmsg_p,
+ *q __user = (*p)++;
+
+ return cobalt_copy_to_user(q, mmsg, sizeof(*q));
+}
+
+COBALT_SYSCALL(recvmmsg, primary,
+ (int fd, struct mmsghdr __user *u_msgvec, unsigned int vlen,
+ unsigned int flags, struct timespec *u_timeout))
+{
+ return __rtdm_fd_recvmmsg(fd, u_msgvec, vlen, flags, u_timeout,
+ get_mmsg, put_mmsg, get_timespec);
+}
+
COBALT_SYSCALL(sendmsg, handover,
(int fd, struct user_msghdr __user *umsg, int flags))
{
@@ -104,6 +131,22 @@ COBALT_SYSCALL(sendmsg, handover,
return ret ?: rtdm_fd_sendmsg(fd, &m, flags);
}
+static int put_mmsglen(void __user **u_mmsg_p, const struct mmsghdr *mmsg)
+{
+ struct mmsghdr __user **p = (struct mmsghdr **)u_mmsg_p,
+ *q __user = (*p)++;
+
+ return __xn_put_user(mmsg->msg_len, &q->msg_len);
+}
+
+COBALT_SYSCALL(sendmmsg, primary,
+ (int fd, struct mmsghdr __user *u_msgvec,
+ unsigned int vlen, unsigned int flags))
+{
+ return __rtdm_fd_sendmmsg(fd, u_msgvec, vlen, flags,
+ get_mmsg, put_mmsglen);
+}
+
COBALT_SYSCALL(mmap, lostage,
(int fd, struct _rtdm_mmap_request __user *u_rma,
void __user **u_addrp))
diff --git a/kernel/cobalt/posix/io.h b/kernel/cobalt/posix/io.h
index b2db702..647f7f3 100644
--- a/kernel/cobalt/posix/io.h
+++ b/kernel/cobalt/posix/io.h
@@ -51,9 +51,17 @@ COBALT_SYSCALL_DECL(write,
COBALT_SYSCALL_DECL(recvmsg,
(int fd, struct user_msghdr __user *umsg, int flags));
+COBALT_SYSCALL_DECL(recvmmsg,
+ (int fd, struct mmsghdr __user *u_msgvec, unsigned int vlen,
+ unsigned int flags, struct timespec *u_timeout));
+
COBALT_SYSCALL_DECL(sendmsg,
(int fd, struct user_msghdr __user *umsg, int flags));
+COBALT_SYSCALL_DECL(sendmmsg,
+ (int fd, struct mmsghdr __user *u_msgvec,
+ unsigned int vlen, unsigned int flags));
+
COBALT_SYSCALL_DECL(mmap,
(int fd, struct _rtdm_mmap_request __user *u_rma,
void __user * __user *u_addrp));
diff --git a/kernel/cobalt/posix/syscall32.c b/kernel/cobalt/posix/syscall32.c
index e79173e..93e5077 100644
--- a/kernel/cobalt/posix/syscall32.c
+++ b/kernel/cobalt/posix/syscall32.c
@@ -784,6 +784,34 @@ COBALT_SYSCALL32emu(recvmsg, handover,
return sys32_put_msghdr(umsg, &m) ?: ret;
}
+static int get_timespec32(struct timespec *ts,
+ const void __user *u_ts)
+{
+ return sys32_get_timespec(ts, u_ts);
+}
+
+static int get_mmsg32(struct mmsghdr *mmsg, void __user *u_mmsg)
+{
+ return sys32_get_mmsghdr(mmsg, u_mmsg);
+}
+
+static int put_mmsg32(void __user **u_mmsg_p, const struct mmsghdr *mmsg)
+{
+ struct compat_mmsghdr __user **p = (struct compat_mmsghdr **)u_mmsg_p,
+ *q __user = (*p)++;
+
+ return sys32_put_mmsghdr(q, mmsg);
+}
+
+COBALT_SYSCALL32emu(recvmmsg, primary,
+ (int ufd, struct compat_mmsghdr __user *u_msgvec, unsigned int
vlen,
+ unsigned int flags, struct compat_timespec *u_timeout))
+{
+ return __rtdm_fd_recvmmsg(ufd, u_msgvec, vlen, flags, u_timeout,
+ get_mmsg32, put_mmsg32,
+ get_timespec32);
+}
+
COBALT_SYSCALL32emu(sendmsg, handover,
(int fd, struct compat_msghdr __user *umsg, int flags))
{
@@ -795,6 +823,22 @@ COBALT_SYSCALL32emu(sendmsg, handover,
return ret ?: rtdm_fd_sendmsg(fd, &m, flags);
}
+static int put_mmsglen32(void __user **u_mmsg_p, const struct mmsghdr *mmsg)
+{
+ struct compat_mmsghdr __user **p = (struct compat_mmsghdr **)u_mmsg_p,
+ *q __user = (*p)++;
+
+ return __xn_put_user(mmsg->msg_len, &q->msg_len);
+}
+
+COBALT_SYSCALL32emu(sendmmsg, primary,
+ (int fd, struct compat_mmsghdr __user *u_msgvec, unsigned
int vlen,
+ unsigned int flags))
+{
+ return __rtdm_fd_sendmmsg(fd, u_msgvec, vlen, flags,
+ get_mmsg32, put_mmsglen32);
+}
+
COBALT_SYSCALL32emu(mmap, lostage,
(int fd, struct compat_rtdm_mmap_request __user *u_crma,
compat_uptr_t __user *u_caddrp))
diff --git a/kernel/cobalt/posix/syscall32.h b/kernel/cobalt/posix/syscall32.h
index 64f0958..730568c 100644
--- a/kernel/cobalt/posix/syscall32.h
+++ b/kernel/cobalt/posix/syscall32.h
@@ -190,10 +190,19 @@ COBALT_SYSCALL32emu_DECL(recvmsg,
(int fd, struct compat_msghdr __user *umsg,
int flags));
+COBALT_SYSCALL32emu_DECL(recvmmsg,
+ (int fd, struct compat_mmsghdr __user *u_msgvec,
+ unsigned int vlen,
+ unsigned int flags, struct compat_timespec
*u_timeout));
+
COBALT_SYSCALL32emu_DECL(sendmsg,
(int fd, struct compat_msghdr __user *umsg,
int flags));
+COBALT_SYSCALL32emu_DECL(sendmmsg,
+ (int fd, struct compat_mmsghdr __user *u_msgvec,
unsigned int vlen,
+ unsigned int flags));
+
COBALT_SYSCALL32emu_DECL(mmap,
(int fd,
struct compat_rtdm_mmap_request __user *u_rma,
diff --git a/kernel/cobalt/rtdm/Makefile b/kernel/cobalt/rtdm/Makefile
index ce215fe..e10568c 100644
--- a/kernel/cobalt/rtdm/Makefile
+++ b/kernel/cobalt/rtdm/Makefile
@@ -7,4 +7,4 @@ xenomai-y := core.o \
fd.o \
wrappers.o
-ccflags-y += -I$(src)/..
+ccflags-y += -I$(src)/.. -Ikernel
diff --git a/kernel/cobalt/rtdm/fd.c b/kernel/cobalt/rtdm/fd.c
index b97e917..459b22b 100644
--- a/kernel/cobalt/rtdm/fd.c
+++ b/kernel/cobalt/rtdm/fd.c
@@ -33,6 +33,7 @@
#include "internal.h"
#include "posix/process.h"
#include "posix/syscall.h"
+#include "posix/clock.h"
#define RTDM_SETFL_MASK (O_NONBLOCK)
@@ -583,6 +584,121 @@ out:
}
EXPORT_SYMBOL_GPL(rtdm_fd_recvmsg);
+struct cobalt_recvmmsg_timer {
+ struct xntimer timer;
+ struct xnthread *waiter;
+};
+
+static void recvmmsg_timeout_handler(struct xntimer *timer)
+{
+ struct cobalt_recvmmsg_timer *rq;
+
+ rq = container_of(timer, struct cobalt_recvmmsg_timer, timer);
+ xnthread_set_info(rq->waiter, XNTIMEO);
+ xnthread_resume(rq->waiter, XNDELAY);
+}
+
+int __rtdm_fd_recvmmsg(int ufd, void __user *u_msgvec, unsigned int vlen,
+ unsigned int flags, void __user *u_timeout,
+ int (*get_mmsg)(struct mmsghdr *mmsg, void __user
*u_mmsg),
+ int (*put_mmsg)(void __user **u_mmsg_p, const struct
mmsghdr *mmsg),
+ int (*get_timespec)(struct timespec *ts, const void
__user *u_ts))
+{
+ struct cobalt_recvmmsg_timer rq;
+ xntmode_t tmode = XN_RELATIVE;
+ struct timespec ts = { 0 };
+ int ret, datagrams = 0;
+ xnticks_t timeout = 0;
+ struct mmsghdr mmsg;
+ struct rtdm_fd *fd;
+ void __user *u_p;
+ ssize_t len;
+ spl_t s;
+
+ if (vlen == 0)
+ return 0;
+
+ fd = rtdm_fd_get(ufd, 0);
+ if (IS_ERR(fd)) {
+ ret = PTR_ERR(fd);
+ goto out;
+ }
+
+ set_compat_bit(fd);
+
+ trace_cobalt_fd_recvmmsg(current, fd, ufd, flags);
+
+ if (u_timeout) {
+ ret = get_timespec(&ts, u_timeout);
+ if (ret)
+ goto fail;
+
+ if ((unsigned long)ts.tv_nsec >= ONE_BILLION) {
+ ret = -EINVAL;
+ goto fail;
+ }
+
+ tmode = XN_ABSOLUTE;
+ timeout = ts2ns(&ts);
+ if (timeout == 0)
+ flags |= MSG_DONTWAIT;
+ else {
+ timeout += xnclock_read_monotonic(&nkclock);
+ rq.waiter = xnthread_current();
+ xntimer_init(&rq.timer, &nkclock,
+ recvmmsg_timeout_handler,
+ NULL, XNTIMER_KGRAVITY);
+ xnlock_get_irqsave(&nklock, s);
+ ret = xntimer_start(&rq.timer, timeout,
+ XN_INFINITE, tmode);
+ xnlock_put_irqrestore(&nklock, s);
+ }
+ }
+
+ if (fd->oflags & O_NONBLOCK)
+ flags |= MSG_DONTWAIT;
+
+ for (u_p = u_msgvec; vlen > 0; vlen--) {
+ ret = get_mmsg(&mmsg, u_p);
+ if (ret)
+ break;
+ len = fd->ops->recvmsg_rt(fd, &mmsg.msg_hdr, flags);
+ if (len < 0) {
+ ret = len;
+ break;
+ }
+ mmsg.msg_len = (unsigned int)len;
+ ret = put_mmsg(&u_p, &mmsg);
+ if (ret)
+ break;
+ datagrams++;
+ /* OOB data requires immediate handling. */
+ if (mmsg.msg_hdr.msg_flags & MSG_OOB)
+ break;
+ if (flags & MSG_WAITFORONE)
+ flags |= MSG_DONTWAIT;
+ }
+
+ if (timeout) {
+ xnlock_get_irqsave(&nklock, s);
+ xntimer_destroy(&rq.timer);
+ xnlock_put_irqrestore(&nklock, s);
+ }
+
+ if (datagrams > 0 &&
+ (ret == 0 || ret == -ETIMEDOUT || ret == -EWOULDBLOCK)) {
+ /* NOTE: SO_ERROR should be honored for other errors. */
+ rtdm_fd_put(fd);
+ return datagrams;
+ }
+fail:
+ rtdm_fd_put(fd);
+out:
+ trace_cobalt_fd_recvmmsg_status(current, fd, ufd, ret);
+
+ return ret;
+}
+
ssize_t rtdm_fd_sendmsg(int ufd, const struct user_msghdr *msg, int flags)
{
struct rtdm_fd *fd;
@@ -618,6 +734,62 @@ out:
}
EXPORT_SYMBOL_GPL(rtdm_fd_sendmsg);
+int __rtdm_fd_sendmmsg(int ufd, void __user *u_msgvec, unsigned int vlen,
+ unsigned int flags,
+ int (*get_mmsg)(struct mmsghdr *mmsg, void __user
*u_mmsg),
+ int (*put_mmsg)(void __user **u_mmsg_p, const struct
mmsghdr *mmsg))
+{
+ int ret, datagrams = 0;
+ struct mmsghdr mmsg;
+ struct rtdm_fd *fd;
+ void __user *u_p;
+ ssize_t len;
+
+ if (vlen == 0)
+ return 0;
+
+ fd = rtdm_fd_get(ufd, 0);
+ if (IS_ERR(fd)) {
+ ret = PTR_ERR(fd);
+ goto out;
+ }
+
+ set_compat_bit(fd);
+
+ trace_cobalt_fd_sendmmsg(current, fd, ufd, flags);
+
+ if (fd->oflags & O_NONBLOCK)
+ flags |= MSG_DONTWAIT;
+
+ for (u_p = u_msgvec; vlen > 0; vlen--) {
+ ret = get_mmsg(&mmsg, u_p);
+ if (ret)
+ break;
+ len = fd->ops->sendmsg_rt(fd, &mmsg.msg_hdr, flags);
+ if (len < 0) {
+ ret = len;
+ break;
+ }
+ mmsg.msg_len = (unsigned int)len;
+ ret = put_mmsg(&u_p, &mmsg);
+ if (ret)
+ break;
+ datagrams++;
+ }
+
+ if (datagrams > 0 && (ret == 0 || ret == -EWOULDBLOCK)) {
+ /* NOTE: SO_ERROR should be honored for other errors. */
+ rtdm_fd_put(fd);
+ return datagrams;
+ }
+
+ rtdm_fd_put(fd);
+out:
+ trace_cobalt_fd_sendmmsg_status(current, fd, ufd, ret);
+
+ return ret;
+}
+
static void
__fd_close(struct cobalt_ppd *p, struct rtdm_fd_index *idx, spl_t s)
{
diff --git a/kernel/cobalt/trace/cobalt-rtdm.h
b/kernel/cobalt/trace/cobalt-rtdm.h
index 6634cd6..91b6390 100644
--- a/kernel/cobalt/trace/cobalt-rtdm.h
+++ b/kernel/cobalt/trace/cobalt-rtdm.h
@@ -268,6 +268,13 @@ DEFINE_EVENT(fd_request, cobalt_fd_sendmsg,
TP_ARGS(task, fd, ufd, flags)
);
+DEFINE_EVENT(fd_request, cobalt_fd_sendmmsg,
+ TP_PROTO(struct task_struct *task,
+ struct rtdm_fd *fd, int ufd,
+ unsigned long flags),
+ TP_ARGS(task, fd, ufd, flags)
+);
+
DEFINE_EVENT(fd_request, cobalt_fd_recvmsg,
TP_PROTO(struct task_struct *task,
struct rtdm_fd *fd, int ufd,
@@ -275,6 +282,13 @@ DEFINE_EVENT(fd_request, cobalt_fd_recvmsg,
TP_ARGS(task, fd, ufd, flags)
);
+DEFINE_EVENT(fd_request, cobalt_fd_recvmmsg,
+ TP_PROTO(struct task_struct *task,
+ struct rtdm_fd *fd, int ufd,
+ unsigned long flags),
+ TP_ARGS(task, fd, ufd, flags)
+);
+
#define cobalt_print_protbits(__prot) \
__print_flags(__prot, "|", \
{PROT_EXEC, "exec"}, \
@@ -357,6 +371,13 @@ DEFINE_EVENT(fd_request_status, cobalt_fd_recvmsg_status,
TP_ARGS(task, fd, ufd, status)
);
+DEFINE_EVENT(fd_request_status, cobalt_fd_recvmmsg_status,
+ TP_PROTO(struct task_struct *task,
+ struct rtdm_fd *fd, int ufd,
+ int status),
+ TP_ARGS(task, fd, ufd, status)
+);
+
DEFINE_EVENT(fd_request_status, cobalt_fd_sendmsg_status,
TP_PROTO(struct task_struct *task,
struct rtdm_fd *fd, int ufd,
@@ -364,6 +385,13 @@ DEFINE_EVENT(fd_request_status, cobalt_fd_sendmsg_status,
TP_ARGS(task, fd, ufd, status)
);
+DEFINE_EVENT(fd_request_status, cobalt_fd_sendmmsg_status,
+ TP_PROTO(struct task_struct *task,
+ struct rtdm_fd *fd, int ufd,
+ int status),
+ TP_ARGS(task, fd, ufd, status)
+);
+
DEFINE_EVENT(fd_request_status, cobalt_fd_mmap_status,
TP_PROTO(struct task_struct *task,
struct rtdm_fd *fd, int ufd,
_______________________________________________
Xenomai-git mailing list
Xenomai-git@xenomai.org
https://xenomai.org/mailman/listinfo/xenomai-git
