Commit fbf5f1be authored by Philippe Gerum's avatar Philippe Gerum
Browse files

cobalt/rtdm: add sendmmsg(), recvmmsg() syscalls

parent 3c5f51d0
......@@ -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);
......
......@@ -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);
......
......@@ -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 */
......
......@@ -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)
......
......@@ -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))
......
......@@ -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));
......
......@@ -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))
......
......@@ -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,
......
......@@ -7,4 +7,4 @@ xenomai-y := core.o \
fd.o \
wrappers.o
ccflags-y += -I$(src)/..
ccflags-y += -I$(src)/.. -Ikernel
......@@ -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_IGRAVITY);
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)
{
......
......@@ -157,8 +157,9 @@
__cobalt_symbolic_syscall(backtrace), \
__cobalt_symbolic_syscall(serialdbg), \
__cobalt_symbolic_syscall(extend), \
__cobalt_symbolic_syscall(ftrace_puts))
__cobalt_symbolic_syscall(ftrace_puts), \
__cobalt_symbolic_syscall(recvmmsg), \
__cobalt_symbolic_syscall(sendmmsg))
DECLARE_EVENT_CLASS(syscall_entry,
TP_PROTO(unsigned int nr),
......
......@@ -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,
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment