1
2
3
4
5
6
7
8
9
10
11
12#define pr_fmt(fmt) "%s: " fmt, __func__
13
14#include <linux/dma-mapping.h>
15#include <linux/idr.h>
16#include <linux/jiffies.h>
17#include <linux/kernel.h>
18#include <linux/module.h>
19#include <linux/mutex.h>
20#include <linux/of_device.h>
21#include <linux/rpmsg.h>
22#include <linux/rpmsg/byteorder.h>
23#include <linux/rpmsg/ns.h>
24#include <linux/scatterlist.h>
25#include <linux/slab.h>
26#include <linux/sched.h>
27#include <linux/virtio.h>
28#include <linux/virtio_ids.h>
29#include <linux/virtio_config.h>
30#include <linux/wait.h>
31
32#include "rpmsg_internal.h"
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57struct virtproc_info {
58 struct virtio_device *vdev;
59 struct virtqueue *rvq, *svq;
60 void *rbufs, *sbufs;
61 unsigned int num_bufs;
62 unsigned int buf_size;
63 int last_sbuf;
64 dma_addr_t bufs_dma;
65 struct mutex tx_lock;
66 struct idr endpoints;
67 struct mutex endpoints_lock;
68 wait_queue_head_t sendq;
69 atomic_t sleepers;
70};
71
72
73#define VIRTIO_RPMSG_F_NS 0
74
75
76
77
78
79
80
81
82
83
84
85
86struct rpmsg_hdr {
87 __rpmsg32 src;
88 __rpmsg32 dst;
89 __rpmsg32 reserved;
90 __rpmsg16 len;
91 __rpmsg16 flags;
92 u8 data[];
93} __packed;
94
95
96
97
98
99
100
101
102
103
104struct virtio_rpmsg_channel {
105 struct rpmsg_device rpdev;
106
107 struct virtproc_info *vrp;
108};
109
110#define to_virtio_rpmsg_channel(_rpdev) \
111 container_of(_rpdev, struct virtio_rpmsg_channel, rpdev)
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131#define MAX_RPMSG_NUM_BUFS (512)
132#define MAX_RPMSG_BUF_SIZE (512)
133
134
135
136
137
138
139#define RPMSG_RESERVED_ADDRESSES (1024)
140
141static void virtio_rpmsg_destroy_ept(struct rpmsg_endpoint *ept);
142static int virtio_rpmsg_send(struct rpmsg_endpoint *ept, void *data, int len);
143static int virtio_rpmsg_sendto(struct rpmsg_endpoint *ept, void *data, int len,
144 u32 dst);
145static int virtio_rpmsg_send_offchannel(struct rpmsg_endpoint *ept, u32 src,
146 u32 dst, void *data, int len);
147static int virtio_rpmsg_trysend(struct rpmsg_endpoint *ept, void *data, int len);
148static int virtio_rpmsg_trysendto(struct rpmsg_endpoint *ept, void *data,
149 int len, u32 dst);
150static int virtio_rpmsg_trysend_offchannel(struct rpmsg_endpoint *ept, u32 src,
151 u32 dst, void *data, int len);
152static struct rpmsg_device *__rpmsg_create_channel(struct virtproc_info *vrp,
153 struct rpmsg_channel_info *chinfo);
154
155static const struct rpmsg_endpoint_ops virtio_endpoint_ops = {
156 .destroy_ept = virtio_rpmsg_destroy_ept,
157 .send = virtio_rpmsg_send,
158 .sendto = virtio_rpmsg_sendto,
159 .send_offchannel = virtio_rpmsg_send_offchannel,
160 .trysend = virtio_rpmsg_trysend,
161 .trysendto = virtio_rpmsg_trysendto,
162 .trysend_offchannel = virtio_rpmsg_trysend_offchannel,
163};
164
165
166
167
168
169
170
171
172
173
174static void
175rpmsg_sg_init(struct scatterlist *sg, void *cpu_addr, unsigned int len)
176{
177 if (is_vmalloc_addr(cpu_addr)) {
178 sg_init_table(sg, 1);
179 sg_set_page(sg, vmalloc_to_page(cpu_addr), len,
180 offset_in_page(cpu_addr));
181 } else {
182 WARN_ON(!virt_addr_valid(cpu_addr));
183 sg_init_one(sg, cpu_addr, len);
184 }
185}
186
187
188
189
190
191
192
193
194
195
196static void __ept_release(struct kref *kref)
197{
198 struct rpmsg_endpoint *ept = container_of(kref, struct rpmsg_endpoint,
199 refcount);
200
201
202
203
204 kfree(ept);
205}
206
207
208static struct rpmsg_endpoint *__rpmsg_create_ept(struct virtproc_info *vrp,
209 struct rpmsg_device *rpdev,
210 rpmsg_rx_cb_t cb,
211 void *priv, u32 addr)
212{
213 int id_min, id_max, id;
214 struct rpmsg_endpoint *ept;
215 struct device *dev = rpdev ? &rpdev->dev : &vrp->vdev->dev;
216
217 ept = kzalloc(sizeof(*ept), GFP_KERNEL);
218 if (!ept)
219 return NULL;
220
221 kref_init(&ept->refcount);
222 mutex_init(&ept->cb_lock);
223
224 ept->rpdev = rpdev;
225 ept->cb = cb;
226 ept->priv = priv;
227 ept->ops = &virtio_endpoint_ops;
228
229
230 if (addr == RPMSG_ADDR_ANY) {
231 id_min = RPMSG_RESERVED_ADDRESSES;
232 id_max = 0;
233 } else {
234 id_min = addr;
235 id_max = addr + 1;
236 }
237
238 mutex_lock(&vrp->endpoints_lock);
239
240
241 id = idr_alloc(&vrp->endpoints, ept, id_min, id_max, GFP_KERNEL);
242 if (id < 0) {
243 dev_err(dev, "idr_alloc failed: %d\n", id);
244 goto free_ept;
245 }
246 ept->addr = id;
247
248 mutex_unlock(&vrp->endpoints_lock);
249
250 return ept;
251
252free_ept:
253 mutex_unlock(&vrp->endpoints_lock);
254 kref_put(&ept->refcount, __ept_release);
255 return NULL;
256}
257
258static struct rpmsg_device *virtio_rpmsg_create_channel(struct rpmsg_device *rpdev,
259 struct rpmsg_channel_info *chinfo)
260{
261 struct virtio_rpmsg_channel *vch = to_virtio_rpmsg_channel(rpdev);
262 struct virtproc_info *vrp = vch->vrp;
263
264 return __rpmsg_create_channel(vrp, chinfo);
265}
266
267static int virtio_rpmsg_release_channel(struct rpmsg_device *rpdev,
268 struct rpmsg_channel_info *chinfo)
269{
270 struct virtio_rpmsg_channel *vch = to_virtio_rpmsg_channel(rpdev);
271 struct virtproc_info *vrp = vch->vrp;
272
273 return rpmsg_unregister_device(&vrp->vdev->dev, chinfo);
274}
275
276static struct rpmsg_endpoint *virtio_rpmsg_create_ept(struct rpmsg_device *rpdev,
277 rpmsg_rx_cb_t cb,
278 void *priv,
279 struct rpmsg_channel_info chinfo)
280{
281 struct virtio_rpmsg_channel *vch = to_virtio_rpmsg_channel(rpdev);
282
283 return __rpmsg_create_ept(vch->vrp, rpdev, cb, priv, chinfo.src);
284}
285
286
287
288
289
290
291
292
293
294
295
296static void
297__rpmsg_destroy_ept(struct virtproc_info *vrp, struct rpmsg_endpoint *ept)
298{
299
300 mutex_lock(&vrp->endpoints_lock);
301 idr_remove(&vrp->endpoints, ept->addr);
302 mutex_unlock(&vrp->endpoints_lock);
303
304
305 mutex_lock(&ept->cb_lock);
306 ept->cb = NULL;
307 mutex_unlock(&ept->cb_lock);
308
309 kref_put(&ept->refcount, __ept_release);
310}
311
312static void virtio_rpmsg_destroy_ept(struct rpmsg_endpoint *ept)
313{
314 struct virtio_rpmsg_channel *vch = to_virtio_rpmsg_channel(ept->rpdev);
315
316 __rpmsg_destroy_ept(vch->vrp, ept);
317}
318
319static int virtio_rpmsg_announce_create(struct rpmsg_device *rpdev)
320{
321 struct virtio_rpmsg_channel *vch = to_virtio_rpmsg_channel(rpdev);
322 struct virtproc_info *vrp = vch->vrp;
323 struct device *dev = &rpdev->dev;
324 int err = 0;
325
326
327 if (rpdev->announce && rpdev->ept &&
328 virtio_has_feature(vrp->vdev, VIRTIO_RPMSG_F_NS)) {
329 struct rpmsg_ns_msg nsm;
330
331 strncpy(nsm.name, rpdev->id.name, RPMSG_NAME_SIZE);
332 nsm.addr = cpu_to_rpmsg32(rpdev, rpdev->ept->addr);
333 nsm.flags = cpu_to_rpmsg32(rpdev, RPMSG_NS_CREATE);
334
335 err = rpmsg_sendto(rpdev->ept, &nsm, sizeof(nsm), RPMSG_NS_ADDR);
336 if (err)
337 dev_err(dev, "failed to announce service %d\n", err);
338 }
339
340 return err;
341}
342
343static int virtio_rpmsg_announce_destroy(struct rpmsg_device *rpdev)
344{
345 struct virtio_rpmsg_channel *vch = to_virtio_rpmsg_channel(rpdev);
346 struct virtproc_info *vrp = vch->vrp;
347 struct device *dev = &rpdev->dev;
348 int err = 0;
349
350
351 if (rpdev->announce && rpdev->ept &&
352 virtio_has_feature(vrp->vdev, VIRTIO_RPMSG_F_NS)) {
353 struct rpmsg_ns_msg nsm;
354
355 strncpy(nsm.name, rpdev->id.name, RPMSG_NAME_SIZE);
356 nsm.addr = cpu_to_rpmsg32(rpdev, rpdev->ept->addr);
357 nsm.flags = cpu_to_rpmsg32(rpdev, RPMSG_NS_DESTROY);
358
359 err = rpmsg_sendto(rpdev->ept, &nsm, sizeof(nsm), RPMSG_NS_ADDR);
360 if (err)
361 dev_err(dev, "failed to announce service %d\n", err);
362 }
363
364 return err;
365}
366
367static const struct rpmsg_device_ops virtio_rpmsg_ops = {
368 .create_channel = virtio_rpmsg_create_channel,
369 .release_channel = virtio_rpmsg_release_channel,
370 .create_ept = virtio_rpmsg_create_ept,
371 .announce_create = virtio_rpmsg_announce_create,
372 .announce_destroy = virtio_rpmsg_announce_destroy,
373};
374
375static void virtio_rpmsg_release_device(struct device *dev)
376{
377 struct rpmsg_device *rpdev = to_rpmsg_device(dev);
378 struct virtio_rpmsg_channel *vch = to_virtio_rpmsg_channel(rpdev);
379
380 kfree(vch);
381}
382
383
384
385
386
387
388static struct rpmsg_device *__rpmsg_create_channel(struct virtproc_info *vrp,
389 struct rpmsg_channel_info *chinfo)
390{
391 struct virtio_rpmsg_channel *vch;
392 struct rpmsg_device *rpdev;
393 struct device *tmp, *dev = &vrp->vdev->dev;
394 int ret;
395
396
397 tmp = rpmsg_find_device(dev, chinfo);
398 if (tmp) {
399
400 put_device(tmp);
401 dev_err(dev, "channel %s:%x:%x already exist\n",
402 chinfo->name, chinfo->src, chinfo->dst);
403 return NULL;
404 }
405
406 vch = kzalloc(sizeof(*vch), GFP_KERNEL);
407 if (!vch)
408 return NULL;
409
410
411 vch->vrp = vrp;
412
413
414 rpdev = &vch->rpdev;
415 rpdev->src = chinfo->src;
416 rpdev->dst = chinfo->dst;
417 rpdev->ops = &virtio_rpmsg_ops;
418 rpdev->little_endian = virtio_is_little_endian(vrp->vdev);
419
420
421
422
423
424 rpdev->announce = rpdev->src != RPMSG_ADDR_ANY;
425
426 strncpy(rpdev->id.name, chinfo->name, RPMSG_NAME_SIZE);
427
428 rpdev->dev.parent = &vrp->vdev->dev;
429 rpdev->dev.release = virtio_rpmsg_release_device;
430 ret = rpmsg_register_device(rpdev);
431 if (ret)
432 return NULL;
433
434 return rpdev;
435}
436
437
438static void *get_a_tx_buf(struct virtproc_info *vrp)
439{
440 unsigned int len;
441 void *ret;
442
443
444 mutex_lock(&vrp->tx_lock);
445
446
447
448
449
450 if (vrp->last_sbuf < vrp->num_bufs / 2)
451 ret = vrp->sbufs + vrp->buf_size * vrp->last_sbuf++;
452
453 else
454 ret = virtqueue_get_buf(vrp->svq, &len);
455
456 mutex_unlock(&vrp->tx_lock);
457
458 return ret;
459}
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477static void rpmsg_upref_sleepers(struct virtproc_info *vrp)
478{
479
480 mutex_lock(&vrp->tx_lock);
481
482
483 if (atomic_inc_return(&vrp->sleepers) == 1)
484
485 virtqueue_enable_cb(vrp->svq);
486
487 mutex_unlock(&vrp->tx_lock);
488}
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504static void rpmsg_downref_sleepers(struct virtproc_info *vrp)
505{
506
507 mutex_lock(&vrp->tx_lock);
508
509
510 if (atomic_dec_and_test(&vrp->sleepers))
511
512 virtqueue_disable_cb(vrp->svq);
513
514 mutex_unlock(&vrp->tx_lock);
515}
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551static int rpmsg_send_offchannel_raw(struct rpmsg_device *rpdev,
552 u32 src, u32 dst,
553 void *data, int len, bool wait)
554{
555 struct virtio_rpmsg_channel *vch = to_virtio_rpmsg_channel(rpdev);
556 struct virtproc_info *vrp = vch->vrp;
557 struct device *dev = &rpdev->dev;
558 struct scatterlist sg;
559 struct rpmsg_hdr *msg;
560 int err;
561
562
563 if (src == RPMSG_ADDR_ANY || dst == RPMSG_ADDR_ANY) {
564 dev_err(dev, "invalid addr (src 0x%x, dst 0x%x)\n", src, dst);
565 return -EINVAL;
566 }
567
568
569
570
571
572
573
574
575
576
577 if (len > vrp->buf_size - sizeof(struct rpmsg_hdr)) {
578 dev_err(dev, "message is too big (%d)\n", len);
579 return -EMSGSIZE;
580 }
581
582
583 msg = get_a_tx_buf(vrp);
584 if (!msg && !wait)
585 return -ENOMEM;
586
587
588 while (!msg) {
589
590 rpmsg_upref_sleepers(vrp);
591
592
593
594
595
596
597
598 err = wait_event_interruptible_timeout(vrp->sendq,
599 (msg = get_a_tx_buf(vrp)),
600 msecs_to_jiffies(15000));
601
602
603 rpmsg_downref_sleepers(vrp);
604
605
606 if (!err) {
607 dev_err(dev, "timeout waiting for a tx buffer\n");
608 return -ERESTARTSYS;
609 }
610 }
611
612 msg->len = cpu_to_rpmsg16(rpdev, len);
613 msg->flags = 0;
614 msg->src = cpu_to_rpmsg32(rpdev, src);
615 msg->dst = cpu_to_rpmsg32(rpdev, dst);
616 msg->reserved = 0;
617 memcpy(msg->data, data, len);
618
619 dev_dbg(dev, "TX From 0x%x, To 0x%x, Len %d, Flags %d, Reserved %d\n",
620 src, dst, len, msg->flags, msg->reserved);
621#if defined(CONFIG_DYNAMIC_DEBUG)
622 dynamic_hex_dump("rpmsg_virtio TX: ", DUMP_PREFIX_NONE, 16, 1,
623 msg, sizeof(*msg) + len, true);
624#endif
625
626 rpmsg_sg_init(&sg, msg, sizeof(*msg) + len);
627
628 mutex_lock(&vrp->tx_lock);
629
630
631 err = virtqueue_add_outbuf(vrp->svq, &sg, 1, msg, GFP_KERNEL);
632 if (err) {
633
634
635
636
637
638 dev_err(dev, "virtqueue_add_outbuf failed: %d\n", err);
639 goto out;
640 }
641
642
643 virtqueue_kick(vrp->svq);
644out:
645 mutex_unlock(&vrp->tx_lock);
646 return err;
647}
648
649static int virtio_rpmsg_send(struct rpmsg_endpoint *ept, void *data, int len)
650{
651 struct rpmsg_device *rpdev = ept->rpdev;
652 u32 src = ept->addr, dst = rpdev->dst;
653
654 return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, true);
655}
656
657static int virtio_rpmsg_sendto(struct rpmsg_endpoint *ept, void *data, int len,
658 u32 dst)
659{
660 struct rpmsg_device *rpdev = ept->rpdev;
661 u32 src = ept->addr;
662
663 return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, true);
664}
665
666static int virtio_rpmsg_send_offchannel(struct rpmsg_endpoint *ept, u32 src,
667 u32 dst, void *data, int len)
668{
669 struct rpmsg_device *rpdev = ept->rpdev;
670
671 return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, true);
672}
673
674static int virtio_rpmsg_trysend(struct rpmsg_endpoint *ept, void *data, int len)
675{
676 struct rpmsg_device *rpdev = ept->rpdev;
677 u32 src = ept->addr, dst = rpdev->dst;
678
679 return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, false);
680}
681
682static int virtio_rpmsg_trysendto(struct rpmsg_endpoint *ept, void *data,
683 int len, u32 dst)
684{
685 struct rpmsg_device *rpdev = ept->rpdev;
686 u32 src = ept->addr;
687
688 return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, false);
689}
690
691static int virtio_rpmsg_trysend_offchannel(struct rpmsg_endpoint *ept, u32 src,
692 u32 dst, void *data, int len)
693{
694 struct rpmsg_device *rpdev = ept->rpdev;
695
696 return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, false);
697}
698
699static int rpmsg_recv_single(struct virtproc_info *vrp, struct device *dev,
700 struct rpmsg_hdr *msg, unsigned int len)
701{
702 struct rpmsg_endpoint *ept;
703 struct scatterlist sg;
704 bool little_endian = virtio_is_little_endian(vrp->vdev);
705 unsigned int msg_len = __rpmsg16_to_cpu(little_endian, msg->len);
706 int err;
707
708 dev_dbg(dev, "From: 0x%x, To: 0x%x, Len: %d, Flags: %d, Reserved: %d\n",
709 __rpmsg32_to_cpu(little_endian, msg->src),
710 __rpmsg32_to_cpu(little_endian, msg->dst), msg_len,
711 __rpmsg16_to_cpu(little_endian, msg->flags),
712 __rpmsg32_to_cpu(little_endian, msg->reserved));
713#if defined(CONFIG_DYNAMIC_DEBUG)
714 dynamic_hex_dump("rpmsg_virtio RX: ", DUMP_PREFIX_NONE, 16, 1,
715 msg, sizeof(*msg) + msg_len, true);
716#endif
717
718
719
720
721
722 if (len > vrp->buf_size ||
723 msg_len > (len - sizeof(struct rpmsg_hdr))) {
724 dev_warn(dev, "inbound msg too big: (%d, %d)\n", len, msg_len);
725 return -EINVAL;
726 }
727
728
729 mutex_lock(&vrp->endpoints_lock);
730
731 ept = idr_find(&vrp->endpoints, __rpmsg32_to_cpu(little_endian, msg->dst));
732
733
734 if (ept)
735 kref_get(&ept->refcount);
736
737 mutex_unlock(&vrp->endpoints_lock);
738
739 if (ept) {
740
741 mutex_lock(&ept->cb_lock);
742
743 if (ept->cb)
744 ept->cb(ept->rpdev, msg->data, msg_len, ept->priv,
745 __rpmsg32_to_cpu(little_endian, msg->src));
746
747 mutex_unlock(&ept->cb_lock);
748
749
750 kref_put(&ept->refcount, __ept_release);
751 } else
752 dev_warn(dev, "msg received with no recipient\n");
753
754
755 rpmsg_sg_init(&sg, msg, vrp->buf_size);
756
757
758 err = virtqueue_add_inbuf(vrp->rvq, &sg, 1, msg, GFP_KERNEL);
759 if (err < 0) {
760 dev_err(dev, "failed to add a virtqueue buffer: %d\n", err);
761 return err;
762 }
763
764 return 0;
765}
766
767
768static void rpmsg_recv_done(struct virtqueue *rvq)
769{
770 struct virtproc_info *vrp = rvq->vdev->priv;
771 struct device *dev = &rvq->vdev->dev;
772 struct rpmsg_hdr *msg;
773 unsigned int len, msgs_received = 0;
774 int err;
775
776 msg = virtqueue_get_buf(rvq, &len);
777 if (!msg) {
778 dev_err(dev, "uhm, incoming signal, but no used buffer ?\n");
779 return;
780 }
781
782 while (msg) {
783 err = rpmsg_recv_single(vrp, dev, msg, len);
784 if (err)
785 break;
786
787 msgs_received++;
788
789 msg = virtqueue_get_buf(rvq, &len);
790 }
791
792 dev_dbg(dev, "Received %u messages\n", msgs_received);
793
794
795 if (msgs_received)
796 virtqueue_kick(vrp->rvq);
797}
798
799
800
801
802
803
804
805
806static void rpmsg_xmit_done(struct virtqueue *svq)
807{
808 struct virtproc_info *vrp = svq->vdev->priv;
809
810 dev_dbg(&svq->vdev->dev, "%s\n", __func__);
811
812
813 wake_up_interruptible(&vrp->sendq);
814}
815
816
817
818
819
820
821static struct rpmsg_device *rpmsg_virtio_add_ctrl_dev(struct virtio_device *vdev)
822{
823 struct virtproc_info *vrp = vdev->priv;
824 struct virtio_rpmsg_channel *vch;
825 struct rpmsg_device *rpdev_ctrl;
826 int err = 0;
827
828 vch = kzalloc(sizeof(*vch), GFP_KERNEL);
829 if (!vch)
830 return ERR_PTR(-ENOMEM);
831
832
833 vch->vrp = vrp;
834
835
836 rpdev_ctrl = &vch->rpdev;
837 rpdev_ctrl->ops = &virtio_rpmsg_ops;
838
839 rpdev_ctrl->dev.parent = &vrp->vdev->dev;
840 rpdev_ctrl->dev.release = virtio_rpmsg_release_device;
841 rpdev_ctrl->little_endian = virtio_is_little_endian(vrp->vdev);
842
843 err = rpmsg_chrdev_register_device(rpdev_ctrl);
844 if (err) {
845 kfree(vch);
846 return ERR_PTR(err);
847 }
848
849 return rpdev_ctrl;
850}
851
852static void rpmsg_virtio_del_ctrl_dev(struct rpmsg_device *rpdev_ctrl)
853{
854 if (!rpdev_ctrl)
855 return;
856 kfree(to_virtio_rpmsg_channel(rpdev_ctrl));
857}
858
859static int rpmsg_probe(struct virtio_device *vdev)
860{
861 vq_callback_t *vq_cbs[] = { rpmsg_recv_done, rpmsg_xmit_done };
862 static const char * const names[] = { "input", "output" };
863 struct virtqueue *vqs[2];
864 struct virtproc_info *vrp;
865 struct virtio_rpmsg_channel *vch = NULL;
866 struct rpmsg_device *rpdev_ns, *rpdev_ctrl;
867 void *bufs_va;
868 int err = 0, i;
869 size_t total_buf_space;
870 bool notify;
871
872 vrp = kzalloc(sizeof(*vrp), GFP_KERNEL);
873 if (!vrp)
874 return -ENOMEM;
875
876 vrp->vdev = vdev;
877
878 idr_init(&vrp->endpoints);
879 mutex_init(&vrp->endpoints_lock);
880 mutex_init(&vrp->tx_lock);
881 init_waitqueue_head(&vrp->sendq);
882
883
884 err = virtio_find_vqs(vdev, 2, vqs, vq_cbs, names, NULL);
885 if (err)
886 goto free_vrp;
887
888 vrp->rvq = vqs[0];
889 vrp->svq = vqs[1];
890
891
892 WARN_ON(virtqueue_get_vring_size(vrp->rvq) !=
893 virtqueue_get_vring_size(vrp->svq));
894
895
896 if (virtqueue_get_vring_size(vrp->rvq) < MAX_RPMSG_NUM_BUFS / 2)
897 vrp->num_bufs = virtqueue_get_vring_size(vrp->rvq) * 2;
898 else
899 vrp->num_bufs = MAX_RPMSG_NUM_BUFS;
900
901 vrp->buf_size = MAX_RPMSG_BUF_SIZE;
902
903 total_buf_space = vrp->num_bufs * vrp->buf_size;
904
905
906 bufs_va = dma_alloc_coherent(vdev->dev.parent,
907 total_buf_space, &vrp->bufs_dma,
908 GFP_KERNEL);
909 if (!bufs_va) {
910 err = -ENOMEM;
911 goto vqs_del;
912 }
913
914 dev_dbg(&vdev->dev, "buffers: va %pK, dma %pad\n",
915 bufs_va, &vrp->bufs_dma);
916
917
918 vrp->rbufs = bufs_va;
919
920
921 vrp->sbufs = bufs_va + total_buf_space / 2;
922
923
924 for (i = 0; i < vrp->num_bufs / 2; i++) {
925 struct scatterlist sg;
926 void *cpu_addr = vrp->rbufs + i * vrp->buf_size;
927
928 rpmsg_sg_init(&sg, cpu_addr, vrp->buf_size);
929
930 err = virtqueue_add_inbuf(vrp->rvq, &sg, 1, cpu_addr,
931 GFP_KERNEL);
932 WARN_ON(err);
933 }
934
935
936 virtqueue_disable_cb(vrp->svq);
937
938 vdev->priv = vrp;
939
940 rpdev_ctrl = rpmsg_virtio_add_ctrl_dev(vdev);
941 if (IS_ERR(rpdev_ctrl)) {
942 err = PTR_ERR(rpdev_ctrl);
943 goto free_coherent;
944 }
945
946
947 if (virtio_has_feature(vdev, VIRTIO_RPMSG_F_NS)) {
948 vch = kzalloc(sizeof(*vch), GFP_KERNEL);
949 if (!vch) {
950 err = -ENOMEM;
951 goto free_ctrldev;
952 }
953
954
955 vch->vrp = vrp;
956
957
958 rpdev_ns = &vch->rpdev;
959 rpdev_ns->ops = &virtio_rpmsg_ops;
960 rpdev_ns->little_endian = virtio_is_little_endian(vrp->vdev);
961
962 rpdev_ns->dev.parent = &vrp->vdev->dev;
963 rpdev_ns->dev.release = virtio_rpmsg_release_device;
964
965 err = rpmsg_ns_register_device(rpdev_ns);
966 if (err)
967 goto free_vch;
968 }
969
970
971
972
973
974 notify = virtqueue_kick_prepare(vrp->rvq);
975
976
977 virtio_device_ready(vdev);
978
979
980
981
982
983
984 if (notify)
985 virtqueue_notify(vrp->rvq);
986
987 dev_info(&vdev->dev, "rpmsg host is online\n");
988
989 return 0;
990
991free_vch:
992 kfree(vch);
993free_ctrldev:
994 rpmsg_virtio_del_ctrl_dev(rpdev_ctrl);
995free_coherent:
996 dma_free_coherent(vdev->dev.parent, total_buf_space,
997 bufs_va, vrp->bufs_dma);
998vqs_del:
999 vdev->config->del_vqs(vrp->vdev);
1000free_vrp:
1001 kfree(vrp);
1002 return err;
1003}
1004
1005static int rpmsg_remove_device(struct device *dev, void *data)
1006{
1007 device_unregister(dev);
1008
1009 return 0;
1010}
1011
1012static void rpmsg_remove(struct virtio_device *vdev)
1013{
1014 struct virtproc_info *vrp = vdev->priv;
1015 size_t total_buf_space = vrp->num_bufs * vrp->buf_size;
1016 int ret;
1017
1018 vdev->config->reset(vdev);
1019
1020 ret = device_for_each_child(&vdev->dev, NULL, rpmsg_remove_device);
1021 if (ret)
1022 dev_warn(&vdev->dev, "can't remove rpmsg device: %d\n", ret);
1023
1024 idr_destroy(&vrp->endpoints);
1025
1026 vdev->config->del_vqs(vrp->vdev);
1027
1028 dma_free_coherent(vdev->dev.parent, total_buf_space,
1029 vrp->rbufs, vrp->bufs_dma);
1030
1031 kfree(vrp);
1032}
1033
1034static struct virtio_device_id id_table[] = {
1035 { VIRTIO_ID_RPMSG, VIRTIO_DEV_ANY_ID },
1036 { 0 },
1037};
1038
1039static unsigned int features[] = {
1040 VIRTIO_RPMSG_F_NS,
1041};
1042
1043static struct virtio_driver virtio_ipc_driver = {
1044 .feature_table = features,
1045 .feature_table_size = ARRAY_SIZE(features),
1046 .driver.name = KBUILD_MODNAME,
1047 .driver.owner = THIS_MODULE,
1048 .id_table = id_table,
1049 .probe = rpmsg_probe,
1050 .remove = rpmsg_remove,
1051};
1052
1053static int __init rpmsg_init(void)
1054{
1055 int ret;
1056
1057 ret = register_virtio_driver(&virtio_ipc_driver);
1058 if (ret)
1059 pr_err("failed to register virtio driver: %d\n", ret);
1060
1061 return ret;
1062}
1063subsys_initcall(rpmsg_init);
1064
1065static void __exit rpmsg_fini(void)
1066{
1067 unregister_virtio_driver(&virtio_ipc_driver);
1068}
1069module_exit(rpmsg_fini);
1070
1071MODULE_DEVICE_TABLE(virtio, id_table);
1072MODULE_DESCRIPTION("Virtio-based remote processor messaging bus");
1073MODULE_LICENSE("GPL v2");
1074