/**
 * This file is part of the Xenomai project.
 *
 * @note Copyright (C) 2009 Philippe Gerum <rpm@xenomai.org>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */
#include <linux/module.h>
#include <linux/string.h>
#include <linux/poll.h>
#include <linux/slab.h>
#include <linux/time.h>
#include <cobalt/kernel/heap.h>
#include <cobalt/kernel/bufd.h>
#include <cobalt/kernel/pipe.h>
#include <rtdm/ipc.h>
#include "internal.h"

#define XDDP_SOCKET_MAGIC 0xa21a21a2

struct xddp_message {
	struct xnpipe_mh mh;
	char data[];
};

struct xddp_socket {
	int magic;
	struct sockaddr_ipc name;
	struct sockaddr_ipc peer;

	int minor;
	size_t poolsz;
	xnhandle_t handle;
	char label[XNOBJECT_NAME_LEN];
	struct rtdm_fd *fd;			/* i.e. RTDM socket fd */

	struct xddp_message *buffer;
	int buffer_port;
	struct xnheap *bufpool;
	struct xnheap privpool;
	size_t fillsz;
	size_t curbufsz;	/* Current streaming buffer size */
	u_long status;
	rtdm_lock_t lock;

	nanosecs_rel_t timeout;	/* connect()/recvmsg() timeout */
	size_t reqbufsz;	/* Requested streaming buffer size */

	int (*monitor)(struct rtdm_fd *fd, int event, long arg);
	struct rtipc_private *priv;
};

static struct sockaddr_ipc nullsa = {
	.sipc_family = AF_RTIPC,
	.sipc_port = -1
};

static struct rtdm_fd *portmap[CONFIG_XENO_OPT_PIPE_NRDEV]; /* indexes RTDM fildes */

#define _XDDP_SYNCWAIT  0
#define _XDDP_ATOMIC    1
#define _XDDP_BINDING   2
#define _XDDP_BOUND     3
#define _XDDP_CONNECTED 4

#ifdef CONFIG_XENO_OPT_VFILE

static char *__xddp_link_target(void *obj)
{
	struct xddp_socket *sk = obj;

	return kasformat("/dev/rtp%d", sk->minor);
}

extern struct xnptree rtipc_ptree;

static struct xnpnode_link __xddp_pnode = {
	.node = {
		.dirname = "xddp",
		.root = &rtipc_ptree,
		.ops = &xnregistry_vlink_ops,
	},
	.target = __xddp_link_target,
};

#else /* !CONFIG_XENO_OPT_VFILE */

static struct xnpnode_link __xddp_pnode = {
	.node = {
		.dirname = "xddp",
	},
};

#endif /* !CONFIG_XENO_OPT_VFILE */

static void *__xddp_alloc_handler(size_t size, void *skarg) /* nklock free */
{
	struct xddp_socket *sk = skarg;
	void *buf;

	/* Try to allocate memory for the incoming message. */
	buf = xnheap_alloc(sk->bufpool, size);
	if (unlikely(buf == NULL)) {
		if (sk->monitor)
			sk->monitor(sk->fd, XDDP_EVTNOBUF, size);
		if (size > xnheap_get_size(sk->bufpool))
			buf = (void *)-1; /* Will never succeed. */
	}

	return buf;
}

static int __xddp_resize_streambuf(struct xddp_socket *sk) /* sk->lock held */
{
	if (sk->buffer)
		xnheap_free(sk->bufpool, sk->buffer);

	if (sk->reqbufsz == 0) {
		sk->buffer = NULL;
		sk->curbufsz = 0;
		return 0;
	}

	sk->buffer = xnheap_alloc(sk->bufpool, sk->reqbufsz);
	if (sk->buffer == NULL) {
		sk->curbufsz = 0;
		return -ENOMEM;
	}

	sk->curbufsz = sk->reqbufsz;

	return 0;
}

static void __xddp_free_handler(void *buf, void *skarg) /* nklock free */
{
	struct xddp_socket *sk = skarg;
	rtdm_lockctx_t s;

	if (buf != sk->buffer) {
		xnheap_free(sk->bufpool, buf);
		return;
	}

	/* Reset the streaming buffer. */

	rtdm_lock_get_irqsave(&sk->lock, s);

	sk->fillsz = 0;
	sk->buffer_port = -1;
	__clear_bit(_XDDP_SYNCWAIT, &sk->status);
	__clear_bit(_XDDP_ATOMIC, &sk->status);

	/*
	 * If a XDDP_BUFSZ request is pending, resize the streaming
	 * buffer on-the-fly.
	 */
	if (unlikely(sk->curbufsz != sk->reqbufsz))
		__xddp_resize_streambuf(sk);

	rtdm_lock_put_irqrestore(&sk->lock, s);
}

