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 };
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");
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
91 sg_init_one(&sg, msg, sizeof(*msg) + len);
93 @@ -891,6 +911,25 @@ static void rpmsg_xmit_done(struct virtqueue *svq)
94 wake_up_interruptible(&vrp->sendq);
95 }
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;
102 +
103 + strncpy(chinfo.name, name, sizeof(chinfo.name));
104 + chinfo.src = src;
105 + chinfo.dst = dst;
106 +
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;
111 +
112 + return __rpmsg_create_channel(vrp, &chinfo);
113 +}
114 +EXPORT_SYMBOL(rpmsg_create_channel);
115 +
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;
123 +#ifdef DEBUG_VERBOSE
124 print_hex_dump(KERN_DEBUG, "NS announcement: ",
125 DUMP_PREFIX_NONE, 16, 1,
126 data, len, true);
127 -
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;
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 }
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;
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);
163 + if (!idr_pre_get(&vprocs, GFP_KERNEL))
164 + goto free_vrp;
165 +
166 + mutex_lock(&vprocs_mutex);
167 +
168 + err = idr_get_new(&vprocs, vrp, &vproc_id);
169 +
170 + mutex_unlock(&vprocs_mutex);
171 +
172 + if (err) {
173 + dev_err(&vdev->dev, "idr_get_new failed: %d\n", err);
174 + goto free_vrp;
175 + }
176 +
177 + vrp->id = vproc_id;
178 +
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;
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);
202 + mutex_lock(&vprocs_mutex);
203 + idr_remove(&vprocs, vrp->id);
204 + mutex_unlock(&vprocs_mutex);
205 +
206 kfree(vrp);
207 }
209 @@ -1089,6 +1153,8 @@ static int __init rpmsg_init(void)
210 {
211 int ret;
213 + idr_init(&vprocs);
214 +
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);
222 +
223 + idr_remove_all(&vprocs);
224 + idr_destroy(&vprocs);
225 }
226 module_exit(rpmsg_fini);
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 }
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);
239 +
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.. */
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
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
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 + */
293 +
294 +#ifndef __NET_RPMSG_H
295 +#define __NET_RPMSG_H
296 +
297 +#include <linux/types.h>
298 +#include <linux/socket.h>
299 +
300 +/* user space needs this */
301 +#ifndef AF_RPMSG
302 +#define AF_RPMSG 40
303 +#define PF_RPMSG AF_RPMSG
304 +#endif
305 +
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 +};
313 +
314 +struct sockaddr_rpmsg {
315 + sa_family_t family;
316 + __u32 vproc_id;
317 + __u32 addr;
318 +};
319 +
320 +#define RPMSG_LOCALHOST ((__u32) ~0UL)
321 +
322 +#ifdef __KERNEL__
323 +
324 +#include <net/sock.h>
325 +#include <linux/rpmsg.h>
326 +
327 +struct rpmsg_socket {
328 + struct sock sk;
329 + struct rpmsg_channel *rpdev;
330 + bool unregister_rpdev;
331 +};
332 +
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 + */
373 +
374 +#define pr_fmt(fmt) "%s: " fmt, __func__
375 +
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>
390 +
391 +#define RPMSG_CB(skb) (*(struct sockaddr_rpmsg *)&((skb)->cb))
392 +
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)
399 +
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);
411 +
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);
417 +
418 +static struct proto rpmsg_proto = {
419 + .name = "RPMSG",
420 + .owner = THIS_MODULE,
421 + .obj_size = sizeof(struct rpmsg_socket),
422 +};
423 +
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;
433 +
434 + pr_debug("sk %p\n", sk);
435 +
436 + if (sk->sk_state != RPMSG_OPEN)
437 + return -EBADFD;
438 +
439 + if (sk->sk_type != SOCK_SEQPACKET)
440 + return -EINVAL;
441 +
442 + if (!addr || addr->sa_family != AF_RPMSG)
443 + return -EINVAL;
444 +
445 + if (alen < sizeof(*sa))
446 + return -EINVAL;
447 +
448 + sa = (struct sockaddr_rpmsg *) addr;
449 +
450 + lock_sock(sk);
451 +
452 + rpsk = container_of(sk, struct rpmsg_socket, sk);
453 +
454 + mutex_lock(&rpmsg_channels_lock);
455 +
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 + }
462 +
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 + }
469 +
470 + rpsk->rpdev = rpdev;
471 +
472 + /* bind this socket with its rpmsg endpoint */
473 + rpdev->ept->priv = sk;
474 +
475 + /* XXX take care of disconnection state too */
476 + sk->sk_state = RPMSG_CONNECTED;
477 +
478 +out:
479 + mutex_unlock(&rpmsg_channels_lock);
480 + release_sock(sk);
481 + return err;
482 +}
483 +
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;
491 +
492 + pr_debug("sk %p len %d\n", sk, len);
493 +
494 + /* XXX check for sock_error as well ? */
495 + /* XXX handle noblock ? */
496 + if (msg->msg_flags & MSG_OOB)
497 + return -EOPNOTSUPP;
498 +
499 + /* no payload ? */
500 + if (msg->msg_iov->iov_base == NULL)
501 + return -EINVAL;
502 +
503 + lock_sock(sk);
504 +
505 + /* we don't support loopback at this point */
506 + if (sk->sk_state != RPMSG_CONNECTED) {
507 + release_sock(sk);
508 + return -ENOTCONN;
509 + }
510 +
511 + rpsk = container_of(sk, struct rpmsg_socket, sk);
512 +
513 + /* XXX for now, ignore the peer address. later use it
514 + * with rpmsg_sendto, but only if user is root */
515 +
516 + err = memcpy_fromiovec(payload, msg->msg_iov, len);
517 + if (err)
518 + goto out;
519 +
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);
524 +
525 +out:
526 + release_sock(sk);
527 + return err;
528 +}
529 +
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;
538 +
539 + pr_debug("sk %p len %d\n", sk, len);
540 +
541 + if (flags & MSG_OOB) {
542 + pr_err("MSG_OOB: %d\n", EOPNOTSUPP);
543 + return -EOPNOTSUPP;
544 + }
545 +
546 + msg->msg_namelen = 0;
547 +
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 + }
554 +
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 + }
562 +
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 + }
570 +
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 + }
576 +
577 + ret = len;
578 +
579 +out_free:
580 + skb_free_datagram(sk, skb);
581 + return ret;
582 +}
583 +
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;
589 +
590 + pr_debug("sk %p\n", sk);
591 +
592 + poll_wait(file, sk_sleep(sk), wait);
593 +
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;
601 +
602 + /* readable? */
603 + if (!skb_queue_empty(&sk->sk_receive_queue) ||
604 + (sk->sk_shutdown & RCV_SHUTDOWN))
605 + mask |= POLLIN | POLLRDNORM;
606 +
607 + if (sk->sk_state == RPMSG_CLOSED)
608 + mask |= POLLHUP;
609 +
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;
618 +
619 + return mask;
620 +}
621 +EXPORT_SYMBOL(rpmsg_sock_poll);
622 +
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;
634 +
635 + pr_debug("sk %p\n", sk);
636 +
637 + rpsk = container_of(sk, struct rpmsg_socket, sk);
638 + rpdev = rpsk->rpdev;
639 +
640 + if (!rpdev)
641 + return -ENOTCONN;
642 +
643 + addr->sa_family = AF_RPMSG;
644 +
645 + sa = (struct sockaddr_rpmsg *) addr;
646 +
647 + *len = sizeof(*sa);
648 +
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 + }
656 +
657 + return 0;
658 +}
659 +
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);
664 +
665 + pr_debug("sk %p\n", sk);
666 +
667 + if (!sk)
668 + return 0;
669 +
670 + if (rpsk->unregister_rpdev)
671 + device_unregister(&rpsk->rpdev->dev);
672 +
673 + sock_put(sock->sk);
674 +
675 + return 0;
676 +}
677 +
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;
692 +
693 + pr_debug("sk %p\n", sk);
694 +
695 + if (sock->state == SS_CONNECTED)
696 + return -EINVAL;
697 +
698 + if (addr_len != sizeof(*sa))
699 + return -EINVAL;
700 +
701 + if (sa->family != AF_RPMSG)
702 + return -EINVAL;
703 +
704 + if (rpsk->rpdev)
705 + return -EBUSY;
706 +
707 + if (sk->sk_state != RPMSG_OPEN)
708 + return -EINVAL;
709 +
710 + rpdev = rpmsg_create_channel(sa->vproc_id, "rpmsg-proto", sa->addr,
711 + RPMSG_ADDR_ANY);
712 + if (!rpdev)
713 + return -EINVAL;
714 +
715 + rpsk->rpdev = rpdev;
716 + rpsk->unregister_rpdev = true;
717 +
718 + /* bind this socket with its rpmsg endpoint */
719 + rpdev->ept->priv = sk;
720 +
721 + sk->sk_state = RPMSG_LISTENING;
722 +
723 + return 0;
724 +}
725 +
726 +static const struct proto_ops rpmsg_sock_ops = {
727 + .family = PF_RPMSG,
728 + .owner = THIS_MODULE,
729 +
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,
737 +
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 +};
747 +
748 +static void rpmsg_sock_destruct(struct sock *sk)
749 +{
750 +}
751 +
752 +static int rpmsg_sock_create(struct net *net, struct socket *sock, int proto,
753 + int kern)
754 +{
755 + struct sock *sk;
756 +
757 + if (sock->type != SOCK_SEQPACKET)
758 + return -ESOCKTNOSUPPORT;
759 + if (proto != 0)
760 + return -EPROTONOSUPPORT;
761 +
762 + sk = sk_alloc(net, PF_RPMSG, GFP_KERNEL, &rpmsg_proto);
763 + if (!sk)
764 + return -ENOMEM;
765 +
766 + pr_debug("sk %p\n", sk);
767 +
768 + sock->state = SS_UNCONNECTED;
769 + sock->ops = &rpmsg_sock_ops;
770 + sock_init_data(sock, sk);
771 +
772 + sk->sk_destruct = rpmsg_sock_destruct;
773 + sk->sk_protocol = proto;
774 +
775 + sk->sk_state = RPMSG_OPEN;
776 +
777 + return 0;
778 +}
779 +
780 +static const struct net_proto_family rpmsg_proto_family = {
781 + .family = PF_RPMSG,
782 + .create = rpmsg_sock_create,
783 + .owner = THIS_MODULE,
784 +};
785 +
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;
792 +
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
797 +
798 + lock_sock(sk);
799 +
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 + }
815 +
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 + }
822 +
823 + RPMSG_CB(skb).vproc_id = from_vproc_id;
824 + RPMSG_CB(skb).addr = src;
825 + RPMSG_CB(skb).family = AF_RPMSG;
826 +
827 + memcpy(skb_put(skb, len), data, len);
828 +
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 + }
834 +
835 +out:
836 + release_sock(sk);
837 +}
838 +
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);
843 +
844 + __rpmsg_proto_cb(&rpdev->dev, id, data, len, priv, src);
845 +}
846 +
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;
853 +
854 + if (dst == RPMSG_ADDR_ANY)
855 + return 0;
856 +
857 + id = get_virtproc_id(rpdev->vrp);
858 +
859 + mutex_lock(&rpmsg_channels_lock);
860 +
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 + }
875 +
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);
881 +
882 + mutex_unlock(&rpmsg_channels_lock);
883 +
884 + return ret;
885 +}
886 +
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;
892 +
893 + if (dst == RPMSG_ADDR_ANY)
894 + return;
895 +
896 + id = get_virtproc_id(rpdev->vrp);
897 +
898 + mutex_lock(&rpmsg_channels_lock);
899 +
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 + }
905 +
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 + }
915 +
916 +out:
917 + mutex_unlock(&rpmsg_channels_lock);
918 +}
919 +
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);
925 +
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 +};
934 +
935 +static int __init rpmsg_proto_init(void)
936 +{
937 + int ret;
938 +
939 + ret = proto_register(&rpmsg_proto, 0);
940 + if (ret) {
941 + pr_err("proto_register failed: %d\n", ret);
942 + return ret;
943 + }
944 +
945 + ret = sock_register(&rpmsg_proto_family);
946 + if (ret) {
947 + pr_err("sock_register failed: %d\n", ret);
948 + goto proto_unreg;
949 + }
950 +
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 + }
957 +
958 + return 0;
959 +
960 +sock_unreg:
961 + sock_unregister(PF_RPMSG);
962 +proto_unreg:
963 + proto_unregister(&rpmsg_proto);
964 + return ret;
965 +}
966 +
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 +}
973 +
974 +module_init(rpmsg_proto_init);
975 +module_exit(rpmsg_proto_exit);
976 +
977 +MODULE_DESCRIPTION("Remote processor messaging protocol");
978 +MODULE_LICENSE("GPL v2");
979 +MODULE_ALIAS_NETPROTO(AF_RPMSG);
980 --
981 1.7.9.4