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