From 86df5badf5430ee676d662a80e6179f4e8a082e3 Mon Sep 17 00:00:00 2001 From: Robert Tivy Date: Mon, 11 Mar 2013 17:01:23 -0700 Subject: [PATCH] Add new RPMSG socket support --- drivers/rpmsg/virtio_rpmsg_bus.c | 87 +++++- include/linux/rpmsg.h | 4 + include/linux/socket.h | 5 +- include/net/rpmsg.h | 58 ++++ net/Makefile | 1 + net/rpmsg/Makefile | 1 + net/rpmsg/rpmsg_proto.c | 623 ++++++++++++++++++++++++++++++++++++++ 7 files changed, 769 insertions(+), 10 deletions(-) create mode 100644 include/net/rpmsg.h create mode 100644 net/rpmsg/Makefile create mode 100644 net/rpmsg/rpmsg_proto.c diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c index ae92ae7..9e2f034 100644 --- a/drivers/rpmsg/virtio_rpmsg_bus.c +++ b/drivers/rpmsg/virtio_rpmsg_bus.c @@ -51,6 +51,7 @@ * @sendq: wait queue of sending contexts waiting for a tx buffers * @sleepers: number of senders that are waiting for a tx buffer * @ns_ept: the bus's name service endpoint + * @id: unique system-wide index id for this vproc * * This structure stores the rpmsg state of a given virtio remote processor * device (there might be several virtio proc devices for each physical @@ -68,8 +69,15 @@ struct virtproc_info { wait_queue_head_t sendq; atomic_t sleepers; struct rpmsg_endpoint *ns_ept; + int id; }; +int get_virtproc_id(struct virtproc_info *vrp) +{ + return vrp->id; +} +EXPORT_SYMBOL(get_virtproc_id); + /** * struct rpmsg_channel_info - internal channel info representation * @name: name of service @@ -134,6 +142,16 @@ rpmsg_show_attr(dst, dst, "0x%x\n"); rpmsg_show_attr(announce, announce ? "true" : "false", "%s\n"); /* + * The virtio devices we are probed with represent remote processors + * on the system. We call them _virtual_ processors, because every physical + * remote processor might actually have several virtio devices. + * + * The idr below is used to assign each vproc a unique, system-wide, index id. + */ +static struct idr vprocs; +static DEFINE_MUTEX(vprocs_mutex); + +/* * Unique (and free running) index for rpmsg devices. * * Yeah, we're not recycling those numbers (yet?). will be easy @@ -499,7 +517,7 @@ static int rpmsg_channel_match(struct device *dev, void *data) * this function will be used to create both static and dynamic * channels. */ -static struct rpmsg_channel *rpmsg_create_channel(struct virtproc_info *vrp, +static struct rpmsg_channel *__rpmsg_create_channel(struct virtproc_info *vrp, struct rpmsg_channel_info *chinfo) { struct rpmsg_channel *rpdev; @@ -555,7 +573,7 @@ static struct rpmsg_channel *rpmsg_create_channel(struct virtproc_info *vrp, * find an existing channel using its name + address properties, * and destroy it */ -static int rpmsg_destroy_channel(struct virtproc_info *vrp, +static int __rpmsg_destroy_channel(struct virtproc_info *vrp, struct rpmsg_channel_info *chinfo) { struct virtio_device *vdev = vrp->vdev; @@ -755,8 +773,10 @@ int rpmsg_send_offchannel_raw(struct rpmsg_channel *rpdev, u32 src, u32 dst, dev_dbg(dev, "TX From 0x%x, To 0x%x, Len %d, Flags %d, Reserved %d\n", msg->src, msg->dst, msg->len, msg->flags, msg->reserved); +#ifdef DEBUG_VERBOSE print_hex_dump(KERN_DEBUG, "rpmsg_virtio TX: ", DUMP_PREFIX_NONE, 16, 1, msg, sizeof(*msg) + msg->len, true); +#endif sg_init_one(&sg, msg, sizeof(*msg) + len); @@ -891,6 +911,25 @@ static void rpmsg_xmit_done(struct virtqueue *svq) wake_up_interruptible(&vrp->sendq); } +struct rpmsg_channel *rpmsg_create_channel(int vrp_id, const char *name, + int src, int dst) +{ + struct rpmsg_channel_info chinfo; + struct virtproc_info *vrp; + + strncpy(chinfo.name, name, sizeof(chinfo.name)); + chinfo.src = src; + chinfo.dst = dst; + + /* TODO we probably want radix tree and fw-induced id numbers ? */ + vrp = idr_find(&vprocs, vrp_id); + if (!vrp) + return NULL; + + return __rpmsg_create_channel(vrp, &chinfo); +} +EXPORT_SYMBOL(rpmsg_create_channel); + /* invoked when a name service announcement arrives */ static void rpmsg_ns_cb(struct rpmsg_channel *rpdev, void *data, int len, void *priv, u32 src) @@ -902,10 +941,11 @@ static void rpmsg_ns_cb(struct rpmsg_channel *rpdev, void *data, int len, struct device *dev = &vrp->vdev->dev; int ret; +#ifdef DEBUG_VERBOSE print_hex_dump(KERN_DEBUG, "NS announcement: ", DUMP_PREFIX_NONE, 16, 1, data, len, true); - +#endif if (len != sizeof(*msg)) { dev_err(dev, "malformed ns msg (%d)\n", len); return; @@ -934,13 +974,13 @@ static void rpmsg_ns_cb(struct rpmsg_channel *rpdev, void *data, int len, chinfo.dst = msg->addr; if (msg->flags & RPMSG_NS_DESTROY) { - ret = rpmsg_destroy_channel(vrp, &chinfo); + ret = __rpmsg_destroy_channel(vrp, &chinfo); if (ret) - dev_err(dev, "rpmsg_destroy_channel failed: %d\n", ret); + dev_err(dev, "__rpmsg_destroy_channel err: %d\n", ret); } else { - newch = rpmsg_create_channel(vrp, &chinfo); + newch = __rpmsg_create_channel(vrp, &chinfo); if (!newch) - dev_err(dev, "rpmsg_create_channel failed\n"); + dev_err(dev, "__rpmsg_create_channel failed\n"); } } @@ -951,7 +991,7 @@ static int rpmsg_probe(struct virtio_device *vdev) struct virtqueue *vqs[2]; struct virtproc_info *vrp; void *bufs_va; - int err = 0, i; + int err = 0, i, vproc_id; vrp = kzalloc(sizeof(*vrp), GFP_KERNEL); if (!vrp) @@ -964,10 +1004,26 @@ static int rpmsg_probe(struct virtio_device *vdev) mutex_init(&vrp->tx_lock); init_waitqueue_head(&vrp->sendq); + if (!idr_pre_get(&vprocs, GFP_KERNEL)) + goto free_vrp; + + mutex_lock(&vprocs_mutex); + + err = idr_get_new(&vprocs, vrp, &vproc_id); + + mutex_unlock(&vprocs_mutex); + + if (err) { + dev_err(&vdev->dev, "idr_get_new failed: %d\n", err); + goto free_vrp; + } + + vrp->id = vproc_id; + /* We expect two virtqueues, rx and tx (and in this order) */ err = vdev->config->find_vqs(vdev, 2, vqs, vq_cbs, names); if (err) - goto free_vrp; + goto rem_idr; vrp->rvq = vqs[0]; vrp->svq = vqs[1]; @@ -1029,6 +1085,10 @@ free_coherent: bufs_va, vrp->bufs_dma); vqs_del: vdev->config->del_vqs(vrp->vdev); +rem_idr: + mutex_lock(&vprocs_mutex); + idr_remove(&vprocs, vproc_id); + mutex_unlock(&vprocs_mutex); free_vrp: kfree(vrp); return err; @@ -1063,6 +1123,10 @@ static void rpmsg_remove(struct virtio_device *vdev) dma_free_coherent(vdev->dev.parent->parent, RPMSG_TOTAL_BUF_SPACE, vrp->rbufs, vrp->bufs_dma); + mutex_lock(&vprocs_mutex); + idr_remove(&vprocs, vrp->id); + mutex_unlock(&vprocs_mutex); + kfree(vrp); } @@ -1089,6 +1153,8 @@ static int __init rpmsg_init(void) { int ret; + idr_init(&vprocs); + ret = bus_register(&rpmsg_bus); if (ret) { pr_err("failed to register rpmsg bus: %d\n", ret); @@ -1109,6 +1175,9 @@ static void __exit rpmsg_fini(void) { unregister_virtio_driver(&virtio_ipc_driver); bus_unregister(&rpmsg_bus); + + idr_remove_all(&vprocs); + idr_destroy(&vprocs); } module_exit(rpmsg_fini); diff --git a/include/linux/rpmsg.h b/include/linux/rpmsg.h index 82a6739..05d0bc8 100644 --- a/include/linux/rpmsg.h +++ b/include/linux/rpmsg.h @@ -329,4 +329,8 @@ int rpmsg_trysend_offchannel(struct rpmsg_channel *rpdev, u32 src, u32 dst, return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, false); } +int get_virtproc_id(struct virtproc_info *vrp); +struct rpmsg_channel *rpmsg_create_channel(int vrp_id, const char *name, + int src, int dst); + #endif /* _LINUX_RPMSG_H */ diff --git a/include/linux/socket.h b/include/linux/socket.h index 9a546ff..a0dc8f1 100644 --- a/include/linux/socket.h +++ b/include/linux/socket.h @@ -178,7 +178,8 @@ struct ucred { #define AF_CAIF 37 /* CAIF sockets */ #define AF_ALG 38 /* Algorithm sockets */ #define AF_NFC 39 /* NFC sockets */ -#define AF_MAX 40 /* For now.. */ +#define AF_RPMSG 40 /* Remote-processor messaging */ +#define AF_MAX 41 /* For now.. */ /* Protocol families, same as address families. */ #define PF_UNSPEC AF_UNSPEC @@ -221,6 +222,7 @@ struct ucred { #define PF_CAIF AF_CAIF #define PF_ALG AF_ALG #define PF_NFC AF_NFC +#define PF_RPMSG AF_RPMSG #define PF_MAX AF_MAX /* Maximum queue length specifiable by listen. */ @@ -296,6 +298,7 @@ struct ucred { #define SOL_IUCV 277 #define SOL_CAIF 278 #define SOL_ALG 279 +#define SOL_RPMSG 280 /* IPX options */ #define IPX_TYPE 1 diff --git a/include/net/rpmsg.h b/include/net/rpmsg.h new file mode 100644 index 0000000..ab5b174 --- /dev/null +++ b/include/net/rpmsg.h @@ -0,0 +1,58 @@ +/* + * Remote processor messaging sockets + * + * Copyright (C) 2011 Texas Instruments, Inc + * + * Ohad Ben-Cohen + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * 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. + */ + +#ifndef __NET_RPMSG_H +#define __NET_RPMSG_H + +#include +#include + +/* user space needs this */ +#ifndef AF_RPMSG +#define AF_RPMSG 40 +#define PF_RPMSG AF_RPMSG +#endif + +/* Connection and socket states */ +enum { + RPMSG_CONNECTED = 1, /* wait_for_packet() wants this... */ + RPMSG_OPEN, + RPMSG_LISTENING, + RPMSG_CLOSED, +}; + +struct sockaddr_rpmsg { + sa_family_t family; + __u32 vproc_id; + __u32 addr; +}; + +#define RPMSG_LOCALHOST ((__u32) ~0UL) + +#ifdef __KERNEL__ + +#include +#include + +struct rpmsg_socket { + struct sock sk; + struct rpmsg_channel *rpdev; + bool unregister_rpdev; +}; + +#endif /* __KERNEL__ */ +#endif /* __NET_RPMSG_H */ diff --git a/net/Makefile b/net/Makefile index 4f4ee08..764e6de 100644 --- a/net/Makefile +++ b/net/Makefile @@ -70,3 +70,4 @@ obj-$(CONFIG_CEPH_LIB) += ceph/ obj-$(CONFIG_BATMAN_ADV) += batman-adv/ obj-$(CONFIG_NFC) += nfc/ obj-$(CONFIG_OPENVSWITCH) += openvswitch/ +obj-$(CONFIG_RPMSG) += rpmsg/ diff --git a/net/rpmsg/Makefile b/net/rpmsg/Makefile new file mode 100644 index 0000000..4db19df --- /dev/null +++ b/net/rpmsg/Makefile @@ -0,0 +1 @@ +obj-$(CONFIG_RPMSG) += rpmsg_proto.o diff --git a/net/rpmsg/rpmsg_proto.c b/net/rpmsg/rpmsg_proto.c new file mode 100644 index 0000000..bb4b78a --- /dev/null +++ b/net/rpmsg/rpmsg_proto.c @@ -0,0 +1,623 @@ +/* + * AF_RPMSG: Remote processor messaging sockets + * + * Copyright (C) 2011 Texas Instruments, Inc. + * + * Ohad Ben-Cohen + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + * + * 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. + */ + +#define pr_fmt(fmt) "%s: " fmt, __func__ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define RPMSG_CB(skb) (*(struct sockaddr_rpmsg *)&((skb)->cb)) + +/* + * Used to distinguish between bound and connected socket channels in the + * radix tree index space. + * Must match value as in drivers/rpmsg/virtio_rpsmg_bus.c: + */ +#define RPMSG_RESERVED_ADDRESSES (1024) + +/* + * A two-level radix-tree-based scheme is used to maintain the rpmsg channels + * we're exposing to userland. The first radix tree maps vproc index id + * to its channels, and the second radix tree associates each channel + * with its destination addresses (so sockaddr_rpmsg lookups are quick). + * + * Currently only channels with a valid dst address are supported (aka 'client' + * channels as opposed to 'server' channels which usually only have a valid + * src address). + */ +static RADIX_TREE(rpmsg_channels, GFP_KERNEL); + +/* + * Synchronization of access to the tree is achieved using a mutex, + * because we're using non-atomic radix tree allocations. + */ +static DEFINE_MUTEX(rpmsg_channels_lock); + +static struct proto rpmsg_proto = { + .name = "RPMSG", + .owner = THIS_MODULE, + .obj_size = sizeof(struct rpmsg_socket), +}; + +static int rpmsg_sock_connect(struct socket *sock, struct sockaddr *addr, + int alen, int flags) +{ + struct sock *sk = sock->sk; + struct rpmsg_socket *rpsk; + struct sockaddr_rpmsg *sa; + int err = 0; + struct radix_tree_root *vrp_channels; + struct rpmsg_channel *rpdev; + + pr_debug("sk %p\n", sk); + + if (sk->sk_state != RPMSG_OPEN) + return -EBADFD; + + if (sk->sk_type != SOCK_SEQPACKET) + return -EINVAL; + + if (!addr || addr->sa_family != AF_RPMSG) + return -EINVAL; + + if (alen < sizeof(*sa)) + return -EINVAL; + + sa = (struct sockaddr_rpmsg *) addr; + + lock_sock(sk); + + rpsk = container_of(sk, struct rpmsg_socket, sk); + + mutex_lock(&rpmsg_channels_lock); + + /* find the set of channels exposed by this remote processor */ + vrp_channels = radix_tree_lookup(&rpmsg_channels, sa->vproc_id); + if (!vrp_channels) { + err = -EINVAL; + goto out; + } + + /* find the specific channel we need to connect with, by dst addr: */ + rpdev = radix_tree_lookup(vrp_channels, sa->addr); + if (!rpdev) { + err = -EINVAL; + goto out; + } + + rpsk->rpdev = rpdev; + + /* bind this socket with its rpmsg endpoint */ + rpdev->ept->priv = sk; + + /* XXX take care of disconnection state too */ + sk->sk_state = RPMSG_CONNECTED; + +out: + mutex_unlock(&rpmsg_channels_lock); + release_sock(sk); + return err; +} + +static int rpmsg_sock_sendmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len) +{ + struct sock *sk = sock->sk; + struct rpmsg_socket *rpsk; + char payload[512];/* todo: sane payload length methodology */ + int err; + + pr_debug("sk %p len %d\n", sk, len); + + /* XXX check for sock_error as well ? */ + /* XXX handle noblock ? */ + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + /* no payload ? */ + if (msg->msg_iov->iov_base == NULL) + return -EINVAL; + + lock_sock(sk); + + /* we don't support loopback at this point */ + if (sk->sk_state != RPMSG_CONNECTED) { + release_sock(sk); + return -ENOTCONN; + } + + rpsk = container_of(sk, struct rpmsg_socket, sk); + + /* XXX for now, ignore the peer address. later use it + * with rpmsg_sendto, but only if user is root */ + + err = memcpy_fromiovec(payload, msg->msg_iov, len); + if (err) + goto out; + + /* XXX add length validation */ + err = rpmsg_send(rpsk->rpdev, payload, len); + if (err) + pr_err("rpmsg_send failed: %d\n", err); + +out: + release_sock(sk); + return err; +} + +static int rpmsg_sock_recvmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len, int flags) +{ + struct sock *sk = sock->sk; + struct sockaddr_rpmsg *sa; + struct sk_buff *skb; + int noblock = flags & MSG_DONTWAIT; + int ret; + + pr_debug("sk %p len %d\n", sk, len); + + if (flags & MSG_OOB) { + pr_err("MSG_OOB: %d\n", EOPNOTSUPP); + return -EOPNOTSUPP; + } + + msg->msg_namelen = 0; + + skb = skb_recv_datagram(sk, flags, noblock, &ret); + if (!skb) { + /* check for shutdown ? */ + pr_err("skb_recv_datagram: %d\n", ret); + return ret; + } + + if (msg->msg_name) { + msg->msg_namelen = sizeof(*sa); + sa = (struct sockaddr_rpmsg *) msg->msg_name; + sa->vproc_id = RPMSG_CB(skb).vproc_id; + sa->addr = RPMSG_CB(skb).addr; + sa->family = AF_RPMSG; + } + + if (len > skb->len) { + len = skb->len; + } else if (len < skb->len) { + pr_warn("user buffer is too small\n"); + /* XXX truncate or error ? */ + msg->msg_flags |= MSG_TRUNC; + } + + ret = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, len); + if (ret) { + pr_err("error copying skb data: %d\n", ret); + goto out_free; + } + + ret = len; + +out_free: + skb_free_datagram(sk, skb); + return ret; +} + +static unsigned int rpmsg_sock_poll(struct file *file, struct socket *sock, + poll_table *wait) +{ + struct sock *sk = sock->sk; + unsigned int mask = 0; + + pr_debug("sk %p\n", sk); + + poll_wait(file, sk_sleep(sk), wait); + + /* exceptional events? */ + if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue)) + mask |= POLLERR; + if (sk->sk_shutdown & RCV_SHUTDOWN) + mask |= POLLRDHUP; + if (sk->sk_shutdown == SHUTDOWN_MASK) + mask |= POLLHUP; + + /* readable? */ + if (!skb_queue_empty(&sk->sk_receive_queue) || + (sk->sk_shutdown & RCV_SHUTDOWN)) + mask |= POLLIN | POLLRDNORM; + + if (sk->sk_state == RPMSG_CLOSED) + mask |= POLLHUP; + + /* + * XXX is writable ? + * this depends on the destination processor. + * if loopback: we're writable unless no memory + * if to remote: we need enabled rpmsg buffer or user supplied bufs + * for now, let's always be writable. + */ + mask |= POLLOUT | POLLWRNORM | POLLWRBAND; + + return mask; +} +EXPORT_SYMBOL(rpmsg_sock_poll); + +/* + * return bound socket address information, either local or remote + * note: len is just an output parameter, doesn't carry any input value + */ +static int rpmsg_sock_getname(struct socket *sock, struct sockaddr *addr, + int *len, int peer) +{ + struct sock *sk = sock->sk; + struct rpmsg_socket *rpsk; + struct rpmsg_channel *rpdev; + struct sockaddr_rpmsg *sa; + + pr_debug("sk %p\n", sk); + + rpsk = container_of(sk, struct rpmsg_socket, sk); + rpdev = rpsk->rpdev; + + if (!rpdev) + return -ENOTCONN; + + addr->sa_family = AF_RPMSG; + + sa = (struct sockaddr_rpmsg *) addr; + + *len = sizeof(*sa); + + if (peer) { + sa->vproc_id = get_virtproc_id(rpdev->vrp); + sa->addr = rpdev->dst; + } else { + sa->vproc_id = RPMSG_LOCALHOST; + sa->addr = rpsk->rpdev->src; + } + + return 0; +} + +static int rpmsg_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + struct rpmsg_socket *rpsk = container_of(sk, struct rpmsg_socket, sk); + + pr_debug("sk %p\n", sk); + + if (!sk) + return 0; + + if (rpsk->unregister_rpdev) + device_unregister(&rpsk->rpdev->dev); + + sock_put(sock->sk); + + return 0; +} + +/* + * Notes: + * - calling connect after bind isn't currently supported (is it even needed ?). + * - userspace arguments to bind aren't intuitive: one needs to provide + * the vproc id of the remote processor he wants the channel to be shared + * with, and the -local- address he wants the channel to be bind with + */ +static int +rpmsg_sock_bind(struct socket *sock, struct sockaddr *uaddr, int addr_len) +{ + struct sock *sk = sock->sk; + struct rpmsg_socket *rpsk = container_of(sk, struct rpmsg_socket, sk); + struct rpmsg_channel *rpdev; + struct sockaddr_rpmsg *sa = (struct sockaddr_rpmsg *)uaddr; + + pr_debug("sk %p\n", sk); + + if (sock->state == SS_CONNECTED) + return -EINVAL; + + if (addr_len != sizeof(*sa)) + return -EINVAL; + + if (sa->family != AF_RPMSG) + return -EINVAL; + + if (rpsk->rpdev) + return -EBUSY; + + if (sk->sk_state != RPMSG_OPEN) + return -EINVAL; + + rpdev = rpmsg_create_channel(sa->vproc_id, "rpmsg-proto", sa->addr, + RPMSG_ADDR_ANY); + if (!rpdev) + return -EINVAL; + + rpsk->rpdev = rpdev; + rpsk->unregister_rpdev = true; + + /* bind this socket with its rpmsg endpoint */ + rpdev->ept->priv = sk; + + sk->sk_state = RPMSG_LISTENING; + + return 0; +} + +static const struct proto_ops rpmsg_sock_ops = { + .family = PF_RPMSG, + .owner = THIS_MODULE, + + .release = rpmsg_sock_release, + .connect = rpmsg_sock_connect, + .getname = rpmsg_sock_getname, + .sendmsg = rpmsg_sock_sendmsg, + .recvmsg = rpmsg_sock_recvmsg, + .poll = rpmsg_sock_poll, + .bind = rpmsg_sock_bind, + + .listen = sock_no_listen, + .accept = sock_no_accept, + .ioctl = sock_no_ioctl, + .mmap = sock_no_mmap, + .socketpair = sock_no_socketpair, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt +}; + +static void rpmsg_sock_destruct(struct sock *sk) +{ +} + +static int rpmsg_sock_create(struct net *net, struct socket *sock, int proto, + int kern) +{ + struct sock *sk; + + if (sock->type != SOCK_SEQPACKET) + return -ESOCKTNOSUPPORT; + if (proto != 0) + return -EPROTONOSUPPORT; + + sk = sk_alloc(net, PF_RPMSG, GFP_KERNEL, &rpmsg_proto); + if (!sk) + return -ENOMEM; + + pr_debug("sk %p\n", sk); + + sock->state = SS_UNCONNECTED; + sock->ops = &rpmsg_sock_ops; + sock_init_data(sock, sk); + + sk->sk_destruct = rpmsg_sock_destruct; + sk->sk_protocol = proto; + + sk->sk_state = RPMSG_OPEN; + + return 0; +} + +static const struct net_proto_family rpmsg_proto_family = { + .family = PF_RPMSG, + .create = rpmsg_sock_create, + .owner = THIS_MODULE, +}; + +static void __rpmsg_proto_cb(struct device *dev, int from_vproc_id, void *data, + int len, struct sock *sk, u32 src) +{ + struct rpmsg_socket *rpsk = container_of(sk, struct rpmsg_socket, sk); + struct sk_buff *skb; + int ret; + +#ifdef DEBUG_VERBOSE /* Quiet this to improve rpmsg benchmarks: */ + print_hex_dump(KERN_DEBUG, __func__, DUMP_PREFIX_NONE, 16, 1, + data, len, true); +#endif + + lock_sock(sk); + + switch (sk->sk_state) { + case RPMSG_CONNECTED: + if (rpsk->rpdev->dst != src) + dev_warn(dev, "unexpected source address: %d\n", src); + break; + case RPMSG_LISTENING: + /* When an inbound message is received while we're listening, + * we implicitly become connected */ + sk->sk_state = RPMSG_CONNECTED; + rpsk->rpdev->dst = src; + break; + default: + dev_warn(dev, "unexpected inbound message (from %d)\n", src); + break; + } + + skb = sock_alloc_send_skb(sk, len, 1, &ret); + if (!skb) { + dev_err(dev, "sock_alloc_send_skb failed: %d\n", ret); + ret = -ENOMEM; + goto out; + } + + RPMSG_CB(skb).vproc_id = from_vproc_id; + RPMSG_CB(skb).addr = src; + RPMSG_CB(skb).family = AF_RPMSG; + + memcpy(skb_put(skb, len), data, len); + + ret = sock_queue_rcv_skb(sk, skb); + if (ret) { + dev_err(dev, "sock_queue_rcv_skb failed: %d\n", ret); + kfree_skb(skb); + } + +out: + release_sock(sk); +} + +static void rpmsg_proto_cb(struct rpmsg_channel *rpdev, void *data, int len, + void *priv, u32 src) +{ + int id = get_virtproc_id(rpdev->vrp); + + __rpmsg_proto_cb(&rpdev->dev, id, data, len, priv, src); +} + +/* every channel we're probed with is exposed to userland via the Socket API */ +static int rpmsg_proto_probe(struct rpmsg_channel *rpdev) +{ + struct device *dev = &rpdev->dev; + int ret, dst = rpdev->dst, src = rpdev->src, id; + struct radix_tree_root *vrp_channels; + + if (dst == RPMSG_ADDR_ANY) + return 0; + + id = get_virtproc_id(rpdev->vrp); + + mutex_lock(&rpmsg_channels_lock); + + /* are we exposing channels for this remote processor yet ? */ + vrp_channels = radix_tree_lookup(&rpmsg_channels, id); + /* not yet ? let's prepare the 2nd radix tree level then */ + if (!vrp_channels) { + vrp_channels = kzalloc(sizeof(*vrp_channels), GFP_KERNEL); + INIT_RADIX_TREE(vrp_channels, GFP_KERNEL); + /* now let's associate the new channel with its vrp */ + ret = radix_tree_insert(&rpmsg_channels, id, vrp_channels); + if (ret) { + dev_err(dev, "radix_tree_insert failed: %d\n", ret); + kfree(vrp_channels); + return ret; + } + } + + /* let's associate the new channel with its dst */ + dev_info(dev, "inserting rpmsg src: %d, dst: %d\n", src, dst); + ret = radix_tree_insert(vrp_channels, dst, rpdev); + if (ret) + dev_err(dev, "failed to add rpmsg addr %d: %d\n", dst, ret); + + mutex_unlock(&rpmsg_channels_lock); + + return ret; +} + +static void rpmsg_proto_remove(struct rpmsg_channel *rpdev) +{ + struct device *dev = &rpdev->dev; + int id, dst = rpdev->dst, src = rpdev->src; + struct radix_tree_root *vrp_channels; + + if (dst == RPMSG_ADDR_ANY) + return; + + id = get_virtproc_id(rpdev->vrp); + + mutex_lock(&rpmsg_channels_lock); + + vrp_channels = radix_tree_lookup(&rpmsg_channels, id); + if (!vrp_channels) { + dev_err(dev, "can't find channels for this vrp: %d\n", id); + goto out; + } + + /* Only remove non-reserved channels from the tree, as only these + * were "probed". Note: bind is not causing a probe, and bound + * sockets have src addresses < RPMSG_RESERVED_ADDRESSES. + */ + if (src >= RPMSG_RESERVED_ADDRESSES) { + dev_info(dev, "deleting rpmsg src: %d, dst: %d\n", src, dst); + if (!radix_tree_delete(vrp_channels, dst)) + dev_err(dev, "failed to delete rpmsg %d\n", dst); + } + +out: + mutex_unlock(&rpmsg_channels_lock); +} + +static struct rpmsg_device_id rpmsg_proto_id_table[] = { + { .name = "rpmsg-proto" }, + { }, +}; +MODULE_DEVICE_TABLE(rpmsg, rpmsg_proto_id_table); + +static struct rpmsg_driver rpmsg_proto_driver = { + .drv.name = KBUILD_MODNAME, + .drv.owner = THIS_MODULE, + .id_table = rpmsg_proto_id_table, + .probe = rpmsg_proto_probe, + .callback = rpmsg_proto_cb, + .remove = rpmsg_proto_remove, +}; + +static int __init rpmsg_proto_init(void) +{ + int ret; + + ret = proto_register(&rpmsg_proto, 0); + if (ret) { + pr_err("proto_register failed: %d\n", ret); + return ret; + } + + ret = sock_register(&rpmsg_proto_family); + if (ret) { + pr_err("sock_register failed: %d\n", ret); + goto proto_unreg; + } + + /* gimme rpmsg channels to expose ! */ + ret = register_rpmsg_driver(&rpmsg_proto_driver); + if (ret) { + pr_err("register_rpmsg_driver failed: %d\n", ret); + goto sock_unreg; + } + + return 0; + +sock_unreg: + sock_unregister(PF_RPMSG); +proto_unreg: + proto_unregister(&rpmsg_proto); + return ret; +} + +static void __exit rpmsg_proto_exit(void) +{ + unregister_rpmsg_driver(&rpmsg_proto_driver); + sock_unregister(PF_RPMSG); + proto_unregister(&rpmsg_proto); +} + +module_init(rpmsg_proto_init); +module_exit(rpmsg_proto_exit); + +MODULE_DESCRIPTION("Remote processor messaging protocol"); +MODULE_LICENSE("GPL v2"); +MODULE_ALIAS_NETPROTO(AF_RPMSG); -- 1.7.9.4