static void __xddp_output_handler(struct xnpipe_mh *mh, void *skarg) /* nklock held */
{
	struct xddp_socket *sk = skarg;

	if (sk->monitor)
		sk->monitor(sk->fd, XDDP_EVTOUT, xnpipe_m_size(mh));
}

static int __xddp_input_handler(struct xnpipe_mh *mh, int retval, void *skarg) /* nklock held */
{
	struct xddp_socket *sk = skarg;

	if (sk->monitor) {
		if (retval == 0)
			/* Callee may alter the return value passed to userland. */
			retval = sk->monitor(sk->fd, XDDP_EVTIN, xnpipe_m_size(mh));
		else if (retval == -EPIPE && mh == NULL)
			sk->monitor(sk->fd, XDDP_EVTDOWN, 0);
	}

	if (retval == 0 &&
	    (__xnpipe_pollstate(sk->minor) & POLLIN) != 0 &&
	    xnselect_signal(&sk->priv->recv_block, POLLIN))
		xnsched_run();

	return retval;
}

static void __xddp_release_handler(void *skarg) /* nklock free */
{
	struct xddp_socket *sk = skarg;
	void *poolmem;
	u32 poolsz;

	if (sk->bufpool == &sk->privpool) {
		poolmem = xnheap_get_membase(&sk->privpool);
		poolsz = xnheap_get_size(&sk->privpool);
		xnheap_destroy(&sk->privpool);
		xnheap_vfree(poolmem);
	} else if (sk->buffer)
		xnfree(sk->buffer);

	kfree(sk);
}

static int xddp_socket(struct rtdm_fd *fd)
{
	struct rtipc_private *priv = rtdm_fd_to_private(fd);
	struct xddp_socket *sk = priv->state;

	sk->magic = XDDP_SOCKET_MAGIC;
	sk->name = nullsa;	/* Unbound */
	sk->peer = nullsa;
	sk->minor = -1;
	sk->handle = 0;
	*sk->label = 0;
	sk->poolsz = 0;
	sk->buffer = NULL;
	sk->buffer_port = -1;
	sk->bufpool = NULL;
	sk->fillsz = 0;
	sk->status = 0;
	sk->timeout = RTDM_TIMEOUT_INFINITE;
	sk->curbufsz = 0;
	sk->reqbufsz = 0;
	sk->monitor = NULL;
	rtdm_lock_init(&sk->lock);
	sk->priv = priv;

	return 0;
}

static void xddp_close(struct rtdm_fd *fd)
{
	struct rtipc_private *priv = rtdm_fd_to_private(fd);
	struct xddp_socket *sk = priv->state;
	rtdm_lockctx_t s;

	sk->monitor = NULL;

	if (!test_bit(_XDDP_BOUND, &sk->status))
		return;

	cobalt_atomic_enter(s);
	portmap[sk->name.sipc_port] = NULL;
	cobalt_atomic_leave(s);

	if (sk->handle)
		xnregistry_remove(sk->handle);

	xnpipe_disconnect(sk->minor);
}

static ssize_t __xddp_recvmsg(struct rtdm_fd *fd,
			      struct iovec *iov, int iovlen, int flags,
			      struct sockaddr_ipc *saddr)
{
	struct rtipc_private *priv = rtdm_fd_to_private(fd);
	struct xddp_message *mbuf = NULL; /* Fake GCC */
	struct xddp_socket *sk = priv->state;
	ssize_t maxlen, len, wrlen, vlen;
	nanosecs_rel_t timeout;
	struct xnpipe_mh *mh;
	int nvec, rdoff, ret;
	struct xnbufd bufd;
	spl_t s;

	if (!test_bit(_XDDP_BOUND, &sk->status))
		return -EAGAIN;

	maxlen = rtdm_get_iov_flatlen(iov, iovlen);
	if (maxlen == 0)
		return 0;

	timeout = (flags & MSG_DONTWAIT) ? RTDM_TIMEOUT_NONE : sk->timeout;
	/* Pull heading message from the input queue. */
	len = xnpipe_recv(sk->minor, &mh, timeout);
	if (len < 0)
		return len == -EIDRM ? 0 : len;
	if (len > maxlen) {
		ret = -ENOBUFS;
		goto out;
	}

	mbuf = container_of(mh, struct xddp_message, mh);

	if (saddr)
		*saddr = sk->name;

