Linux: TransportRpmsg: Fix Mutex Deadlock Issue
[ipc/ipcdev.git] / linux / patches / 3.8.0 / rpmsg-socket.patch
1 From 86df5badf5430ee676d662a80e6179f4e8a082e3 Mon Sep 17 00:00:00 2001
2 From: Robert Tivy <rtivy@ti.com>
3 Date: Mon, 11 Mar 2013 17:01:23 -0700
4 Subject: [PATCH] Add new RPMSG socket support
6 ---
7  drivers/rpmsg/virtio_rpmsg_bus.c |   87 +++++-
8  include/linux/rpmsg.h            |    4 +
9  include/linux/socket.h           |    5 +-
10  include/net/rpmsg.h              |   58 ++++
11  net/Makefile                     |    1 +
12  net/rpmsg/Makefile               |    1 +
13  net/rpmsg/rpmsg_proto.c          |  623 ++++++++++++++++++++++++++++++++++++++
14  7 files changed, 769 insertions(+), 10 deletions(-)
15  create mode 100644 include/net/rpmsg.h
16  create mode 100644 net/rpmsg/Makefile
17  create mode 100644 net/rpmsg/rpmsg_proto.c
19 diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c
20 index ae92ae7..9e2f034 100644
21 --- a/drivers/rpmsg/virtio_rpmsg_bus.c
22 +++ b/drivers/rpmsg/virtio_rpmsg_bus.c
23 @@ -51,6 +51,7 @@
24   * @sendq:     wait queue of sending contexts waiting for a tx buffers
25   * @sleepers:  number of senders that are waiting for a tx buffer
26   * @ns_ept:    the bus's name service endpoint
27 + * @id:                unique system-wide index id for this vproc
28   *
29   * This structure stores the rpmsg state of a given virtio remote processor
30   * device (there might be several virtio proc devices for each physical
31 @@ -68,8 +69,15 @@ struct virtproc_info {
32         wait_queue_head_t sendq;
33         atomic_t sleepers;
34         struct rpmsg_endpoint *ns_ept;
35 +       int id;
36  };
37  
38 +int get_virtproc_id(struct virtproc_info *vrp)
39 +{
40 +       return vrp->id;
41 +}
42 +EXPORT_SYMBOL(get_virtproc_id);
43 +
44  /**
45   * struct rpmsg_channel_info - internal channel info representation
46   * @name: name of service
47 @@ -134,6 +142,16 @@ rpmsg_show_attr(dst, dst, "0x%x\n");
48  rpmsg_show_attr(announce, announce ? "true" : "false", "%s\n");
49  
50  /*
51 + * The virtio devices we are probed with represent remote processors
52 + * on the system. We call them _virtual_ processors, because every physical
53 + * remote processor might actually have several virtio devices.
54 + *
55 + * The idr below is used to assign each vproc a unique, system-wide, index id.
56 + */
57 +static struct idr vprocs;
58 +static DEFINE_MUTEX(vprocs_mutex);
59 +
60 +/*
61   * Unique (and free running) index for rpmsg devices.
62   *
63   * Yeah, we're not recycling those numbers (yet?). will be easy
64 @@ -499,7 +517,7 @@ static int rpmsg_channel_match(struct device *dev, void *data)
65   * this function will be used to create both static and dynamic
66   * channels.
67   */
68 -static struct rpmsg_channel *rpmsg_create_channel(struct virtproc_info *vrp,
69 +static struct rpmsg_channel *__rpmsg_create_channel(struct virtproc_info *vrp,
70                                 struct rpmsg_channel_info *chinfo)
71  {
72         struct rpmsg_channel *rpdev;
73 @@ -555,7 +573,7 @@ static struct rpmsg_channel *rpmsg_create_channel(struct virtproc_info *vrp,
74   * find an existing channel using its name + address properties,
75   * and destroy it
76   */
77 -static int rpmsg_destroy_channel(struct virtproc_info *vrp,
78 +static int __rpmsg_destroy_channel(struct virtproc_info *vrp,
79                                         struct rpmsg_channel_info *chinfo)
80  {
81         struct virtio_device *vdev = vrp->vdev;
82 @@ -755,8 +773,10 @@ int rpmsg_send_offchannel_raw(struct rpmsg_channel *rpdev, u32 src, u32 dst,
83         dev_dbg(dev, "TX From 0x%x, To 0x%x, Len %d, Flags %d, Reserved %d\n",
84                                         msg->src, msg->dst, msg->len,
85                                         msg->flags, msg->reserved);
86 +#ifdef DEBUG_VERBOSE
87         print_hex_dump(KERN_DEBUG, "rpmsg_virtio TX: ", DUMP_PREFIX_NONE, 16, 1,
88                                         msg, sizeof(*msg) + msg->len, true);
89 +#endif
90  
91         sg_init_one(&sg, msg, sizeof(*msg) + len);
92  
93 @@ -891,6 +911,25 @@ static void rpmsg_xmit_done(struct virtqueue *svq)
94         wake_up_interruptible(&vrp->sendq);
95  }
96  
97 +struct rpmsg_channel *rpmsg_create_channel(int vrp_id, const char *name,
98 +                                                       int src, int dst)
99 +{
100 +       struct rpmsg_channel_info chinfo;
101 +       struct virtproc_info *vrp;
103 +       strncpy(chinfo.name, name, sizeof(chinfo.name));
104 +       chinfo.src = src;
105 +       chinfo.dst = dst;
107 +       /* TODO we probably want radix tree and fw-induced id numbers ? */
108 +       vrp = idr_find(&vprocs, vrp_id);
109 +       if (!vrp)
110 +               return NULL;
112 +       return __rpmsg_create_channel(vrp, &chinfo);
113 +}
114 +EXPORT_SYMBOL(rpmsg_create_channel);
116  /* invoked when a name service announcement arrives */
117  static void rpmsg_ns_cb(struct rpmsg_channel *rpdev, void *data, int len,
118                                                         void *priv, u32 src)
119 @@ -902,10 +941,11 @@ static void rpmsg_ns_cb(struct rpmsg_channel *rpdev, void *data, int len,
120         struct device *dev = &vrp->vdev->dev;
121         int ret;
122  
123 +#ifdef DEBUG_VERBOSE
124         print_hex_dump(KERN_DEBUG, "NS announcement: ",
125                         DUMP_PREFIX_NONE, 16, 1,
126                         data, len, true);
128 +#endif
129         if (len != sizeof(*msg)) {
130                 dev_err(dev, "malformed ns msg (%d)\n", len);
131                 return;
132 @@ -934,13 +974,13 @@ static void rpmsg_ns_cb(struct rpmsg_channel *rpdev, void *data, int len,
133         chinfo.dst = msg->addr;
134  
135         if (msg->flags & RPMSG_NS_DESTROY) {
136 -               ret = rpmsg_destroy_channel(vrp, &chinfo);
137 +               ret = __rpmsg_destroy_channel(vrp, &chinfo);
138                 if (ret)
139 -                       dev_err(dev, "rpmsg_destroy_channel failed: %d\n", ret);
140 +                       dev_err(dev, "__rpmsg_destroy_channel err: %d\n", ret);
141         } else {
142 -               newch = rpmsg_create_channel(vrp, &chinfo);
143 +               newch = __rpmsg_create_channel(vrp, &chinfo);
144                 if (!newch)
145 -                       dev_err(dev, "rpmsg_create_channel failed\n");
146 +                       dev_err(dev, "__rpmsg_create_channel failed\n");
147         }
148  }
149  
150 @@ -951,7 +991,7 @@ static int rpmsg_probe(struct virtio_device *vdev)
151         struct virtqueue *vqs[2];
152         struct virtproc_info *vrp;
153         void *bufs_va;
154 -       int err = 0, i;
155 +       int err = 0, i, vproc_id;
156  
157         vrp = kzalloc(sizeof(*vrp), GFP_KERNEL);
158         if (!vrp)
159 @@ -964,10 +1004,26 @@ static int rpmsg_probe(struct virtio_device *vdev)
160         mutex_init(&vrp->tx_lock);
161         init_waitqueue_head(&vrp->sendq);
162  
163 +       if (!idr_pre_get(&vprocs, GFP_KERNEL))
164 +               goto free_vrp;
166 +       mutex_lock(&vprocs_mutex);
168 +       err = idr_get_new(&vprocs, vrp, &vproc_id);
170 +       mutex_unlock(&vprocs_mutex);
172 +       if (err) {
173 +               dev_err(&vdev->dev, "idr_get_new failed: %d\n", err);
174 +               goto free_vrp;
175 +       }
177 +       vrp->id = vproc_id;
179         /* We expect two virtqueues, rx and tx (and in this order) */
180         err = vdev->config->find_vqs(vdev, 2, vqs, vq_cbs, names);
181         if (err)
182 -               goto free_vrp;
183 +               goto rem_idr;
184  
185         vrp->rvq = vqs[0];
186         vrp->svq = vqs[1];
187 @@ -1029,6 +1085,10 @@ free_coherent:
188                                         bufs_va, vrp->bufs_dma);
189  vqs_del:
190         vdev->config->del_vqs(vrp->vdev);
191 +rem_idr:
192 +       mutex_lock(&vprocs_mutex);
193 +       idr_remove(&vprocs, vproc_id);
194 +       mutex_unlock(&vprocs_mutex);
195  free_vrp:
196         kfree(vrp);
197         return err;
198 @@ -1063,6 +1123,10 @@ static void rpmsg_remove(struct virtio_device *vdev)
199         dma_free_coherent(vdev->dev.parent->parent, RPMSG_TOTAL_BUF_SPACE,
200                                         vrp->rbufs, vrp->bufs_dma);
201  
202 +       mutex_lock(&vprocs_mutex);
203 +       idr_remove(&vprocs, vrp->id);
204 +       mutex_unlock(&vprocs_mutex);
206         kfree(vrp);
207  }
208  
209 @@ -1089,6 +1153,8 @@ static int __init rpmsg_init(void)
210  {
211         int ret;
212  
213 +       idr_init(&vprocs);
215         ret = bus_register(&rpmsg_bus);
216         if (ret) {
217                 pr_err("failed to register rpmsg bus: %d\n", ret);
218 @@ -1109,6 +1175,9 @@ static void __exit rpmsg_fini(void)
219  {
220         unregister_virtio_driver(&virtio_ipc_driver);
221         bus_unregister(&rpmsg_bus);
223 +       idr_remove_all(&vprocs);
224 +       idr_destroy(&vprocs);
225  }
226  module_exit(rpmsg_fini);
227  
228 diff --git a/include/linux/rpmsg.h b/include/linux/rpmsg.h
229 index 82a6739..05d0bc8 100644
230 --- a/include/linux/rpmsg.h
231 +++ b/include/linux/rpmsg.h
232 @@ -329,4 +329,8 @@ int rpmsg_trysend_offchannel(struct rpmsg_channel *rpdev, u32 src, u32 dst,
233         return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, false);
234  }
235  
236 +int get_virtproc_id(struct virtproc_info *vrp);
237 +struct rpmsg_channel *rpmsg_create_channel(int vrp_id, const char *name,
238 +                                                       int src, int dst);
240  #endif /* _LINUX_RPMSG_H */
241 diff --git a/include/linux/socket.h b/include/linux/socket.h
242 index 9a546ff..a0dc8f1 100644
243 --- a/include/linux/socket.h
244 +++ b/include/linux/socket.h
245 @@ -178,7 +178,8 @@ struct ucred {
246  #define AF_CAIF                37      /* CAIF sockets                 */
247  #define AF_ALG         38      /* Algorithm sockets            */
248  #define AF_NFC         39      /* NFC sockets                  */
249 -#define AF_MAX         40      /* For now.. */
250 +#define AF_RPMSG       40      /* Remote-processor messaging   */
251 +#define AF_MAX         41      /* For now.. */
252  
253  /* Protocol families, same as address families. */
254  #define PF_UNSPEC      AF_UNSPEC
255 @@ -221,6 +222,7 @@ struct ucred {
256  #define PF_CAIF                AF_CAIF
257  #define PF_ALG         AF_ALG
258  #define PF_NFC         AF_NFC
259 +#define PF_RPMSG       AF_RPMSG
260  #define PF_MAX         AF_MAX
261  
262  /* Maximum queue length specifiable by listen.  */
263 @@ -296,6 +298,7 @@ struct ucred {
264  #define SOL_IUCV       277
265  #define SOL_CAIF       278
266  #define SOL_ALG                279
267 +#define SOL_RPMSG      280
268  
269  /* IPX options */
270  #define IPX_TYPE       1
271 diff --git a/include/net/rpmsg.h b/include/net/rpmsg.h
272 new file mode 100644
273 index 0000000..ab5b174
274 --- /dev/null
275 +++ b/include/net/rpmsg.h
276 @@ -0,0 +1,58 @@
277 +/*
278 + * Remote processor messaging sockets
279 + *
280 + * Copyright (C) 2011 Texas Instruments, Inc
281 + *
282 + * Ohad Ben-Cohen <ohad@wizery.com>
283 + *
284 + * This program is free software; you can redistribute it and/or
285 + * modify it under the terms of the GNU General Public License
286 + * version 2 as published by the Free Software Foundation.
287 + *
288 + * This program is distributed in the hope that it will be useful,
289 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
290 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
291 + * GNU General Public License for more details.
292 + */
294 +#ifndef __NET_RPMSG_H
295 +#define __NET_RPMSG_H
297 +#include <linux/types.h>
298 +#include <linux/socket.h>
300 +/* user space needs this */
301 +#ifndef AF_RPMSG
302 +#define AF_RPMSG       40
303 +#define PF_RPMSG       AF_RPMSG
304 +#endif
306 +/* Connection and socket states */
307 +enum {
308 +       RPMSG_CONNECTED = 1, /* wait_for_packet() wants this... */
309 +       RPMSG_OPEN,
310 +       RPMSG_LISTENING,
311 +       RPMSG_CLOSED,
312 +};
314 +struct sockaddr_rpmsg {
315 +       sa_family_t family;
316 +       __u32 vproc_id;
317 +       __u32 addr;
318 +};
320 +#define RPMSG_LOCALHOST ((__u32) ~0UL)
322 +#ifdef __KERNEL__
324 +#include <net/sock.h>
325 +#include <linux/rpmsg.h>
327 +struct rpmsg_socket {
328 +       struct sock sk;
329 +       struct rpmsg_channel *rpdev;
330 +       bool unregister_rpdev;
331 +};
333 +#endif /* __KERNEL__ */
334 +#endif /* __NET_RPMSG_H */
335 diff --git a/net/Makefile b/net/Makefile
336 index 4f4ee08..764e6de 100644
337 --- a/net/Makefile
338 +++ b/net/Makefile
339 @@ -70,3 +70,4 @@ obj-$(CONFIG_CEPH_LIB)                += ceph/
340  obj-$(CONFIG_BATMAN_ADV)       += batman-adv/
341  obj-$(CONFIG_NFC)              += nfc/
342  obj-$(CONFIG_OPENVSWITCH)      += openvswitch/
343 +obj-$(CONFIG_RPMSG)            += rpmsg/
344 diff --git a/net/rpmsg/Makefile b/net/rpmsg/Makefile
345 new file mode 100644
346 index 0000000..4db19df
347 --- /dev/null
348 +++ b/net/rpmsg/Makefile
349 @@ -0,0 +1 @@
350 +obj-$(CONFIG_RPMSG)    += rpmsg_proto.o
351 diff --git a/net/rpmsg/rpmsg_proto.c b/net/rpmsg/rpmsg_proto.c
352 new file mode 100644
353 index 0000000..bb4b78a
354 --- /dev/null
355 +++ b/net/rpmsg/rpmsg_proto.c
356 @@ -0,0 +1,623 @@
357 +/*
358 + * AF_RPMSG: Remote processor messaging sockets
359 + *
360 + * Copyright (C) 2011 Texas Instruments, Inc.
361 + *
362 + * Ohad Ben-Cohen <ohad@wizery.com>
363 + *
364 + * This program is free software; you can redistribute it and/or
365 + * modify it under the terms of the GNU General Public License
366 + * version 2 as published by the Free Software Foundation.
367 + *
368 + * This program is distributed in the hope that it will be useful,
369 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
370 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
371 + * GNU General Public License for more details.
372 + */
374 +#define pr_fmt(fmt)    "%s: " fmt, __func__
376 +#include <linux/kernel.h>
377 +#include <linux/module.h>
378 +#include <linux/device.h>
379 +#include <linux/types.h>
380 +#include <linux/list.h>
381 +#include <linux/errno.h>
382 +#include <linux/skbuff.h>
383 +#include <linux/rwlock.h>
384 +#include <linux/err.h>
385 +#include <linux/mutex.h>
386 +#include <linux/rpmsg.h>
387 +#include <net/sock.h>
388 +#include <net/rpmsg.h>
389 +#include <linux/radix-tree.h>
391 +#define RPMSG_CB(skb)  (*(struct sockaddr_rpmsg *)&((skb)->cb))
393 +/*
394 + * Used to distinguish between bound and connected socket channels in the
395 + * radix tree index space.
396 + * Must match value as in drivers/rpmsg/virtio_rpsmg_bus.c:
397 + */
398 +#define RPMSG_RESERVED_ADDRESSES     (1024)
400 +/*
401 + * A two-level radix-tree-based scheme is used to maintain the rpmsg channels
402 + * we're exposing to userland. The first radix tree maps vproc index id
403 + * to its channels, and the second radix tree associates each channel
404 + * with its destination addresses (so sockaddr_rpmsg lookups are quick).
405 + *
406 + * Currently only channels with a valid dst address are supported (aka 'client'
407 + * channels as opposed to 'server' channels which usually only have a valid
408 + * src address).
409 + */
410 +static RADIX_TREE(rpmsg_channels, GFP_KERNEL);
412 +/*
413 + * Synchronization of access to the tree is achieved using a mutex,
414 + * because we're using non-atomic radix tree allocations.
415 + */
416 +static DEFINE_MUTEX(rpmsg_channels_lock);
418 +static struct proto rpmsg_proto = {
419 +       .name           = "RPMSG",
420 +       .owner          = THIS_MODULE,
421 +       .obj_size = sizeof(struct rpmsg_socket),
422 +};
424 +static int rpmsg_sock_connect(struct socket *sock, struct sockaddr *addr,
425 +                                                       int alen, int flags)
426 +{
427 +       struct sock *sk = sock->sk;
428 +       struct rpmsg_socket *rpsk;
429 +       struct sockaddr_rpmsg *sa;
430 +       int err = 0;
431 +       struct radix_tree_root *vrp_channels;
432 +       struct rpmsg_channel *rpdev;
434 +       pr_debug("sk %p\n", sk);
436 +       if (sk->sk_state != RPMSG_OPEN)
437 +               return -EBADFD;
439 +       if (sk->sk_type != SOCK_SEQPACKET)
440 +               return -EINVAL;
442 +       if (!addr || addr->sa_family != AF_RPMSG)
443 +               return -EINVAL;
445 +       if (alen < sizeof(*sa))
446 +               return -EINVAL;
448 +       sa = (struct sockaddr_rpmsg *) addr;
450 +       lock_sock(sk);
452 +       rpsk = container_of(sk, struct rpmsg_socket, sk);
454 +       mutex_lock(&rpmsg_channels_lock);
456 +       /* find the set of channels exposed by this remote processor */
457 +       vrp_channels = radix_tree_lookup(&rpmsg_channels, sa->vproc_id);
458 +       if (!vrp_channels) {
459 +               err = -EINVAL;
460 +               goto out;
461 +       }
463 +       /* find the specific channel we need to connect with, by dst addr:  */
464 +       rpdev = radix_tree_lookup(vrp_channels, sa->addr);
465 +       if (!rpdev) {
466 +               err = -EINVAL;
467 +               goto out;
468 +       }
470 +       rpsk->rpdev = rpdev;
472 +       /* bind this socket with its rpmsg endpoint */
473 +       rpdev->ept->priv = sk;
475 +       /* XXX take care of disconnection state too */
476 +       sk->sk_state = RPMSG_CONNECTED;
478 +out:
479 +       mutex_unlock(&rpmsg_channels_lock);
480 +       release_sock(sk);
481 +       return err;
482 +}
484 +static int rpmsg_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
485 +                                       struct msghdr *msg, size_t len)
486 +{
487 +       struct sock *sk = sock->sk;
488 +       struct rpmsg_socket *rpsk;
489 +       char payload[512];/* todo: sane payload length methodology */
490 +       int err;
492 +       pr_debug("sk %p len %d\n", sk, len);
494 +       /* XXX check for sock_error as well ? */
495 +       /* XXX handle noblock ? */
496 +       if (msg->msg_flags & MSG_OOB)
497 +               return -EOPNOTSUPP;
499 +       /* no payload ? */
500 +       if (msg->msg_iov->iov_base == NULL)
501 +               return -EINVAL;
503 +       lock_sock(sk);
505 +       /* we don't support loopback at this point */
506 +       if (sk->sk_state != RPMSG_CONNECTED) {
507 +               release_sock(sk);
508 +               return -ENOTCONN;
509 +       }
511 +       rpsk = container_of(sk, struct rpmsg_socket, sk);
513 +       /* XXX for now, ignore the peer address. later use it
514 +        * with rpmsg_sendto, but only if user is root */
516 +       err = memcpy_fromiovec(payload, msg->msg_iov, len);
517 +       if (err)
518 +               goto out;
520 +       /* XXX add length validation */
521 +       err = rpmsg_send(rpsk->rpdev, payload, len);
522 +       if (err)
523 +               pr_err("rpmsg_send failed: %d\n", err);
525 +out:
526 +       release_sock(sk);
527 +       return err;
528 +}
530 +static int rpmsg_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
531 +                               struct msghdr *msg, size_t len, int flags)
532 +{
533 +       struct sock *sk = sock->sk;
534 +       struct sockaddr_rpmsg *sa;
535 +       struct sk_buff *skb;
536 +       int noblock = flags & MSG_DONTWAIT;
537 +       int ret;
539 +       pr_debug("sk %p len %d\n", sk, len);
541 +       if (flags & MSG_OOB) {
542 +               pr_err("MSG_OOB: %d\n", EOPNOTSUPP);
543 +               return -EOPNOTSUPP;
544 +       }
546 +       msg->msg_namelen = 0;
548 +       skb = skb_recv_datagram(sk, flags, noblock, &ret);
549 +       if (!skb) {
550 +               /* check for shutdown ? */
551 +               pr_err("skb_recv_datagram: %d\n", ret);
552 +               return ret;
553 +       }
555 +       if (msg->msg_name) {
556 +               msg->msg_namelen = sizeof(*sa);
557 +               sa = (struct sockaddr_rpmsg *) msg->msg_name;
558 +               sa->vproc_id = RPMSG_CB(skb).vproc_id;
559 +               sa->addr = RPMSG_CB(skb).addr;
560 +               sa->family = AF_RPMSG;
561 +       }
563 +       if (len > skb->len) {
564 +               len = skb->len;
565 +       } else if (len < skb->len) {
566 +               pr_warn("user buffer is too small\n");
567 +               /* XXX truncate or error ? */
568 +               msg->msg_flags |= MSG_TRUNC;
569 +       }
571 +       ret = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, len);
572 +       if (ret) {
573 +               pr_err("error copying skb data: %d\n", ret);
574 +               goto out_free;
575 +       }
577 +       ret = len;
579 +out_free:
580 +       skb_free_datagram(sk, skb);
581 +       return ret;
582 +}
584 +static unsigned int rpmsg_sock_poll(struct file *file, struct socket *sock,
585 +                                                       poll_table *wait)
586 +{
587 +       struct sock *sk = sock->sk;
588 +       unsigned int mask = 0;
590 +       pr_debug("sk %p\n", sk);
592 +       poll_wait(file, sk_sleep(sk), wait);
594 +       /* exceptional events? */
595 +       if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue))
596 +               mask |= POLLERR;
597 +       if (sk->sk_shutdown & RCV_SHUTDOWN)
598 +               mask |= POLLRDHUP;
599 +       if (sk->sk_shutdown == SHUTDOWN_MASK)
600 +               mask |= POLLHUP;
602 +       /* readable? */
603 +       if (!skb_queue_empty(&sk->sk_receive_queue) ||
604 +           (sk->sk_shutdown & RCV_SHUTDOWN))
605 +               mask |= POLLIN | POLLRDNORM;
607 +       if (sk->sk_state == RPMSG_CLOSED)
608 +               mask |= POLLHUP;
610 +       /*
611 +        * XXX is writable ?
612 +        * this depends on the destination processor.
613 +        * if loopback: we're writable unless no memory
614 +        * if to remote: we need enabled rpmsg buffer or user supplied bufs
615 +        * for now, let's always be writable.
616 +        */
617 +       mask |= POLLOUT | POLLWRNORM | POLLWRBAND;
619 +       return mask;
620 +}
621 +EXPORT_SYMBOL(rpmsg_sock_poll);
623 +/*
624 + * return bound socket address information, either local or remote
625 + * note: len is just an output parameter, doesn't carry any input value
626 + */
627 +static int rpmsg_sock_getname(struct socket *sock, struct sockaddr *addr,
628 +                                                       int *len, int peer)
629 +{
630 +       struct sock *sk = sock->sk;
631 +       struct rpmsg_socket *rpsk;
632 +       struct rpmsg_channel *rpdev;
633 +       struct sockaddr_rpmsg *sa;
635 +       pr_debug("sk %p\n", sk);
637 +       rpsk = container_of(sk, struct rpmsg_socket, sk);
638 +       rpdev = rpsk->rpdev;
640 +       if (!rpdev)
641 +               return -ENOTCONN;
643 +       addr->sa_family = AF_RPMSG;
645 +       sa = (struct sockaddr_rpmsg *) addr;
647 +       *len = sizeof(*sa);
649 +       if (peer) {
650 +               sa->vproc_id = get_virtproc_id(rpdev->vrp);
651 +               sa->addr = rpdev->dst;
652 +       } else {
653 +               sa->vproc_id = RPMSG_LOCALHOST;
654 +               sa->addr = rpsk->rpdev->src;
655 +       }
657 +       return 0;
658 +}
660 +static int rpmsg_sock_release(struct socket *sock)
661 +{
662 +       struct sock *sk = sock->sk;
663 +       struct rpmsg_socket *rpsk = container_of(sk, struct rpmsg_socket, sk);
665 +       pr_debug("sk %p\n", sk);
667 +       if (!sk)
668 +               return 0;
670 +       if (rpsk->unregister_rpdev)
671 +               device_unregister(&rpsk->rpdev->dev);
673 +       sock_put(sock->sk);
675 +       return 0;
676 +}
678 +/*
679 + * Notes:
680 + * - calling connect after bind isn't currently supported (is it even needed ?).
681 + * - userspace arguments to bind aren't intuitive: one needs to provide
682 + *   the vproc id of the remote processor he wants the channel to be shared
683 + *   with, and the -local- address he wants the channel to be bind with
684 + */
685 +static int
686 +rpmsg_sock_bind(struct socket *sock, struct sockaddr *uaddr, int addr_len)
687 +{
688 +       struct sock *sk = sock->sk;
689 +       struct rpmsg_socket *rpsk = container_of(sk, struct rpmsg_socket, sk);
690 +       struct rpmsg_channel *rpdev;
691 +       struct sockaddr_rpmsg *sa = (struct sockaddr_rpmsg *)uaddr;
693 +       pr_debug("sk %p\n", sk);
695 +       if (sock->state == SS_CONNECTED)
696 +               return -EINVAL;
698 +       if (addr_len != sizeof(*sa))
699 +               return -EINVAL;
701 +       if (sa->family != AF_RPMSG)
702 +               return -EINVAL;
704 +       if (rpsk->rpdev)
705 +               return -EBUSY;
707 +       if (sk->sk_state != RPMSG_OPEN)
708 +               return -EINVAL;
710 +       rpdev = rpmsg_create_channel(sa->vproc_id, "rpmsg-proto", sa->addr,
711 +                                                       RPMSG_ADDR_ANY);
712 +       if (!rpdev)
713 +               return -EINVAL;
715 +       rpsk->rpdev = rpdev;
716 +       rpsk->unregister_rpdev = true;
718 +       /* bind this socket with its rpmsg endpoint */
719 +       rpdev->ept->priv = sk;
721 +       sk->sk_state = RPMSG_LISTENING;
723 +       return 0;
724 +}
726 +static const struct proto_ops rpmsg_sock_ops = {
727 +       .family         = PF_RPMSG,
728 +       .owner          = THIS_MODULE,
730 +       .release        = rpmsg_sock_release,
731 +       .connect        = rpmsg_sock_connect,
732 +       .getname        = rpmsg_sock_getname,
733 +       .sendmsg        = rpmsg_sock_sendmsg,
734 +       .recvmsg        = rpmsg_sock_recvmsg,
735 +       .poll           = rpmsg_sock_poll,
736 +       .bind           = rpmsg_sock_bind,
738 +       .listen         = sock_no_listen,
739 +       .accept         = sock_no_accept,
740 +       .ioctl          = sock_no_ioctl,
741 +       .mmap           = sock_no_mmap,
742 +       .socketpair     = sock_no_socketpair,
743 +       .shutdown       = sock_no_shutdown,
744 +       .setsockopt     = sock_no_setsockopt,
745 +       .getsockopt     = sock_no_getsockopt
746 +};
748 +static void rpmsg_sock_destruct(struct sock *sk)
749 +{
750 +}
752 +static int rpmsg_sock_create(struct net *net, struct socket *sock, int proto,
753 +                               int kern)
754 +{
755 +       struct sock *sk;
757 +       if (sock->type != SOCK_SEQPACKET)
758 +               return -ESOCKTNOSUPPORT;
759 +       if (proto != 0)
760 +               return -EPROTONOSUPPORT;
762 +       sk = sk_alloc(net, PF_RPMSG, GFP_KERNEL, &rpmsg_proto);
763 +       if (!sk)
764 +               return -ENOMEM;
766 +       pr_debug("sk %p\n", sk);
768 +       sock->state = SS_UNCONNECTED;
769 +       sock->ops = &rpmsg_sock_ops;
770 +       sock_init_data(sock, sk);
772 +       sk->sk_destruct = rpmsg_sock_destruct;
773 +       sk->sk_protocol = proto;
775 +       sk->sk_state = RPMSG_OPEN;
777 +       return 0;
778 +}
780 +static const struct net_proto_family rpmsg_proto_family = {
781 +       .family = PF_RPMSG,
782 +       .create = rpmsg_sock_create,
783 +       .owner = THIS_MODULE,
784 +};
786 +static void __rpmsg_proto_cb(struct device *dev, int from_vproc_id, void *data,
787 +                                       int len, struct sock *sk, u32 src)
788 +{
789 +       struct rpmsg_socket *rpsk = container_of(sk, struct rpmsg_socket, sk);
790 +       struct sk_buff *skb;
791 +       int ret;
793 +#ifdef DEBUG_VERBOSE /* Quiet this to improve rpmsg benchmarks: */
794 +       print_hex_dump(KERN_DEBUG, __func__, DUMP_PREFIX_NONE, 16, 1,
795 +                      data, len,  true);
796 +#endif
798 +       lock_sock(sk);
800 +       switch (sk->sk_state) {
801 +       case RPMSG_CONNECTED:
802 +               if (rpsk->rpdev->dst != src)
803 +                       dev_warn(dev, "unexpected source address: %d\n", src);
804 +               break;
805 +       case RPMSG_LISTENING:
806 +               /* When an inbound message is received while we're listening,
807 +                * we implicitly become connected */
808 +               sk->sk_state = RPMSG_CONNECTED;
809 +               rpsk->rpdev->dst = src;
810 +               break;
811 +       default:
812 +               dev_warn(dev, "unexpected inbound message (from %d)\n", src);
813 +               break;
814 +       }
816 +       skb = sock_alloc_send_skb(sk, len, 1, &ret);
817 +       if (!skb) {
818 +               dev_err(dev, "sock_alloc_send_skb failed: %d\n", ret);
819 +               ret = -ENOMEM;
820 +               goto out;
821 +       }
823 +       RPMSG_CB(skb).vproc_id = from_vproc_id;
824 +       RPMSG_CB(skb).addr = src;
825 +       RPMSG_CB(skb).family = AF_RPMSG;
827 +       memcpy(skb_put(skb, len), data, len);
829 +       ret = sock_queue_rcv_skb(sk, skb);
830 +       if (ret) {
831 +               dev_err(dev, "sock_queue_rcv_skb failed: %d\n", ret);
832 +               kfree_skb(skb);
833 +       }
835 +out:
836 +       release_sock(sk);
837 +}
839 +static void rpmsg_proto_cb(struct rpmsg_channel *rpdev, void *data, int len,
840 +                                               void *priv, u32 src)
841 +{
842 +       int id = get_virtproc_id(rpdev->vrp);
844 +       __rpmsg_proto_cb(&rpdev->dev, id, data, len, priv, src);
845 +}
847 +/* every channel we're probed with is exposed to userland via the Socket API */
848 +static int rpmsg_proto_probe(struct rpmsg_channel *rpdev)
849 +{
850 +       struct device *dev = &rpdev->dev;
851 +       int ret, dst = rpdev->dst, src = rpdev->src, id;
852 +       struct radix_tree_root *vrp_channels;
854 +       if (dst == RPMSG_ADDR_ANY)
855 +               return 0;
857 +       id = get_virtproc_id(rpdev->vrp);
859 +       mutex_lock(&rpmsg_channels_lock);
861 +       /* are we exposing channels for this remote processor yet ? */
862 +       vrp_channels = radix_tree_lookup(&rpmsg_channels, id);
863 +       /* not yet ? let's prepare the 2nd radix tree level then */
864 +       if (!vrp_channels) {
865 +               vrp_channels = kzalloc(sizeof(*vrp_channels), GFP_KERNEL);
866 +               INIT_RADIX_TREE(vrp_channels, GFP_KERNEL);
867 +               /* now let's associate the new channel with its vrp */
868 +               ret = radix_tree_insert(&rpmsg_channels, id, vrp_channels);
869 +               if (ret) {
870 +                       dev_err(dev, "radix_tree_insert failed: %d\n", ret);
871 +                       kfree(vrp_channels);
872 +                       return ret;
873 +               }
874 +       }
876 +       /* let's associate the new channel with its dst */
877 +       dev_info(dev, "inserting rpmsg src: %d, dst: %d\n", src, dst);
878 +       ret = radix_tree_insert(vrp_channels, dst, rpdev);
879 +       if (ret)
880 +               dev_err(dev, "failed to add rpmsg addr %d: %d\n", dst, ret);
882 +       mutex_unlock(&rpmsg_channels_lock);
884 +       return ret;
885 +}
887 +static void rpmsg_proto_remove(struct rpmsg_channel *rpdev)
888 +{
889 +       struct device *dev = &rpdev->dev;
890 +       int id, dst = rpdev->dst, src = rpdev->src;
891 +       struct radix_tree_root *vrp_channels;
893 +       if (dst == RPMSG_ADDR_ANY)
894 +               return;
896 +       id = get_virtproc_id(rpdev->vrp);
898 +       mutex_lock(&rpmsg_channels_lock);
900 +       vrp_channels = radix_tree_lookup(&rpmsg_channels, id);
901 +       if (!vrp_channels) {
902 +               dev_err(dev, "can't find channels for this vrp: %d\n", id);
903 +               goto out;
904 +       }
906 +       /* Only remove non-reserved channels from the tree, as only these
907 +        * were "probed".  Note: bind is not causing a probe, and bound
908 +        * sockets have src addresses < RPMSG_RESERVED_ADDRESSES.
909 +        */
910 +       if (src >= RPMSG_RESERVED_ADDRESSES) {
911 +               dev_info(dev, "deleting rpmsg src: %d, dst: %d\n", src, dst);
912 +               if (!radix_tree_delete(vrp_channels, dst))
913 +                       dev_err(dev, "failed to delete rpmsg %d\n", dst);
914 +       }
916 +out:
917 +       mutex_unlock(&rpmsg_channels_lock);
918 +}
920 +static struct rpmsg_device_id rpmsg_proto_id_table[] = {
921 +       { .name = "rpmsg-proto" },
922 +       { },
923 +};
924 +MODULE_DEVICE_TABLE(rpmsg, rpmsg_proto_id_table);
926 +static struct rpmsg_driver rpmsg_proto_driver = {
927 +       .drv.name       = KBUILD_MODNAME,
928 +       .drv.owner      = THIS_MODULE,
929 +       .id_table       = rpmsg_proto_id_table,
930 +       .probe          = rpmsg_proto_probe,
931 +       .callback       = rpmsg_proto_cb,
932 +       .remove         = rpmsg_proto_remove,
933 +};
935 +static int __init rpmsg_proto_init(void)
936 +{
937 +       int ret;
939 +       ret = proto_register(&rpmsg_proto, 0);
940 +       if (ret) {
941 +               pr_err("proto_register failed: %d\n", ret);
942 +               return ret;
943 +       }
945 +       ret = sock_register(&rpmsg_proto_family);
946 +       if (ret) {
947 +               pr_err("sock_register failed: %d\n", ret);
948 +               goto proto_unreg;
949 +       }
951 +       /* gimme rpmsg channels to expose ! */
952 +       ret = register_rpmsg_driver(&rpmsg_proto_driver);
953 +       if (ret) {
954 +               pr_err("register_rpmsg_driver failed: %d\n", ret);
955 +               goto sock_unreg;
956 +       }
958 +       return 0;
960 +sock_unreg:
961 +       sock_unregister(PF_RPMSG);
962 +proto_unreg:
963 +       proto_unregister(&rpmsg_proto);
964 +       return ret;
965 +}
967 +static void __exit rpmsg_proto_exit(void)
968 +{
969 +       unregister_rpmsg_driver(&rpmsg_proto_driver);
970 +       sock_unregister(PF_RPMSG);
971 +       proto_unregister(&rpmsg_proto);
972 +}
974 +module_init(rpmsg_proto_init);
975 +module_exit(rpmsg_proto_exit);
977 +MODULE_DESCRIPTION("Remote processor messaging protocol");
978 +MODULE_LICENSE("GPL v2");
979 +MODULE_ALIAS_NETPROTO(AF_RPMSG);
980 -- 
981 1.7.9.4