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