	/* Write "len" bytes from mbuf->data to the vector cells */
	for (ret = 0, nvec = 0, rdoff = 0, wrlen = len;
	     nvec < iovlen && wrlen > 0; nvec++) {
		if (iov[nvec].iov_len == 0)
			continue;
		vlen = wrlen >= iov[nvec].iov_len ? iov[nvec].iov_len : wrlen;
		if (rtdm_fd_is_user(fd)) {
			xnbufd_map_uread(&bufd, iov[nvec].iov_base, vlen);
			ret = xnbufd_copy_from_kmem(&bufd, mbuf->data + rdoff, vlen);
			xnbufd_unmap_uread(&bufd);
		} else {
			xnbufd_map_kread(&bufd, iov[nvec].iov_base, vlen);
			ret = xnbufd_copy_from_kmem(&bufd, mbuf->data + rdoff, vlen);
			xnbufd_unmap_kread(&bufd);
		}
		if (ret < 0)
			goto out;
		iov[nvec].iov_base += vlen;
		iov[nvec].iov_len -= vlen;
		wrlen -= vlen;
		rdoff += vlen;
	}
out:
	xnheap_free(sk->bufpool, mbuf);
	cobalt_atomic_enter(s);
	if ((__xnpipe_pollstate(sk->minor) & POLLIN) == 0 &&
	    xnselect_signal(&priv->recv_block, 0))
		xnsched_run();
	cobalt_atomic_leave(s);

	return ret ?: len;
}

static ssize_t xddp_recvmsg(struct rtdm_fd *fd,
			    struct user_msghdr *msg, int flags)
{
	struct iovec iov_fast[RTDM_IOV_FASTMAX], *iov;
	struct sockaddr_ipc saddr;
	ssize_t ret;

	if (flags & ~MSG_DONTWAIT)
		return -EINVAL;

	if (msg->msg_name) {
		if (msg->msg_namelen < sizeof(struct sockaddr_ipc))
			return -EINVAL;
	} else if (msg->msg_namelen != 0)
		return -EINVAL;

	if (msg->msg_iovlen >= UIO_MAXIOV)
		return -EINVAL;

	/* Copy I/O vector in */
	ret = rtdm_get_iovec(fd, &iov, msg, iov_fast);
	if (ret)
		return ret;

	ret = __xddp_recvmsg(fd, iov, msg->msg_iovlen, flags, &saddr);
	if (ret <= 0) {
		rtdm_drop_iovec(iov, iov_fast);
		return ret;
	}

	/* Copy the updated I/O vector back */
	if (rtdm_put_iovec(fd, iov, msg, iov_fast))
		return -EFAULT;

	/* Copy the source address if required. */
	if (msg->msg_name) {
		if (rtipc_put_arg(fd, msg->msg_name, &saddr, sizeof(saddr)))
			return -EFAULT;
		msg->msg_namelen = sizeof(struct sockaddr_ipc);
	}

	return ret;
}

static ssize_t xddp_read(struct rtdm_fd *fd, void *buf, size_t len)
{
	struct iovec iov = { .iov_base = buf, .iov_len = len };

	return __xddp_recvmsg(fd, &iov, 1, 0, NULL);
}

static ssize_t __xddp_stream(struct xddp_socket *sk,
			     int from, struct xnbufd *bufd)
{
	struct xddp_message *mbuf;
	size_t fillptr, rembytes;
	rtdm_lockctx_t s;
	ssize_t outbytes;
	int ret;

	/*
	 * xnpipe_msend() and xnpipe_mfixup() routines will only grab
	 * the nklock directly or indirectly, so holding our socket
	 * lock across those calls is fine.
	 */
	rtdm_lock_get_irqsave(&sk->lock, s);

	/*
	 * There are two cases in which we must remove the cork
	 * unconditionally and send the incoming data as a standalone
	 * datagram: the destination port does not support streaming,
	 * or its streaming buffer is already filled with data issued
	 * from another port.
	 */
	if (sk->curbufsz == 0 ||
	    (sk->buffer_port >= 0 && sk->buffer_port != from)) {
		/* This will end up into a standalone datagram. */
		outbytes = 0;
		goto out;
	}

	mbuf = sk->buffer;
	rembytes = sk->curbufsz - sizeof(*mbuf) - sk->fillsz;
	outbytes = bufd->b_len > rembytes ? rembytes : bufd->b_len;
	if (likely(outbytes > 0)) {
	repeat:
		/* Mark the beginning of a should-be-atomic section. */
		__set_bit(_XDDP_ATOMIC, &sk->status);
		fillptr = sk->fillsz;
		sk->fillsz += outbytes;

		rtdm_lock_put_irqrestore(&sk->lock, s);
		ret = xnbufd_copy_to_kmem(mbuf->data + fillptr,
					  bufd, outbytes);
		rtdm_lock_get_irqsave(&sk->lock, s);

		if (ret < 0) {
			outbytes = ret;
			__clear_bit(_XDDP_ATOMIC, &sk->status);
			goto out;
		}

		/* We haven't been atomic, let's try again. */
		if (!__test_and_clear_bit(_XDDP_ATOMIC, &sk->status))
			goto repeat;

		if (__test_and_set_bit(_XDDP_SYNCWAIT, &sk->status))
			outbytes = xnpipe_mfixup(sk->minor,
						 &mbuf->mh, outbytes);
		else {
			sk->buffer_port = from;
			outbytes = xnpipe_send(sk->minor, &mbuf->mh,
					       outbytes + sizeof(*mbuf),
					       XNPIPE_NORMAL);
			if (outbytes > 0)
				outbytes -= sizeof(*mbuf);
		}
	}

out:
	rtdm_lock_put_irqrestore(&sk->lock, s);

	return outbytes;
}

static ssize_t __xddp_sendmsg(struct rtdm_fd *fd,
			      struct iovec *iov, int iovlen, int flags,
			      const struct sockaddr_ipc *daddr)
{
	struct rtipc_private *priv = rtdm_fd_to_private(fd);
	ssize_t len, rdlen, wrlen, vlen, ret, sublen;
	struct xddp_socket *sk = priv->state;
	struct xddp_message *mbuf;
	struct xddp_socket *rsk;
	struct rtdm_fd *rfd;
	int nvec, to, from;
	struct xnbufd bufd;
	rtdm_lockctx_t s;

	len = rtdm_get_iov_flatlen(iov, iovlen);
	if (len == 0)
		return 0;

	from = sk->name.sipc_port;
	to = daddr->sipc_port;

	cobalt_atomic_enter(s);
	rfd = portmap[to];
	if (rfd && rtdm_fd_lock(rfd) < 0)
		rfd = NULL;
	cobalt_atomic_leave(s);

	if (rfd == NULL)
		return -ECONNRESET;

	rsk = rtipc_fd_to_state(rfd);
	if (!test_bit(_XDDP_BOUND, &rsk->status)) {
		rtdm_fd_unlock(rfd);
		return -ECONNREFUSED;
	}

	sublen = len;
	nvec = 0;

	/*
	 * If active, the streaming buffer is already pending on the
	 * output queue, so we basically have nothing to do during a
	 * MSG_MORE -> MSG_NONE transition. Therefore, we only have to
	 * take care of filling that buffer when MSG_MORE is
	 * given. Yummie.
	 */
	if (flags & MSG_MORE) {
		for (rdlen = sublen, wrlen = 0;
		     nvec < iovlen && rdlen > 0; nvec++) {
			if (iov[nvec].iov_len == 0)
				continue;
			vlen = rdlen >= iov[nvec].iov_len ? iov[nvec].iov_len : rdlen;
			if (rtdm_fd_is_user(fd)) {
				xnbufd_map_uread(&bufd, iov[nvec].iov_base, vlen);
				ret = __xddp_stream(rsk, from, &bufd);
				xnbufd_unmap_uread(&bufd);
			} else {
				xnbufd_map_kread(&bufd, iov[nvec].iov_base, vlen);
				ret = __xddp_stream(rsk, from, &bufd);
				xnbufd_unmap_kread(&bufd);
			}
			if (ret < 0)
				goto fail_unlock;
			wrlen += ret;
			rdlen -= ret;
			iov[nvec].iov_base += ret;
			iov[nvec].iov_len -= ret;
			/*
			 * In case of a short write to the streaming
			 * buffer, send the unsent part as a
			 * standalone datagram.
			 */
			if (ret < vlen) {
				sublen = rdlen;
				goto nostream;
			}
		}
		len = wrlen;
		goto done;
	}

nostream:
	mbuf = xnheap_alloc(rsk->bufpool, sublen + sizeof(*mbuf));
	if (unlikely(mbuf == NULL)) {
		ret = -ENOMEM;
		goto fail_unlock;
	}

	/*
	 * Move "sublen" bytes to mbuf->data from the vector cells
	 */
	for (rdlen = sublen, wrlen = 0; nvec < iovlen && rdlen > 0; nvec++) {
		if (iov[nvec].iov_len == 0)
			continue;
		vlen = rdlen >= iov[nvec].iov_len ? iov[nvec].iov_len : rdlen;
		if (rtdm_fd_is_user(fd)) {
			xnbufd_map_uread(&bufd, iov[nvec].iov_base, vlen);
			ret = xnbufd_copy_to_kmem(mbuf->data + wrlen, &bufd, vlen);
			xnbufd_unmap_uread(&bufd);
		} else {
			xnbufd_map_kread(&bufd, iov[nvec].iov_base, vlen);
			ret = xnbufd_copy_to_kmem(mbuf->data + wrlen, &bufd, vlen);
			xnbufd_unmap_kread(&bufd);
		}
		if (ret < 0)
			goto fail_freebuf;
		iov[nvec].iov_base += vlen;
		iov[nvec].iov_len -= vlen;
		rdlen -= vlen;
		wrlen += vlen;
	}

	ret = xnpipe_send(rsk->minor, &mbuf->mh,
			  sublen + sizeof(*mbuf),
			  (flags & MSG_OOB) ?
			  XNPIPE_URGENT : XNPIPE_NORMAL);

	if (unlikely(ret < 0)) {
	fail_freebuf:
		xnheap_free(rsk->bufpool, mbuf);
	fail_unlock:
		rtdm_fd_unlock(rfd);
		return ret;
	}
done:
	rtdm_fd_unlock(rfd);

	return len;
}

static ssize_t xddp_sendmsg(struct rtdm_fd *fd,
			    const struct user_msghdr *msg, int flags)
{
	struct rtipc_private *priv = rtdm_fd_to_private(fd);
	struct iovec iov_fast[RTDM_IOV_FASTMAX], *iov;
	struct xddp_socket *sk = priv->state;
	struct sockaddr_ipc daddr;
	ssize_t ret;

	/*
	 * We accept MSG_DONTWAIT, but do not care about it, since
	 * writing to the real-time endpoint of a message pipe must be
	 * a non-blocking operation.
	 */
	if (flags & ~(MSG_MORE | MSG_OOB | MSG_DONTWAIT))
		return -EINVAL;

	/*
	 * MSG_MORE and MSG_OOB are mutually exclusive in our
	 * implementation.
	 */
	if ((flags & (MSG_MORE | MSG_OOB)) == (MSG_MORE | MSG_OOB))
		return -EINVAL;

	if (msg->msg_name) {
		if (msg->msg_namelen != sizeof(struct sockaddr_ipc))
			return -EINVAL;

		/* Fetch the destination address to send to. */
		if (rtipc_get_arg(fd, &daddr, msg->msg_name, sizeof(daddr)))
			return -EFAULT;

		if (daddr.sipc_port < 0 ||
		    daddr.sipc_port >= CONFIG_XENO_OPT_PIPE_NRDEV)
			return -EINVAL;
	} else {
		if (msg->msg_namelen != 0)
			return -EINVAL;
		daddr = sk->peer;
		if (daddr.sipc_port < 0)
			return -EDESTADDRREQ;
	}

	if (msg->msg_iovlen >= UIO_MAXIOV)
		return -EINVAL;

	/* Copy I/O vector in */
	ret = rtdm_get_iovec(fd, &iov, msg, iov_fast);
	if (ret)
		return ret;

	ret = __xddp_sendmsg(fd, iov, msg->msg_iovlen, flags, &daddr);
	if (ret <= 0) {
		rtdm_drop_iovec(iov, iov_fast);
		return ret;
	}

	/* Copy updated I/O vector back */
	return rtdm_put_iovec(fd, iov, msg, iov_fast) ?: ret;
}

static ssize_t xddp_write(struct rtdm_fd *fd,
			  const void *buf, size_t len)
{
	struct rtipc_private *priv = rtdm_fd_to_private(fd);
	struct iovec iov = { .iov_base = (void *)buf, .iov_len = len };
	struct xddp_socket *sk = priv->state;

	if (sk->peer.sipc_port < 0)
		return -EDESTADDRREQ;

	return __xddp_sendmsg(fd, &iov, 1, 0, &sk->peer);
}

static int __xddp_bind_socket(struct rtipc_private *priv,
			      struct sockaddr_ipc *sa)
{
	struct xddp_socket *sk = priv->state;
	struct xnpipe_operations ops;
	rtdm_lockctx_t s;
	size_t poolsz;
	void *poolmem;
	int ret = 0;

	if (sa->sipc_family != AF_RTIPC)
		return -EINVAL;

	/* Allow special port -1 for auto-selection. */
	if (sa->sipc_port < -1 ||
	    sa->sipc_port >= CONFIG_XENO_OPT_PIPE_NRDEV)
		return -EINVAL;

	cobalt_atomic_enter(s);
	if (test_bit(_XDDP_BOUND, &sk->status) ||
	    __test_and_set_bit(_XDDP_BINDING, &sk->status))
		ret = -EADDRINUSE;
	cobalt_atomic_leave(s);
	if (ret)
		return ret;

	poolsz = sk->poolsz;
	if (poolsz > 0) {
		poolsz = PAGE_ALIGN(poolsz);
		poolsz += PAGE_ALIGN(sk->reqbufsz);
		poolmem = xnheap_vmalloc(poolsz);
		if (poolmem == NULL) {
			ret = -ENOMEM;
			goto fail;
		}

		ret = xnheap_init(&sk->privpool, poolmem, poolsz);
		if (ret) {
			xnheap_vfree(poolmem);
			goto fail;
		}

		sk->bufpool = &sk->privpool;
	} else
		sk->bufpool = &cobalt_heap;

	if (sk->reqbufsz > 0) {
		sk->buffer = xnheap_alloc(sk->bufpool, sk->reqbufsz);
		if (sk->buffer == NULL) {
			ret = -ENOMEM;
			goto fail_freeheap;
		}
		sk->curbufsz = sk->reqbufsz;
	}

	sk->fd = rtdm_private_to_fd(priv);

	ops.output = &__xddp_output_handler;
	ops.input = &__xddp_input_handler;
	ops.alloc_ibuf = &__xddp_alloc_handler;
	ops.free_ibuf = &__xddp_free_handler;
	ops.free_obuf = &__xddp_free_handler;
	ops.release = &__xddp_release_handler;

	ret = xnpipe_connect(sa->sipc_port, &ops, sk);
	if (ret < 0) {
		if (ret == -EBUSY)
			ret = -EADDRINUSE;
	fail_freeheap:
		if (poolsz > 0) {
			xnheap_destroy(&sk->privpool);
			xnheap_vfree(poolmem);
		}
	fail:
		clear_bit(_XDDP_BINDING, &sk->status);
		return ret;
	}

	sk->minor = ret;
	sa->sipc_port = ret;
	sk->name = *sa;
	/* Set default destination if unset at binding time. */
	if (sk->peer.sipc_port < 0)
		sk->peer = *sa;

	if (poolsz > 0)
		xnheap_set_name(sk->bufpool, "xddp-pool@%d", sa->sipc_port);

	if (*sk->label) {
		ret = xnregistry_enter(sk->label, sk, &sk->handle,
				       &__xddp_pnode.node);
		if (ret) {
			/* The release handler will cleanup the pool for us. */
			xnpipe_disconnect(sk->minor);
			return ret;
		}
	}

	cobalt_atomic_enter(s);
	portmap[sk->minor] = rtdm_private_to_fd(priv);
	__clear_bit(_XDDP_BINDING, &sk->status);
	__set_bit(_XDDP_BOUND, &sk->status);
	if (xnselect_signal(&priv->send_block, POLLOUT))
		xnsched_run();
	cobalt_atomic_leave(s);

	return 0;
}

static int __xddp_connect_socket(struct xddp_socket *sk,
				 struct sockaddr_ipc *sa)
{
	struct sockaddr_ipc _sa;
	struct xddp_socket *rsk;
	int ret, resched = 0;
	rtdm_lockctx_t s;
	xnhandle_t h;

	if (sa == NULL) {
		_sa = nullsa;
		sa = &_sa;
		goto set_assoc;
	}

	if (sa->sipc_family != AF_RTIPC)
		return -EINVAL;

	if (sa->sipc_port < -1 ||
	    sa->sipc_port >= CONFIG_XENO_OPT_PIPE_NRDEV)
		return -EINVAL;
	/*
	 * - If a valid sipc_port is passed in the [0..NRDEV-1] range,
	 * it is used verbatim and the connection succeeds
	 * immediately, regardless of whether the destination is
	 * bound at the time of the call.
	 *
	 * - If sipc_port is -1 and a label was set via XDDP_LABEL,
	 * connect() blocks for the requested amount of time (see
	 * SO_RCVTIMEO) until a socket is bound to the same label.
	 *
	 * - If sipc_port is -1 and no label is given, the default
	 * destination address is cleared, meaning that any subsequent
	 * write() to the socket will return -EDESTADDRREQ, until a
	 * valid destination address is set via connect() or bind().
	 *
	 * - In all other cases, -EINVAL is returned.
	 */
	if (sa->sipc_port < 0 && *sk->label) {
		ret = xnregistry_bind(sk->label,
				      sk->timeout, XN_RELATIVE, &h);
		if (ret)
			return ret;

		cobalt_atomic_enter(s);
		rsk = xnregistry_lookup(h, NULL);
		if (rsk == NULL || rsk->magic != XDDP_SOCKET_MAGIC)
			ret = -EINVAL;
		else {
			/* Fetch labeled port number. */
			sa->sipc_port = rsk->minor;
			resched = xnselect_signal(&sk->priv->send_block, POLLOUT);
		}
		cobalt_atomic_leave(s);
		if (ret)
			return ret;
	} else if (sa->sipc_port < 0)
		sa = &nullsa;
set_assoc:
	cobalt_atomic_enter(s);
	if (!test_bit(_XDDP_BOUND, &sk->status))
		/* Set default name. */
		sk->name = *sa;
	/* Set default destination. */
	sk->peer = *sa;
	if (sa->sipc_port < 0)
		__clear_bit(_XDDP_CONNECTED, &sk->status);
	else
		__set_bit(_XDDP_CONNECTED, &sk->status);
	if (resched)
		xnsched_run();
	cobalt_atomic_leave(s);

	return 0;
}

static int __xddp_setsockopt(struct xddp_socket *sk,
			     struct rtdm_fd *fd,
			     void *arg)
{
	int (*monitor)(struct rtdm_fd *fd, int event, long arg);
	struct _rtdm_setsockopt_args sopt;
	struct rtipc_port_label plabel;
	struct __kernel_old_timeval tv;
	rtdm_lockctx_t s;
	size_t len;
	int ret;

	ret = rtipc_get_sockoptin(fd, &sopt, arg);
	if (ret)
		return ret;

	if (sopt.level == SOL_SOCKET) {
		switch (sopt.optname) {

		case SO_RCVTIMEO_OLD:
			ret = rtipc_get_timeval(fd, &tv, sopt.optval, sopt.optlen);
			if (ret)
				return ret;
			sk->timeout = rtipc_timeval_to_ns(&tv);
			break;

		default:
			ret = -EINVAL;
		}

		return ret;
	}

	if (sopt.level != SOL_XDDP)
		return -ENOPROTOOPT;

	switch (sopt.optname) {

	case XDDP_BUFSZ:
		ret = rtipc_get_length(fd, &len, sopt.optval, sopt.optlen);
		if (ret)
			return ret;
		if (len > 0) {
			len += sizeof(struct xddp_message);
			if (sk->bufpool &&
			    len > xnheap_get_size(sk->bufpool)) {
				return -EINVAL;
			}
		}
		rtdm_lock_get_irqsave(&sk->lock, s);
		sk->reqbufsz = len;
		if (len != sk->curbufsz &&
		    !test_bit(_XDDP_SYNCWAIT, &sk->status) &&
		    test_bit(_XDDP_BOUND, &sk->status))
			ret = __xddp_resize_streambuf(sk);
		rtdm_lock_put_irqrestore(&sk->lock, s);
		break;

	case XDDP_POOLSZ:
		ret = rtipc_get_length(fd, &len, sopt.optval, sopt.optlen);
		if (ret)
			return ret;
		if (len == 0)
			return -EINVAL;
		cobalt_atomic_enter(s);
		if (test_bit(_XDDP_BOUND, &sk->status) ||
		    test_bit(_XDDP_BINDING, &sk->status))
			ret = -EALREADY;
		else
			sk->poolsz = len;
		cobalt_atomic_leave(s);
		break;

	case XDDP_MONITOR:
		/* Monitoring is available from kernel-space only. */
		if (rtdm_fd_is_user(fd))
			return -EPERM;
		if (sopt.optlen != sizeof(monitor))
			return -EINVAL;
		if (rtipc_get_arg(NULL, &monitor, sopt.optval, sizeof(monitor)))
			return -EFAULT;
		sk->monitor = monitor;
		break;

	case XDDP_LABEL:
		if (sopt.optlen < sizeof(plabel))
			return -EINVAL;
		if (rtipc_get_arg(fd, &plabel, sopt.optval, sizeof(plabel)))
			return -EFAULT;
		cobalt_atomic_enter(s);
		if (test_bit(_XDDP_BOUND, &sk->status) ||
		    test_bit(_XDDP_BINDING, &sk->status))
			ret = -EALREADY;
		else {
			strcpy(sk->label, plabel.label);
			sk->label[XNOBJECT_NAME_LEN-1] = 0;
		}
		cobalt_atomic_leave(s);
		break;

	default:
		ret = -EINVAL;
	}

	return ret;
}

static int __xddp_getsockopt(struct xddp_socket *sk,
			     struct rtdm_fd *fd,
			     void *arg)
{
	struct _rtdm_getsockopt_args sopt;
	struct rtipc_port_label plabel;
	struct __kernel_old_timeval tv;
	rtdm_lockctx_t s;
	socklen_t len;
	int ret;

	ret = rtipc_get_sockoptout(fd, &sopt, arg);
	if (ret)
		return ret;

	if (rtipc_get_arg(fd, &len, sopt.optlen, sizeof(len)))
		return -EFAULT;

	if (sopt.level == SOL_SOCKET) {
		switch (sopt.optname) {

		case SO_RCVTIMEO_OLD:
			rtipc_ns_to_timeval(&tv, sk->timeout);
			ret = rtipc_put_timeval(fd, sopt.optval, &tv, len);
			if (ret)
				return ret;
			break;

		default:
			ret = -EINVAL;
		}

		return ret;
	}

	if (sopt.level != SOL_XDDP)
		return -ENOPROTOOPT;

	switch (sopt.optname) {

	case XDDP_LABEL:
		if (len < sizeof(plabel))
			return -EINVAL;
		cobalt_atomic_enter(s);
		strcpy(plabel.label, sk->label);
		cobalt_atomic_leave(s);
		if (rtipc_put_arg(fd, sopt.optval, &plabel, sizeof(plabel)))
			return -EFAULT;
		break;

	default:
		ret = -EINVAL;
	}

	return ret;
}

static int __xddp_ioctl(struct rtdm_fd *fd,
			unsigned int request, void *arg)
{
	struct rtipc_private *priv = rtdm_fd_to_private(fd);
	struct sockaddr_ipc saddr, *saddrp = &saddr;
	struct xddp_socket *sk = priv->state;
	int ret = 0;

	switch (request) {

	COMPAT_CASE(_RTIOC_CONNECT):
		ret = rtipc_get_sockaddr(fd, &saddrp, arg);
		if (ret == 0)
			ret = __xddp_connect_socket(sk, saddrp);
		break;

	COMPAT_CASE(_RTIOC_BIND):
		ret = rtipc_get_sockaddr(fd, &saddrp, arg);
		if (ret)
			return ret;
		if (saddrp == NULL)
			return -EFAULT;
		ret = __xddp_bind_socket(priv, saddrp);
		break;

	COMPAT_CASE(_RTIOC_GETSOCKNAME):
		ret = rtipc_put_sockaddr(fd, arg, &sk->name);
		break;

	COMPAT_CASE(_RTIOC_GETPEERNAME):
		ret = rtipc_put_sockaddr(fd, arg, &sk->peer);
		break;

	COMPAT_CASE(_RTIOC_SETSOCKOPT):
		ret = __xddp_setsockopt(sk, fd, arg);
		break;

	COMPAT_CASE(_RTIOC_GETSOCKOPT):
		ret = __xddp_getsockopt(sk, fd, arg);
		break;

	case _RTIOC_LISTEN:
	COMPAT_CASE(_RTIOC_ACCEPT):
		ret = -EOPNOTSUPP;
		break;

	case _RTIOC_SHUTDOWN:
		ret = -ENOTCONN;
		break;

	default:
		ret = -EINVAL;
	}

	return ret;
}

static int xddp_ioctl(struct rtdm_fd *fd,
		      unsigned int request, void *arg)
{
	int ret;

	switch (request) {
	COMPAT_CASE(_RTIOC_BIND):
		if (rtdm_in_rt_context())
			return -ENOSYS;	/* Try downgrading to NRT */
		fallthrough;
	default:
		ret = __xddp_ioctl(fd, request, arg);
	}

	return ret;
}

static unsigned int xddp_pollstate(struct rtdm_fd *fd) /* atomic */
{
	struct rtipc_private *priv = rtdm_fd_to_private(fd);
	struct xddp_socket *sk = priv->state, *rsk;
	unsigned int mask = 0, pollstate;
	struct rtdm_fd *rfd;

	pollstate = __xnpipe_pollstate(sk->minor);
	if (test_bit(_XDDP_BOUND, &sk->status))
		mask |= (pollstate & POLLIN);

	/*
	 * If the socket is connected, POLLOUT means that the peer
	 * exists, is bound and can receive data. Otherwise POLLOUT is
	 * always set, assuming the client is likely to use explicit
	 * addressing in send operations.
	 */
	if (test_bit(_XDDP_CONNECTED, &sk->status)) {
		rfd = portmap[sk->peer.sipc_port];
		if (rfd) {
			rsk = rtipc_fd_to_state(rfd);
			mask |= (pollstate & POLLOUT);
		}
	} else
		mask |= POLLOUT;

	return mask;
}

struct rtipc_protocol xddp_proto_driver = {
	.proto_name = "xddp",
	.proto_statesz = sizeof(struct xddp_socket),
	.proto_ops = {
		.socket = xddp_socket,
		.close = xddp_close,
		.recvmsg = xddp_recvmsg,
		.sendmsg = xddp_sendmsg,
		.read = xddp_read,
		.write = xddp_write,
		.ioctl = xddp_ioctl,
		.pollstate = xddp_pollstate,
	}
};