1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20#include <linux/cdev.h>
21#include <linux/debugfs.h>
22#include <linux/completion.h>
23#include <linux/device.h>
24#include <linux/err.h>
25#include <linux/freezer.h>
26#include <linux/fs.h>
27#include <linux/splice.h>
28#include <linux/pagemap.h>
29#include <linux/init.h>
30#include <linux/list.h>
31#include <linux/poll.h>
32#include <linux/sched.h>
33#include <linux/slab.h>
34#include <linux/spinlock.h>
35#include <linux/virtio.h>
36#include <linux/virtio_console.h>
37#include <linux/wait.h>
38#include <linux/workqueue.h>
39#include <linux/module.h>
40#include <linux/dma-mapping.h>
41#include "../tty/hvc/hvc_console.h"
42
43#define is_rproc_enabled IS_ENABLED(CONFIG_REMOTEPROC)
44
45
46
47
48
49
50
51
52
53struct ports_driver_data {
54
55 struct class *class;
56
57
58 struct dentry *debugfs_dir;
59
60
61 struct list_head portdevs;
62
63
64
65
66
67
68
69
70
71
72
73 unsigned int next_vtermno;
74
75
76 struct list_head consoles;
77};
78static struct ports_driver_data pdrvdata;
79
80static DEFINE_SPINLOCK(pdrvdata_lock);
81static DECLARE_COMPLETION(early_console_added);
82
83
84struct console {
85
86 struct list_head list;
87
88
89 struct hvc_struct *hvc;
90
91
92 struct winsize ws;
93
94
95
96
97
98
99
100
101 u32 vtermno;
102};
103
104struct port_buffer {
105 char *buf;
106
107
108 size_t size;
109
110
111 size_t len;
112
113 size_t offset;
114
115
116 dma_addr_t dma;
117
118
119 struct device *dev;
120
121
122 struct list_head list;
123
124
125 unsigned int sgpages;
126
127
128 struct scatterlist sg[0];
129};
130
131
132
133
134
135struct ports_device {
136
137 struct list_head list;
138
139
140
141
142
143 struct work_struct control_work;
144 struct work_struct config_work;
145
146 struct list_head ports;
147
148
149 spinlock_t ports_lock;
150
151
152 spinlock_t c_ivq_lock;
153 spinlock_t c_ovq_lock;
154
155
156 struct virtio_console_config config;
157
158
159 struct virtio_device *vdev;
160
161
162
163
164
165 struct virtqueue *c_ivq, *c_ovq;
166
167
168
169
170
171 struct virtio_console_control cpkt;
172
173
174 struct virtqueue **in_vqs, **out_vqs;
175
176
177 int chr_major;
178};
179
180struct port_stats {
181 unsigned long bytes_sent, bytes_received, bytes_discarded;
182};
183
184
185struct port {
186
187 struct list_head list;
188
189
190 struct ports_device *portdev;
191
192
193 struct port_buffer *inbuf;
194
195
196
197
198
199
200 spinlock_t inbuf_lock;
201
202
203 spinlock_t outvq_lock;
204
205
206 struct virtqueue *in_vq, *out_vq;
207
208
209 struct dentry *debugfs_file;
210
211
212
213
214
215
216 struct port_stats stats;
217
218
219
220
221
222 struct console cons;
223
224
225 struct cdev *cdev;
226 struct device *dev;
227
228
229 struct kref kref;
230
231
232 wait_queue_head_t waitqueue;
233
234
235 char *name;
236
237
238 struct fasync_struct *async_queue;
239
240
241 u32 id;
242
243 bool outvq_full;
244
245
246 bool host_connected;
247
248
249 bool guest_connected;
250};
251
252
253static int (*early_put_chars)(u32, const char *, int);
254
255static struct port *find_port_by_vtermno(u32 vtermno)
256{
257 struct port *port;
258 struct console *cons;
259 unsigned long flags;
260
261 spin_lock_irqsave(&pdrvdata_lock, flags);
262 list_for_each_entry(cons, &pdrvdata.consoles, list) {
263 if (cons->vtermno == vtermno) {
264 port = container_of(cons, struct port, cons);
265 goto out;
266 }
267 }
268 port = NULL;
269out:
270 spin_unlock_irqrestore(&pdrvdata_lock, flags);
271 return port;
272}
273
274static struct port *find_port_by_devt_in_portdev(struct ports_device *portdev,
275 dev_t dev)
276{
277 struct port *port;
278 unsigned long flags;
279
280 spin_lock_irqsave(&portdev->ports_lock, flags);
281 list_for_each_entry(port, &portdev->ports, list) {
282 if (port->cdev->dev == dev) {
283 kref_get(&port->kref);
284 goto out;
285 }
286 }
287 port = NULL;
288out:
289 spin_unlock_irqrestore(&portdev->ports_lock, flags);
290
291 return port;
292}
293
294static struct port *find_port_by_devt(dev_t dev)
295{
296 struct ports_device *portdev;
297 struct port *port;
298 unsigned long flags;
299
300 spin_lock_irqsave(&pdrvdata_lock, flags);
301 list_for_each_entry(portdev, &pdrvdata.portdevs, list) {
302 port = find_port_by_devt_in_portdev(portdev, dev);
303 if (port)
304 goto out;
305 }
306 port = NULL;
307out:
308 spin_unlock_irqrestore(&pdrvdata_lock, flags);
309 return port;
310}
311
312static struct port *find_port_by_id(struct ports_device *portdev, u32 id)
313{
314 struct port *port;
315 unsigned long flags;
316
317 spin_lock_irqsave(&portdev->ports_lock, flags);
318 list_for_each_entry(port, &portdev->ports, list)
319 if (port->id == id)
320 goto out;
321 port = NULL;
322out:
323 spin_unlock_irqrestore(&portdev->ports_lock, flags);
324
325 return port;
326}
327
328static struct port *find_port_by_vq(struct ports_device *portdev,
329 struct virtqueue *vq)
330{
331 struct port *port;
332 unsigned long flags;
333
334 spin_lock_irqsave(&portdev->ports_lock, flags);
335 list_for_each_entry(port, &portdev->ports, list)
336 if (port->in_vq == vq || port->out_vq == vq)
337 goto out;
338 port = NULL;
339out:
340 spin_unlock_irqrestore(&portdev->ports_lock, flags);
341 return port;
342}
343
344static bool is_console_port(struct port *port)
345{
346 if (port->cons.hvc)
347 return true;
348 return false;
349}
350
351static bool is_rproc_serial(const struct virtio_device *vdev)
352{
353 return is_rproc_enabled && vdev->id.device == VIRTIO_ID_RPROC_SERIAL;
354}
355
356static inline bool use_multiport(struct ports_device *portdev)
357{
358
359
360
361
362 if (!portdev->vdev)
363 return false;
364 return __virtio_test_bit(portdev->vdev, VIRTIO_CONSOLE_F_MULTIPORT);
365}
366
367static DEFINE_SPINLOCK(dma_bufs_lock);
368static LIST_HEAD(pending_free_dma_bufs);
369
370static void free_buf(struct port_buffer *buf, bool can_sleep)
371{
372 unsigned int i;
373
374 for (i = 0; i < buf->sgpages; i++) {
375 struct page *page = sg_page(&buf->sg[i]);
376 if (!page)
377 break;
378 put_page(page);
379 }
380
381 if (!buf->dev) {
382 kfree(buf->buf);
383 } else if (is_rproc_enabled) {
384 unsigned long flags;
385
386
387 if (!can_sleep) {
388
389 spin_lock_irqsave(&dma_bufs_lock, flags);
390 list_add_tail(&buf->list, &pending_free_dma_bufs);
391 spin_unlock_irqrestore(&dma_bufs_lock, flags);
392 return;
393 }
394 dma_free_coherent(buf->dev, buf->size, buf->buf, buf->dma);
395
396
397 put_device(buf->dev);
398 }
399
400 kfree(buf);
401}
402
403static void reclaim_dma_bufs(void)
404{
405 unsigned long flags;
406 struct port_buffer *buf, *tmp;
407 LIST_HEAD(tmp_list);
408
409 if (list_empty(&pending_free_dma_bufs))
410 return;
411
412
413 spin_lock_irqsave(&dma_bufs_lock, flags);
414 list_cut_position(&tmp_list, &pending_free_dma_bufs,
415 pending_free_dma_bufs.prev);
416 spin_unlock_irqrestore(&dma_bufs_lock, flags);
417
418
419 list_for_each_entry_safe(buf, tmp, &tmp_list, list) {
420 list_del(&buf->list);
421 free_buf(buf, true);
422 }
423}
424
425static struct port_buffer *alloc_buf(struct virtqueue *vq, size_t buf_size,
426 int pages)
427{
428 struct port_buffer *buf;
429
430 reclaim_dma_bufs();
431
432
433
434
435
436 buf = kmalloc(sizeof(*buf) + sizeof(struct scatterlist) * pages,
437 GFP_KERNEL);
438 if (!buf)
439 goto fail;
440
441 buf->sgpages = pages;
442 if (pages > 0) {
443 buf->dev = NULL;
444 buf->buf = NULL;
445 return buf;
446 }
447
448 if (is_rproc_serial(vq->vdev)) {
449
450
451
452
453
454
455
456
457
458 if (!vq->vdev->dev.parent || !vq->vdev->dev.parent->parent)
459 goto free_buf;
460 buf->dev = vq->vdev->dev.parent->parent;
461
462
463 get_device(buf->dev);
464 buf->buf = dma_alloc_coherent(buf->dev, buf_size, &buf->dma,
465 GFP_KERNEL);
466 } else {
467 buf->dev = NULL;
468 buf->buf = kmalloc(buf_size, GFP_KERNEL);
469 }
470
471 if (!buf->buf)
472 goto free_buf;
473 buf->len = 0;
474 buf->offset = 0;
475 buf->size = buf_size;
476 return buf;
477
478free_buf:
479 kfree(buf);
480fail:
481 return NULL;
482}
483
484
485static struct port_buffer *get_inbuf(struct port *port)
486{
487 struct port_buffer *buf;
488 unsigned int len;
489
490 if (port->inbuf)
491 return port->inbuf;
492
493 buf = virtqueue_get_buf(port->in_vq, &len);
494 if (buf) {
495 buf->len = len;
496 buf->offset = 0;
497 port->stats.bytes_received += len;
498 }
499 return buf;
500}
501
502
503
504
505
506
507
508static int add_inbuf(struct virtqueue *vq, struct port_buffer *buf)
509{
510 struct scatterlist sg[1];
511 int ret;
512
513 sg_init_one(sg, buf->buf, buf->size);
514
515 ret = virtqueue_add_inbuf(vq, sg, 1, buf, GFP_ATOMIC);
516 virtqueue_kick(vq);
517 if (!ret)
518 ret = vq->num_free;
519 return ret;
520}
521
522
523static void discard_port_data(struct port *port)
524{
525 struct port_buffer *buf;
526 unsigned int err;
527
528 if (!port->portdev) {
529
530 return;
531 }
532 buf = get_inbuf(port);
533
534 err = 0;
535 while (buf) {
536 port->stats.bytes_discarded += buf->len - buf->offset;
537 if (add_inbuf(port->in_vq, buf) < 0) {
538 err++;
539 free_buf(buf, false);
540 }
541 port->inbuf = NULL;
542 buf = get_inbuf(port);
543 }
544 if (err)
545 dev_warn(port->dev, "Errors adding %d buffers back to vq\n",
546 err);
547}
548
549static bool port_has_data(struct port *port)
550{
551 unsigned long flags;
552 bool ret;
553
554 ret = false;
555 spin_lock_irqsave(&port->inbuf_lock, flags);
556 port->inbuf = get_inbuf(port);
557 if (port->inbuf)
558 ret = true;
559
560 spin_unlock_irqrestore(&port->inbuf_lock, flags);
561 return ret;
562}
563
564static ssize_t __send_control_msg(struct ports_device *portdev, u32 port_id,
565 unsigned int event, unsigned int value)
566{
567 struct scatterlist sg[1];
568 struct virtqueue *vq;
569 unsigned int len;
570
571 if (!use_multiport(portdev))
572 return 0;
573
574 vq = portdev->c_ovq;
575
576 spin_lock(&portdev->c_ovq_lock);
577
578 portdev->cpkt.id = cpu_to_virtio32(portdev->vdev, port_id);
579 portdev->cpkt.event = cpu_to_virtio16(portdev->vdev, event);
580 portdev->cpkt.value = cpu_to_virtio16(portdev->vdev, value);
581
582 sg_init_one(sg, &portdev->cpkt, sizeof(struct virtio_console_control));
583
584 if (virtqueue_add_outbuf(vq, sg, 1, &portdev->cpkt, GFP_ATOMIC) == 0) {
585 virtqueue_kick(vq);
586 while (!virtqueue_get_buf(vq, &len)
587 && !virtqueue_is_broken(vq))
588 cpu_relax();
589 }
590
591 spin_unlock(&portdev->c_ovq_lock);
592 return 0;
593}
594
595static ssize_t send_control_msg(struct port *port, unsigned int event,
596 unsigned int value)
597{
598
599 if (port->portdev)
600 return __send_control_msg(port->portdev, port->id, event, value);
601 return 0;
602}
603
604
605
606static void reclaim_consumed_buffers(struct port *port)
607{
608 struct port_buffer *buf;
609 unsigned int len;
610
611 if (!port->portdev) {
612
613 return;
614 }
615 while ((buf = virtqueue_get_buf(port->out_vq, &len))) {
616 free_buf(buf, false);
617 port->outvq_full = false;
618 }
619}
620
621static ssize_t __send_to_port(struct port *port, struct scatterlist *sg,
622 int nents, size_t in_count,
623 void *data, bool nonblock)
624{
625 struct virtqueue *out_vq;
626 int err;
627 unsigned long flags;
628 unsigned int len;
629
630 out_vq = port->out_vq;
631
632 spin_lock_irqsave(&port->outvq_lock, flags);
633
634 reclaim_consumed_buffers(port);
635
636 err = virtqueue_add_outbuf(out_vq, sg, nents, data, GFP_ATOMIC);
637
638
639 virtqueue_kick(out_vq);
640
641 if (err) {
642 in_count = 0;
643 goto done;
644 }
645
646 if (out_vq->num_free == 0)
647 port->outvq_full = true;
648
649 if (nonblock)
650 goto done;
651
652
653
654
655
656
657
658
659
660
661 while (!virtqueue_get_buf(out_vq, &len)
662 && !virtqueue_is_broken(out_vq))
663 cpu_relax();
664done:
665 spin_unlock_irqrestore(&port->outvq_lock, flags);
666
667 port->stats.bytes_sent += in_count;
668
669
670
671
672 return in_count;
673}
674
675
676
677
678
679static ssize_t fill_readbuf(struct port *port, char __user *out_buf,
680 size_t out_count, bool to_user)
681{
682 struct port_buffer *buf;
683 unsigned long flags;
684
685 if (!out_count || !port_has_data(port))
686 return 0;
687
688 buf = port->inbuf;
689 out_count = min(out_count, buf->len - buf->offset);
690
691 if (to_user) {
692 ssize_t ret;
693
694 ret = copy_to_user(out_buf, buf->buf + buf->offset, out_count);
695 if (ret)
696 return -EFAULT;
697 } else {
698 memcpy((__force char *)out_buf, buf->buf + buf->offset,
699 out_count);
700 }
701
702 buf->offset += out_count;
703
704 if (buf->offset == buf->len) {
705
706
707
708
709 spin_lock_irqsave(&port->inbuf_lock, flags);
710 port->inbuf = NULL;
711
712 if (add_inbuf(port->in_vq, buf) < 0)
713 dev_warn(port->dev, "failed add_buf\n");
714
715 spin_unlock_irqrestore(&port->inbuf_lock, flags);
716 }
717
718 return out_count;
719}
720
721
722static bool will_read_block(struct port *port)
723{
724 if (!port->guest_connected) {
725
726 return false;
727 }
728 return !port_has_data(port) && port->host_connected;
729}
730
731static bool will_write_block(struct port *port)
732{
733 bool ret;
734
735 if (!port->guest_connected) {
736
737 return false;
738 }
739 if (!port->host_connected)
740 return true;
741
742 spin_lock_irq(&port->outvq_lock);
743
744
745
746
747 reclaim_consumed_buffers(port);
748 ret = port->outvq_full;
749 spin_unlock_irq(&port->outvq_lock);
750
751 return ret;
752}
753
754static ssize_t port_fops_read(struct file *filp, char __user *ubuf,
755 size_t count, loff_t *offp)
756{
757 struct port *port;
758 ssize_t ret;
759
760 port = filp->private_data;
761
762
763 if (!port->guest_connected)
764 return -ENODEV;
765
766 if (!port_has_data(port)) {
767
768
769
770
771
772 if (!port->host_connected)
773 return 0;
774 if (filp->f_flags & O_NONBLOCK)
775 return -EAGAIN;
776
777 ret = wait_event_freezable(port->waitqueue,
778 !will_read_block(port));
779 if (ret < 0)
780 return ret;
781 }
782
783 if (!port->guest_connected)
784 return -ENODEV;
785
786
787
788
789
790
791
792
793
794
795 if (!port_has_data(port) && !port->host_connected)
796 return 0;
797
798 return fill_readbuf(port, ubuf, count, true);
799}
800
801static int wait_port_writable(struct port *port, bool nonblock)
802{
803 int ret;
804
805 if (will_write_block(port)) {
806 if (nonblock)
807 return -EAGAIN;
808
809 ret = wait_event_freezable(port->waitqueue,
810 !will_write_block(port));
811 if (ret < 0)
812 return ret;
813 }
814
815 if (!port->guest_connected)
816 return -ENODEV;
817
818 return 0;
819}
820
821static ssize_t port_fops_write(struct file *filp, const char __user *ubuf,
822 size_t count, loff_t *offp)
823{
824 struct port *port;
825 struct port_buffer *buf;
826 ssize_t ret;
827 bool nonblock;
828 struct scatterlist sg[1];
829
830
831 if (!count)
832 return 0;
833
834 port = filp->private_data;
835
836 nonblock = filp->f_flags & O_NONBLOCK;
837
838 ret = wait_port_writable(port, nonblock);
839 if (ret < 0)
840 return ret;
841
842 count = min((size_t)(32 * 1024), count);
843
844 buf = alloc_buf(port->out_vq, count, 0);
845 if (!buf)
846 return -ENOMEM;
847
848 ret = copy_from_user(buf->buf, ubuf, count);
849 if (ret) {
850 ret = -EFAULT;
851 goto free_buf;
852 }
853
854
855
856
857
858
859
860
861 nonblock = true;
862 sg_init_one(sg, buf->buf, count);
863 ret = __send_to_port(port, sg, 1, count, buf, nonblock);
864
865 if (nonblock && ret > 0)
866 goto out;
867
868free_buf:
869 free_buf(buf, true);
870out:
871 return ret;
872}
873
874struct sg_list {
875 unsigned int n;
876 unsigned int size;
877 size_t len;
878 struct scatterlist *sg;
879};
880
881static int pipe_to_sg(struct pipe_inode_info *pipe, struct pipe_buffer *buf,
882 struct splice_desc *sd)
883{
884 struct sg_list *sgl = sd->u.data;
885 unsigned int offset, len;
886
887 if (sgl->n == sgl->size)
888 return 0;
889
890
891 if (pipe_buf_steal(pipe, buf) == 0) {
892
893 get_page(buf->page);
894 unlock_page(buf->page);
895
896 len = min(buf->len, sd->len);
897 sg_set_page(&(sgl->sg[sgl->n]), buf->page, len, buf->offset);
898 } else {
899
900 struct page *page = alloc_page(GFP_KERNEL);
901 char *src;
902
903 if (!page)
904 return -ENOMEM;
905
906 offset = sd->pos & ~PAGE_MASK;
907
908 len = sd->len;
909 if (len + offset > PAGE_SIZE)
910 len = PAGE_SIZE - offset;
911
912 src = kmap_atomic(buf->page);
913 memcpy(page_address(page) + offset, src + buf->offset, len);
914 kunmap_atomic(src);
915
916 sg_set_page(&(sgl->sg[sgl->n]), page, len, offset);
917 }
918 sgl->n++;
919 sgl->len += len;
920
921 return len;
922}
923
924
925static ssize_t port_fops_splice_write(struct pipe_inode_info *pipe,
926 struct file *filp, loff_t *ppos,
927 size_t len, unsigned int flags)
928{
929 struct port *port = filp->private_data;
930 struct sg_list sgl;
931 ssize_t ret;
932 struct port_buffer *buf;
933 struct splice_desc sd = {
934 .total_len = len,
935 .flags = flags,
936 .pos = *ppos,
937 .u.data = &sgl,
938 };
939
940
941
942
943
944
945
946 if (is_rproc_serial(port->out_vq->vdev))
947 return -EINVAL;
948
949
950
951
952
953 pipe_lock(pipe);
954 if (!pipe->nrbufs) {
955 ret = 0;
956 goto error_out;
957 }
958
959 ret = wait_port_writable(port, filp->f_flags & O_NONBLOCK);
960 if (ret < 0)
961 goto error_out;
962
963 buf = alloc_buf(port->out_vq, 0, pipe->nrbufs);
964 if (!buf) {
965 ret = -ENOMEM;
966 goto error_out;
967 }
968
969 sgl.n = 0;
970 sgl.len = 0;
971 sgl.size = pipe->nrbufs;
972 sgl.sg = buf->sg;
973 sg_init_table(sgl.sg, sgl.size);
974 ret = __splice_from_pipe(pipe, &sd, pipe_to_sg);
975 pipe_unlock(pipe);
976 if (likely(ret > 0))
977 ret = __send_to_port(port, buf->sg, sgl.n, sgl.len, buf, true);
978
979 if (unlikely(ret <= 0))
980 free_buf(buf, true);
981 return ret;
982
983error_out:
984 pipe_unlock(pipe);
985 return ret;
986}
987
988static unsigned int port_fops_poll(struct file *filp, poll_table *wait)
989{
990 struct port *port;
991 unsigned int ret;
992
993 port = filp->private_data;
994 poll_wait(filp, &port->waitqueue, wait);
995
996 if (!port->guest_connected) {
997
998 return POLLHUP;
999 }
1000 ret = 0;
1001 if (!will_read_block(port))
1002 ret |= POLLIN | POLLRDNORM;
1003 if (!will_write_block(port))
1004 ret |= POLLOUT;
1005 if (!port->host_connected)
1006 ret |= POLLHUP;
1007
1008 return ret;
1009}
1010
1011static void remove_port(struct kref *kref);
1012
1013static int port_fops_release(struct inode *inode, struct file *filp)
1014{
1015 struct port *port;
1016
1017 port = filp->private_data;
1018
1019
1020 send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 0);
1021
1022 spin_lock_irq(&port->inbuf_lock);
1023 port->guest_connected = false;
1024
1025 discard_port_data(port);
1026
1027 spin_unlock_irq(&port->inbuf_lock);
1028
1029 spin_lock_irq(&port->outvq_lock);
1030 reclaim_consumed_buffers(port);
1031 spin_unlock_irq(&port->outvq_lock);
1032
1033 reclaim_dma_bufs();
1034
1035
1036
1037
1038
1039
1040
1041
1042 kref_put(&port->kref, remove_port);
1043
1044 return 0;
1045}
1046
1047static int port_fops_open(struct inode *inode, struct file *filp)
1048{
1049 struct cdev *cdev = inode->i_cdev;
1050 struct port *port;
1051 int ret;
1052
1053
1054 port = find_port_by_devt(cdev->dev);
1055 if (!port) {
1056
1057 return -ENXIO;
1058 }
1059 filp->private_data = port;
1060
1061
1062
1063
1064
1065 if (is_console_port(port)) {
1066 ret = -ENXIO;
1067 goto out;
1068 }
1069
1070
1071 spin_lock_irq(&port->inbuf_lock);
1072 if (port->guest_connected) {
1073 spin_unlock_irq(&port->inbuf_lock);
1074 ret = -EBUSY;
1075 goto out;
1076 }
1077
1078 port->guest_connected = true;
1079 spin_unlock_irq(&port->inbuf_lock);
1080
1081 spin_lock_irq(&port->outvq_lock);
1082
1083
1084
1085
1086
1087 reclaim_consumed_buffers(port);
1088 spin_unlock_irq(&port->outvq_lock);
1089
1090 nonseekable_open(inode, filp);
1091
1092
1093 send_control_msg(filp->private_data, VIRTIO_CONSOLE_PORT_OPEN, 1);
1094
1095 return 0;
1096out:
1097 kref_put(&port->kref, remove_port);
1098 return ret;
1099}
1100
1101static int port_fops_fasync(int fd, struct file *filp, int mode)
1102{
1103 struct port *port;
1104
1105 port = filp->private_data;
1106 return fasync_helper(fd, filp, mode, &port->async_queue);
1107}
1108
1109
1110
1111
1112
1113
1114
1115static const struct file_operations port_fops = {
1116 .owner = THIS_MODULE,
1117 .open = port_fops_open,
1118 .read = port_fops_read,
1119 .write = port_fops_write,
1120 .splice_write = port_fops_splice_write,
1121 .poll = port_fops_poll,
1122 .release = port_fops_release,
1123 .fasync = port_fops_fasync,
1124 .llseek = no_llseek,
1125};
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135static int put_chars(u32 vtermno, const char *buf, int count)
1136{
1137 struct port *port;
1138 struct scatterlist sg[1];
1139
1140 if (unlikely(early_put_chars))
1141 return early_put_chars(vtermno, buf, count);
1142
1143 port = find_port_by_vtermno(vtermno);
1144 if (!port)
1145 return -EPIPE;
1146
1147 sg_init_one(sg, buf, count);
1148 return __send_to_port(port, sg, 1, count, (void *)buf, false);
1149}
1150
1151
1152
1153
1154
1155
1156
1157
1158static int get_chars(u32 vtermno, char *buf, int count)
1159{
1160 struct port *port;
1161
1162
1163 if (unlikely(early_put_chars))
1164 return 0;
1165
1166 port = find_port_by_vtermno(vtermno);
1167 if (!port)
1168 return -EPIPE;
1169
1170
1171 BUG_ON(!port->in_vq);
1172
1173 return fill_readbuf(port, (__force char __user *)buf, count, false);
1174}
1175
1176static void resize_console(struct port *port)
1177{
1178 struct virtio_device *vdev;
1179
1180
1181 if (!port || !is_console_port(port))
1182 return;
1183
1184 vdev = port->portdev->vdev;
1185
1186
1187 if (!is_rproc_serial(vdev) &&
1188 virtio_has_feature(vdev, VIRTIO_CONSOLE_F_SIZE))
1189 hvc_resize(port->cons.hvc, port->cons.ws);
1190}
1191
1192
1193static int notifier_add_vio(struct hvc_struct *hp, int data)
1194{
1195 struct port *port;
1196
1197 port = find_port_by_vtermno(hp->vtermno);
1198 if (!port)
1199 return -EINVAL;
1200
1201 hp->irq_requested = 1;
1202 resize_console(port);
1203
1204 return 0;
1205}
1206
1207static void notifier_del_vio(struct hvc_struct *hp, int data)
1208{
1209 hp->irq_requested = 0;
1210}
1211
1212
1213static const struct hv_ops hv_ops = {
1214 .get_chars = get_chars,
1215 .put_chars = put_chars,
1216 .notifier_add = notifier_add_vio,
1217 .notifier_del = notifier_del_vio,
1218 .notifier_hangup = notifier_del_vio,
1219};
1220
1221
1222
1223
1224
1225
1226
1227
1228
1229
1230int __init virtio_cons_early_init(int (*put_chars)(u32, const char *, int))
1231{
1232 early_put_chars = put_chars;
1233 return hvc_instantiate(0, 0, &hv_ops);
1234}
1235
1236static int init_port_console(struct port *port)
1237{
1238 int ret;
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257 port->cons.vtermno = pdrvdata.next_vtermno;
1258
1259 port->cons.hvc = hvc_alloc(port->cons.vtermno, 0, &hv_ops, PAGE_SIZE);
1260 if (IS_ERR(port->cons.hvc)) {
1261 ret = PTR_ERR(port->cons.hvc);
1262 dev_err(port->dev,
1263 "error %d allocating hvc for port\n", ret);
1264 port->cons.hvc = NULL;
1265 return ret;
1266 }
1267 spin_lock_irq(&pdrvdata_lock);
1268 pdrvdata.next_vtermno++;
1269 list_add_tail(&port->cons.list, &pdrvdata.consoles);
1270 spin_unlock_irq(&pdrvdata_lock);
1271 port->guest_connected = true;
1272
1273
1274
1275
1276
1277 if (early_put_chars)
1278 early_put_chars = NULL;
1279
1280
1281 send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 1);
1282
1283 return 0;
1284}
1285
1286static ssize_t show_port_name(struct device *dev,
1287 struct device_attribute *attr, char *buffer)
1288{
1289 struct port *port;
1290
1291 port = dev_get_drvdata(dev);
1292
1293 return sprintf(buffer, "%s\n", port->name);
1294}
1295
1296static DEVICE_ATTR(name, S_IRUGO, show_port_name, NULL);
1297
1298static struct attribute *port_sysfs_entries[] = {
1299 &dev_attr_name.attr,
1300 NULL
1301};
1302
1303static struct attribute_group port_attribute_group = {
1304 .name = NULL,
1305 .attrs = port_sysfs_entries,
1306};
1307
1308static ssize_t debugfs_read(struct file *filp, char __user *ubuf,
1309 size_t count, loff_t *offp)
1310{
1311 struct port *port;
1312 char *buf;
1313 ssize_t ret, out_offset, out_count;
1314
1315 out_count = 1024;
1316 buf = kmalloc(out_count, GFP_KERNEL);
1317 if (!buf)
1318 return -ENOMEM;
1319
1320 port = filp->private_data;
1321 out_offset = 0;
1322 out_offset += snprintf(buf + out_offset, out_count,
1323 "name: %s\n", port->name ? port->name : "");
1324 out_offset += snprintf(buf + out_offset, out_count - out_offset,
1325 "guest_connected: %d\n", port->guest_connected);
1326 out_offset += snprintf(buf + out_offset, out_count - out_offset,
1327 "host_connected: %d\n", port->host_connected);
1328 out_offset += snprintf(buf + out_offset, out_count - out_offset,
1329 "outvq_full: %d\n", port->outvq_full);
1330 out_offset += snprintf(buf + out_offset, out_count - out_offset,
1331 "bytes_sent: %lu\n", port->stats.bytes_sent);
1332 out_offset += snprintf(buf + out_offset, out_count - out_offset,
1333 "bytes_received: %lu\n",
1334 port->stats.bytes_received);
1335 out_offset += snprintf(buf + out_offset, out_count - out_offset,
1336 "bytes_discarded: %lu\n",
1337 port->stats.bytes_discarded);
1338 out_offset += snprintf(buf + out_offset, out_count - out_offset,
1339 "is_console: %s\n",
1340 is_console_port(port) ? "yes" : "no");
1341 out_offset += snprintf(buf + out_offset, out_count - out_offset,
1342 "console_vtermno: %u\n", port->cons.vtermno);
1343
1344 ret = simple_read_from_buffer(ubuf, count, offp, buf, out_offset);
1345 kfree(buf);
1346 return ret;
1347}
1348
1349static const struct file_operations port_debugfs_ops = {
1350 .owner = THIS_MODULE,
1351 .open = simple_open,
1352 .read = debugfs_read,
1353};
1354
1355static void set_console_size(struct port *port, u16 rows, u16 cols)
1356{
1357 if (!port || !is_console_port(port))
1358 return;
1359
1360 port->cons.ws.ws_row = rows;
1361 port->cons.ws.ws_col = cols;
1362}
1363
1364static unsigned int fill_queue(struct virtqueue *vq, spinlock_t *lock)
1365{
1366 struct port_buffer *buf;
1367 unsigned int nr_added_bufs;
1368 int ret;
1369
1370 nr_added_bufs = 0;
1371 do {
1372 buf = alloc_buf(vq, PAGE_SIZE, 0);
1373 if (!buf)
1374 break;
1375
1376 spin_lock_irq(lock);
1377 ret = add_inbuf(vq, buf);
1378 if (ret < 0) {
1379 spin_unlock_irq(lock);
1380 free_buf(buf, true);
1381 break;
1382 }
1383 nr_added_bufs++;
1384 spin_unlock_irq(lock);
1385 } while (ret > 0);
1386
1387 return nr_added_bufs;
1388}
1389
1390static void send_sigio_to_port(struct port *port)
1391{
1392 if (port->async_queue && port->guest_connected)
1393 kill_fasync(&port->async_queue, SIGIO, POLL_OUT);
1394}
1395
1396static int add_port(struct ports_device *portdev, u32 id)
1397{
1398 char debugfs_name[16];
1399 struct port *port;
1400 struct port_buffer *buf;
1401 dev_t devt;
1402 unsigned int nr_added_bufs;
1403 int err;
1404
1405 port = kmalloc(sizeof(*port), GFP_KERNEL);
1406 if (!port) {
1407 err = -ENOMEM;
1408 goto fail;
1409 }
1410 kref_init(&port->kref);
1411
1412 port->portdev = portdev;
1413 port->id = id;
1414
1415 port->name = NULL;
1416 port->inbuf = NULL;
1417 port->cons.hvc = NULL;
1418 port->async_queue = NULL;
1419
1420 port->cons.ws.ws_row = port->cons.ws.ws_col = 0;
1421
1422 port->host_connected = port->guest_connected = false;
1423 port->stats = (struct port_stats) { 0 };
1424
1425 port->outvq_full = false;
1426
1427 port->in_vq = portdev->in_vqs[port->id];
1428 port->out_vq = portdev->out_vqs[port->id];
1429
1430 port->cdev = cdev_alloc();
1431 if (!port->cdev) {
1432 dev_err(&port->portdev->vdev->dev, "Error allocating cdev\n");
1433 err = -ENOMEM;
1434 goto free_port;
1435 }
1436 port->cdev->ops = &port_fops;
1437
1438 devt = MKDEV(portdev->chr_major, id);
1439 err = cdev_add(port->cdev, devt, 1);
1440 if (err < 0) {
1441 dev_err(&port->portdev->vdev->dev,
1442 "Error %d adding cdev for port %u\n", err, id);
1443 goto free_cdev;
1444 }
1445 port->dev = device_create(pdrvdata.class, &port->portdev->vdev->dev,
1446 devt, port, "vport%up%u",
1447 port->portdev->vdev->index, id);
1448 if (IS_ERR(port->dev)) {
1449 err = PTR_ERR(port->dev);
1450 dev_err(&port->portdev->vdev->dev,
1451 "Error %d creating device for port %u\n",
1452 err, id);
1453 goto free_cdev;
1454 }
1455
1456 spin_lock_init(&port->inbuf_lock);
1457 spin_lock_init(&port->outvq_lock);
1458 init_waitqueue_head(&port->waitqueue);
1459
1460
1461 nr_added_bufs = fill_queue(port->in_vq, &port->inbuf_lock);
1462 if (!nr_added_bufs) {
1463 dev_err(port->dev, "Error allocating inbufs\n");
1464 err = -ENOMEM;
1465 goto free_device;
1466 }
1467
1468 if (is_rproc_serial(port->portdev->vdev))
1469
1470
1471
1472
1473
1474 port->host_connected = true;
1475 else if (!use_multiport(port->portdev)) {
1476
1477
1478
1479
1480 err = init_port_console(port);
1481 if (err)
1482 goto free_inbufs;
1483 }
1484
1485 spin_lock_irq(&portdev->ports_lock);
1486 list_add_tail(&port->list, &port->portdev->ports);
1487 spin_unlock_irq(&portdev->ports_lock);
1488
1489
1490
1491
1492
1493
1494 send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
1495
1496 if (pdrvdata.debugfs_dir) {
1497
1498
1499
1500
1501 snprintf(debugfs_name, sizeof(debugfs_name), "vport%up%u",
1502 port->portdev->vdev->index, id);
1503 port->debugfs_file = debugfs_create_file(debugfs_name, 0444,
1504 pdrvdata.debugfs_dir,
1505 port,
1506 &port_debugfs_ops);
1507 }
1508 return 0;
1509
1510free_inbufs:
1511 while ((buf = virtqueue_detach_unused_buf(port->in_vq)))
1512 free_buf(buf, true);
1513free_device:
1514 device_destroy(pdrvdata.class, port->dev->devt);
1515free_cdev:
1516 cdev_del(port->cdev);
1517free_port:
1518 kfree(port);
1519fail:
1520
1521 __send_control_msg(portdev, id, VIRTIO_CONSOLE_PORT_READY, 0);
1522 return err;
1523}
1524
1525
1526static void remove_port(struct kref *kref)
1527{
1528 struct port *port;
1529
1530 port = container_of(kref, struct port, kref);
1531
1532 kfree(port);
1533}
1534
1535static void remove_port_data(struct port *port)
1536{
1537 struct port_buffer *buf;
1538
1539 spin_lock_irq(&port->inbuf_lock);
1540
1541 discard_port_data(port);
1542 spin_unlock_irq(&port->inbuf_lock);
1543
1544
1545 do {
1546 spin_lock_irq(&port->inbuf_lock);
1547 buf = virtqueue_detach_unused_buf(port->in_vq);
1548 spin_unlock_irq(&port->inbuf_lock);
1549 if (buf)
1550 free_buf(buf, true);
1551 } while (buf);
1552
1553 spin_lock_irq(&port->outvq_lock);
1554 reclaim_consumed_buffers(port);
1555 spin_unlock_irq(&port->outvq_lock);
1556
1557
1558 do {
1559 spin_lock_irq(&port->outvq_lock);
1560 buf = virtqueue_detach_unused_buf(port->out_vq);
1561 spin_unlock_irq(&port->outvq_lock);
1562 if (buf)
1563 free_buf(buf, true);
1564 } while (buf);
1565}
1566
1567
1568
1569
1570
1571
1572static void unplug_port(struct port *port)
1573{
1574 spin_lock_irq(&port->portdev->ports_lock);
1575 list_del(&port->list);
1576 spin_unlock_irq(&port->portdev->ports_lock);
1577
1578 spin_lock_irq(&port->inbuf_lock);
1579 if (port->guest_connected) {
1580
1581 send_sigio_to_port(port);
1582
1583
1584 port->guest_connected = false;
1585 port->host_connected = false;
1586
1587 wake_up_interruptible(&port->waitqueue);
1588 }
1589 spin_unlock_irq(&port->inbuf_lock);
1590
1591 if (is_console_port(port)) {
1592 spin_lock_irq(&pdrvdata_lock);
1593 list_del(&port->cons.list);
1594 spin_unlock_irq(&pdrvdata_lock);
1595 hvc_remove(port->cons.hvc);
1596 }
1597
1598 remove_port_data(port);
1599
1600
1601
1602
1603
1604
1605 port->portdev = NULL;
1606
1607 sysfs_remove_group(&port->dev->kobj, &port_attribute_group);
1608 device_destroy(pdrvdata.class, port->dev->devt);
1609 cdev_del(port->cdev);
1610
1611 debugfs_remove(port->debugfs_file);
1612 kfree(port->name);
1613
1614
1615
1616
1617
1618
1619 kref_put(&port->kref, remove_port);
1620}
1621
1622
1623static void handle_control_message(struct virtio_device *vdev,
1624 struct ports_device *portdev,
1625 struct port_buffer *buf)
1626{
1627 struct virtio_console_control *cpkt;
1628 struct port *port;
1629 size_t name_size;
1630 int err;
1631
1632 cpkt = (struct virtio_console_control *)(buf->buf + buf->offset);
1633
1634 port = find_port_by_id(portdev, virtio32_to_cpu(vdev, cpkt->id));
1635 if (!port &&
1636 cpkt->event != cpu_to_virtio16(vdev, VIRTIO_CONSOLE_PORT_ADD)) {
1637
1638 dev_dbg(&portdev->vdev->dev,
1639 "Invalid index %u in control packet\n", cpkt->id);
1640 return;
1641 }
1642
1643 switch (virtio16_to_cpu(vdev, cpkt->event)) {
1644 case VIRTIO_CONSOLE_PORT_ADD:
1645 if (port) {
1646 dev_dbg(&portdev->vdev->dev,
1647 "Port %u already added\n", port->id);
1648 send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
1649 break;
1650 }
1651 if (virtio32_to_cpu(vdev, cpkt->id) >=
1652 portdev->config.max_nr_ports) {
1653 dev_warn(&portdev->vdev->dev,
1654 "Request for adding port with "
1655 "out-of-bound id %u, max. supported id: %u\n",
1656 cpkt->id, portdev->config.max_nr_ports - 1);
1657 break;
1658 }
1659 add_port(portdev, virtio32_to_cpu(vdev, cpkt->id));
1660 break;
1661 case VIRTIO_CONSOLE_PORT_REMOVE:
1662 unplug_port(port);
1663 break;
1664 case VIRTIO_CONSOLE_CONSOLE_PORT:
1665 if (!cpkt->value)
1666 break;
1667 if (is_console_port(port))
1668 break;
1669
1670 init_port_console(port);
1671 complete(&early_console_added);
1672
1673
1674
1675
1676 break;
1677 case VIRTIO_CONSOLE_RESIZE: {
1678 struct {
1679 __u16 rows;
1680 __u16 cols;
1681 } size;
1682
1683 if (!is_console_port(port))
1684 break;
1685
1686 memcpy(&size, buf->buf + buf->offset + sizeof(*cpkt),
1687 sizeof(size));
1688 set_console_size(port, size.rows, size.cols);
1689
1690 port->cons.hvc->irq_requested = 1;
1691 resize_console(port);
1692 break;
1693 }
1694 case VIRTIO_CONSOLE_PORT_OPEN:
1695 port->host_connected = virtio16_to_cpu(vdev, cpkt->value);
1696 wake_up_interruptible(&port->waitqueue);
1697
1698
1699
1700
1701
1702 spin_lock_irq(&port->outvq_lock);
1703 reclaim_consumed_buffers(port);
1704 spin_unlock_irq(&port->outvq_lock);
1705
1706
1707
1708
1709
1710 spin_lock_irq(&port->inbuf_lock);
1711 send_sigio_to_port(port);
1712 spin_unlock_irq(&port->inbuf_lock);
1713 break;
1714 case VIRTIO_CONSOLE_PORT_NAME:
1715
1716
1717
1718
1719 if (port->name)
1720 break;
1721
1722
1723
1724
1725
1726 name_size = buf->len - buf->offset - sizeof(*cpkt) + 1;
1727
1728 port->name = kmalloc(name_size, GFP_KERNEL);
1729 if (!port->name) {
1730 dev_err(port->dev,
1731 "Not enough space to store port name\n");
1732 break;
1733 }
1734 strncpy(port->name, buf->buf + buf->offset + sizeof(*cpkt),
1735 name_size - 1);
1736 port->name[name_size - 1] = 0;
1737
1738
1739
1740
1741
1742 err = sysfs_create_group(&port->dev->kobj,
1743 &port_attribute_group);
1744 if (err) {
1745 dev_err(port->dev,
1746 "Error %d creating sysfs device attributes\n",
1747 err);
1748 } else {
1749
1750
1751
1752
1753
1754 kobject_uevent(&port->dev->kobj, KOBJ_CHANGE);
1755 }
1756 break;
1757 }
1758}
1759
1760static void control_work_handler(struct work_struct *work)
1761{
1762 struct ports_device *portdev;
1763 struct virtqueue *vq;
1764 struct port_buffer *buf;
1765 unsigned int len;
1766
1767 portdev = container_of(work, struct ports_device, control_work);
1768 vq = portdev->c_ivq;
1769
1770 spin_lock(&portdev->c_ivq_lock);
1771 while ((buf = virtqueue_get_buf(vq, &len))) {
1772 spin_unlock(&portdev->c_ivq_lock);
1773
1774 buf->len = len;
1775 buf->offset = 0;
1776
1777 handle_control_message(vq->vdev, portdev, buf);
1778
1779 spin_lock(&portdev->c_ivq_lock);
1780 if (add_inbuf(portdev->c_ivq, buf) < 0) {
1781 dev_warn(&portdev->vdev->dev,
1782 "Error adding buffer to queue\n");
1783 free_buf(buf, false);
1784 }
1785 }
1786 spin_unlock(&portdev->c_ivq_lock);
1787}
1788
1789static void out_intr(struct virtqueue *vq)
1790{
1791 struct port *port;
1792
1793 port = find_port_by_vq(vq->vdev->priv, vq);
1794 if (!port)
1795 return;
1796
1797 wake_up_interruptible(&port->waitqueue);
1798}
1799
1800static void in_intr(struct virtqueue *vq)
1801{
1802 struct port *port;
1803 unsigned long flags;
1804
1805 port = find_port_by_vq(vq->vdev->priv, vq);
1806 if (!port)
1807 return;
1808
1809 spin_lock_irqsave(&port->inbuf_lock, flags);
1810 port->inbuf = get_inbuf(port);
1811
1812
1813
1814
1815
1816
1817
1818
1819
1820
1821
1822
1823
1824
1825
1826
1827
1828
1829 if (!port->guest_connected && !is_rproc_serial(port->portdev->vdev))
1830 discard_port_data(port);
1831
1832
1833 send_sigio_to_port(port);
1834
1835 spin_unlock_irqrestore(&port->inbuf_lock, flags);
1836
1837 wake_up_interruptible(&port->waitqueue);
1838
1839 if (is_console_port(port) && hvc_poll(port->cons.hvc))
1840 hvc_kick();
1841}
1842
1843static void control_intr(struct virtqueue *vq)
1844{
1845 struct ports_device *portdev;
1846
1847 portdev = vq->vdev->priv;
1848 schedule_work(&portdev->control_work);
1849}
1850
1851static void config_intr(struct virtio_device *vdev)
1852{
1853 struct ports_device *portdev;
1854
1855 portdev = vdev->priv;
1856
1857 if (!use_multiport(portdev))
1858 schedule_work(&portdev->config_work);
1859}
1860
1861static void config_work_handler(struct work_struct *work)
1862{
1863 struct ports_device *portdev;
1864
1865 portdev = container_of(work, struct ports_device, control_work);
1866 if (!use_multiport(portdev)) {
1867 struct virtio_device *vdev;
1868 struct port *port;
1869 u16 rows, cols;
1870
1871 vdev = portdev->vdev;
1872 virtio_cread(vdev, struct virtio_console_config, cols, &cols);
1873 virtio_cread(vdev, struct virtio_console_config, rows, &rows);
1874
1875 port = find_port_by_id(portdev, 0);
1876 set_console_size(port, rows, cols);
1877
1878
1879
1880
1881
1882
1883
1884
1885 resize_console(port);
1886 }
1887}
1888
1889static int init_vqs(struct ports_device *portdev)
1890{
1891 vq_callback_t **io_callbacks;
1892 char **io_names;
1893 struct virtqueue **vqs;
1894 u32 i, j, nr_ports, nr_queues;
1895 int err;
1896
1897 nr_ports = portdev->config.max_nr_ports;
1898 nr_queues = use_multiport(portdev) ? (nr_ports + 1) * 2 : 2;
1899
1900 vqs = kmalloc(nr_queues * sizeof(struct virtqueue *), GFP_KERNEL);
1901 io_callbacks = kmalloc(nr_queues * sizeof(vq_callback_t *), GFP_KERNEL);
1902 io_names = kmalloc(nr_queues * sizeof(char *), GFP_KERNEL);
1903 portdev->in_vqs = kmalloc(nr_ports * sizeof(struct virtqueue *),
1904 GFP_KERNEL);
1905 portdev->out_vqs = kmalloc(nr_ports * sizeof(struct virtqueue *),
1906 GFP_KERNEL);
1907 if (!vqs || !io_callbacks || !io_names || !portdev->in_vqs ||
1908 !portdev->out_vqs) {
1909 err = -ENOMEM;
1910 goto free;
1911 }
1912
1913
1914
1915
1916
1917
1918 j = 0;
1919 io_callbacks[j] = in_intr;
1920 io_callbacks[j + 1] = out_intr;
1921 io_names[j] = "input";
1922 io_names[j + 1] = "output";
1923 j += 2;
1924
1925 if (use_multiport(portdev)) {
1926 io_callbacks[j] = control_intr;
1927 io_callbacks[j + 1] = NULL;
1928 io_names[j] = "control-i";
1929 io_names[j + 1] = "control-o";
1930
1931 for (i = 1; i < nr_ports; i++) {
1932 j += 2;
1933 io_callbacks[j] = in_intr;
1934 io_callbacks[j + 1] = out_intr;
1935 io_names[j] = "input";
1936 io_names[j + 1] = "output";
1937 }
1938 }
1939
1940 err = portdev->vdev->config->find_vqs(portdev->vdev, nr_queues, vqs,
1941 io_callbacks,
1942 (const char **)io_names);
1943 if (err)
1944 goto free;
1945
1946 j = 0;
1947 portdev->in_vqs[0] = vqs[0];
1948 portdev->out_vqs[0] = vqs[1];
1949 j += 2;
1950 if (use_multiport(portdev)) {
1951 portdev->c_ivq = vqs[j];
1952 portdev->c_ovq = vqs[j + 1];
1953
1954 for (i = 1; i < nr_ports; i++) {
1955 j += 2;
1956 portdev->in_vqs[i] = vqs[j];
1957 portdev->out_vqs[i] = vqs[j + 1];
1958 }
1959 }
1960 kfree(io_names);
1961 kfree(io_callbacks);
1962 kfree(vqs);
1963
1964 return 0;
1965
1966free:
1967 kfree(portdev->out_vqs);
1968 kfree(portdev->in_vqs);
1969 kfree(io_names);
1970 kfree(io_callbacks);
1971 kfree(vqs);
1972
1973 return err;
1974}
1975
1976static const struct file_operations portdev_fops = {
1977 .owner = THIS_MODULE,
1978};
1979
1980static void remove_vqs(struct ports_device *portdev)
1981{
1982 portdev->vdev->config->del_vqs(portdev->vdev);
1983 kfree(portdev->in_vqs);
1984 kfree(portdev->out_vqs);
1985}
1986
1987static void remove_controlq_data(struct ports_device *portdev)
1988{
1989 struct port_buffer *buf;
1990 unsigned int len;
1991
1992 if (!use_multiport(portdev))
1993 return;
1994
1995 while ((buf = virtqueue_get_buf(portdev->c_ivq, &len)))
1996 free_buf(buf, true);
1997
1998 while ((buf = virtqueue_detach_unused_buf(portdev->c_ivq)))
1999 free_buf(buf, true);
2000}
2001
2002
2003
2004
2005
2006
2007
2008
2009
2010static int virtcons_probe(struct virtio_device *vdev)
2011{
2012 struct ports_device *portdev;
2013 int err;
2014 bool multiport;
2015 bool early = early_put_chars != NULL;
2016
2017
2018 if (!vdev->config->get &&
2019 (virtio_has_feature(vdev, VIRTIO_CONSOLE_F_SIZE)
2020 || virtio_has_feature(vdev, VIRTIO_CONSOLE_F_MULTIPORT))) {
2021 dev_err(&vdev->dev, "%s failure: config access disabled\n",
2022 __func__);
2023 return -EINVAL;
2024 }
2025
2026
2027 barrier();
2028
2029 portdev = kmalloc(sizeof(*portdev), GFP_KERNEL);
2030 if (!portdev) {
2031 err = -ENOMEM;
2032 goto fail;
2033 }
2034
2035
2036 portdev->vdev = vdev;
2037 vdev->priv = portdev;
2038
2039 portdev->chr_major = register_chrdev(0, "virtio-portsdev",
2040 &portdev_fops);
2041 if (portdev->chr_major < 0) {
2042 dev_err(&vdev->dev,
2043 "Error %d registering chrdev for device %u\n",
2044 portdev->chr_major, vdev->index);
2045 err = portdev->chr_major;
2046 goto free;
2047 }
2048
2049 multiport = false;
2050 portdev->config.max_nr_ports = 1;
2051
2052
2053 if (!is_rproc_serial(vdev) &&
2054 virtio_cread_feature(vdev, VIRTIO_CONSOLE_F_MULTIPORT,
2055 struct virtio_console_config, max_nr_ports,
2056 &portdev->config.max_nr_ports) == 0) {
2057 multiport = true;
2058 }
2059
2060 err = init_vqs(portdev);
2061 if (err < 0) {
2062 dev_err(&vdev->dev, "Error %d initializing vqs\n", err);
2063 goto free_chrdev;
2064 }
2065
2066 spin_lock_init(&portdev->ports_lock);
2067 INIT_LIST_HEAD(&portdev->ports);
2068
2069 virtio_device_ready(portdev->vdev);
2070
2071 INIT_WORK(&portdev->config_work, &config_work_handler);
2072 INIT_WORK(&portdev->control_work, &control_work_handler);
2073
2074 if (multiport) {
2075 unsigned int nr_added_bufs;
2076
2077 spin_lock_init(&portdev->c_ivq_lock);
2078 spin_lock_init(&portdev->c_ovq_lock);
2079
2080 nr_added_bufs = fill_queue(portdev->c_ivq,
2081 &portdev->c_ivq_lock);
2082 if (!nr_added_bufs) {
2083 dev_err(&vdev->dev,
2084 "Error allocating buffers for control queue\n");
2085 err = -ENOMEM;
2086 goto free_vqs;
2087 }
2088 } else {
2089
2090
2091
2092
2093 add_port(portdev, 0);
2094 }
2095
2096 spin_lock_irq(&pdrvdata_lock);
2097 list_add_tail(&portdev->list, &pdrvdata.portdevs);
2098 spin_unlock_irq(&pdrvdata_lock);
2099
2100 __send_control_msg(portdev, VIRTIO_CONSOLE_BAD_ID,
2101 VIRTIO_CONSOLE_DEVICE_READY, 1);
2102
2103
2104
2105
2106
2107
2108
2109
2110
2111
2112 if (multiport && early)
2113 wait_for_completion(&early_console_added);
2114
2115 return 0;
2116
2117free_vqs:
2118
2119 __send_control_msg(portdev, VIRTIO_CONSOLE_BAD_ID,
2120 VIRTIO_CONSOLE_DEVICE_READY, 0);
2121 remove_vqs(portdev);
2122free_chrdev:
2123 unregister_chrdev(portdev->chr_major, "virtio-portsdev");
2124free:
2125 kfree(portdev);
2126fail:
2127 return err;
2128}
2129
2130static void virtcons_remove(struct virtio_device *vdev)
2131{
2132 struct ports_device *portdev;
2133 struct port *port, *port2;
2134
2135 portdev = vdev->priv;
2136
2137 spin_lock_irq(&pdrvdata_lock);
2138 list_del(&portdev->list);
2139 spin_unlock_irq(&pdrvdata_lock);
2140
2141
2142 vdev->config->reset(vdev);
2143
2144 if (use_multiport(portdev))
2145 cancel_work_sync(&portdev->control_work);
2146 else
2147 cancel_work_sync(&portdev->config_work);
2148
2149 list_for_each_entry_safe(port, port2, &portdev->ports, list)
2150 unplug_port(port);
2151
2152 unregister_chrdev(portdev->chr_major, "virtio-portsdev");
2153
2154
2155
2156
2157
2158
2159
2160
2161
2162 remove_controlq_data(portdev);
2163 remove_vqs(portdev);
2164 kfree(portdev);
2165}
2166
2167static struct virtio_device_id id_table[] = {
2168 { VIRTIO_ID_CONSOLE, VIRTIO_DEV_ANY_ID },
2169 { 0 },
2170};
2171
2172static unsigned int features[] = {
2173 VIRTIO_CONSOLE_F_SIZE,
2174 VIRTIO_CONSOLE_F_MULTIPORT,
2175};
2176
2177static struct virtio_device_id rproc_serial_id_table[] = {
2178#if IS_ENABLED(CONFIG_REMOTEPROC)
2179 { VIRTIO_ID_RPROC_SERIAL, VIRTIO_DEV_ANY_ID },
2180#endif
2181 { 0 },
2182};
2183
2184static unsigned int rproc_serial_features[] = {
2185};
2186
2187#ifdef CONFIG_PM_SLEEP
2188static int virtcons_freeze(struct virtio_device *vdev)
2189{
2190 struct ports_device *portdev;
2191 struct port *port;
2192
2193 portdev = vdev->priv;
2194
2195 vdev->config->reset(vdev);
2196
2197 virtqueue_disable_cb(portdev->c_ivq);
2198 cancel_work_sync(&portdev->control_work);
2199 cancel_work_sync(&portdev->config_work);
2200
2201
2202
2203
2204 virtqueue_disable_cb(portdev->c_ivq);
2205 remove_controlq_data(portdev);
2206
2207 list_for_each_entry(port, &portdev->ports, list) {
2208 virtqueue_disable_cb(port->in_vq);
2209 virtqueue_disable_cb(port->out_vq);
2210
2211
2212
2213
2214 port->host_connected = false;
2215 remove_port_data(port);
2216 }
2217 remove_vqs(portdev);
2218
2219 return 0;
2220}
2221
2222static int virtcons_restore(struct virtio_device *vdev)
2223{
2224 struct ports_device *portdev;
2225 struct port *port;
2226 int ret;
2227
2228 portdev = vdev->priv;
2229
2230 ret = init_vqs(portdev);
2231 if (ret)
2232 return ret;
2233
2234 virtio_device_ready(portdev->vdev);
2235
2236 if (use_multiport(portdev))
2237 fill_queue(portdev->c_ivq, &portdev->c_ivq_lock);
2238
2239 list_for_each_entry(port, &portdev->ports, list) {
2240 port->in_vq = portdev->in_vqs[port->id];
2241 port->out_vq = portdev->out_vqs[port->id];
2242
2243 fill_queue(port->in_vq, &port->inbuf_lock);
2244
2245
2246 send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
2247
2248
2249
2250
2251
2252 if (port->guest_connected)
2253 send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 1);
2254 }
2255 return 0;
2256}
2257#endif
2258
2259static struct virtio_driver virtio_console = {
2260 .feature_table = features,
2261 .feature_table_size = ARRAY_SIZE(features),
2262 .driver.name = KBUILD_MODNAME,
2263 .driver.owner = THIS_MODULE,
2264 .id_table = id_table,
2265 .probe = virtcons_probe,
2266 .remove = virtcons_remove,
2267 .config_changed = config_intr,
2268#ifdef CONFIG_PM_SLEEP
2269 .freeze = virtcons_freeze,
2270 .restore = virtcons_restore,
2271#endif
2272};
2273
2274static struct virtio_driver virtio_rproc_serial = {
2275 .feature_table = rproc_serial_features,
2276 .feature_table_size = ARRAY_SIZE(rproc_serial_features),
2277 .driver.name = "virtio_rproc_serial",
2278 .driver.owner = THIS_MODULE,
2279 .id_table = rproc_serial_id_table,
2280 .probe = virtcons_probe,
2281 .remove = virtcons_remove,
2282};
2283
2284static int __init init(void)
2285{
2286 int err;
2287
2288 pdrvdata.class = class_create(THIS_MODULE, "virtio-ports");
2289 if (IS_ERR(pdrvdata.class)) {
2290 err = PTR_ERR(pdrvdata.class);
2291 pr_err("Error %d creating virtio-ports class\n", err);
2292 return err;
2293 }
2294
2295 pdrvdata.debugfs_dir = debugfs_create_dir("virtio-ports", NULL);
2296 if (!pdrvdata.debugfs_dir)
2297 pr_warning("Error creating debugfs dir for virtio-ports\n");
2298 INIT_LIST_HEAD(&pdrvdata.consoles);
2299 INIT_LIST_HEAD(&pdrvdata.portdevs);
2300
2301 err = register_virtio_driver(&virtio_console);
2302 if (err < 0) {
2303 pr_err("Error %d registering virtio driver\n", err);
2304 goto free;
2305 }
2306 err = register_virtio_driver(&virtio_rproc_serial);
2307 if (err < 0) {
2308 pr_err("Error %d registering virtio rproc serial driver\n",
2309 err);
2310 goto unregister;
2311 }
2312 return 0;
2313unregister:
2314 unregister_virtio_driver(&virtio_console);
2315free:
2316 debugfs_remove_recursive(pdrvdata.debugfs_dir);
2317 class_destroy(pdrvdata.class);
2318 return err;
2319}
2320
2321static void __exit fini(void)
2322{
2323 reclaim_dma_bufs();
2324
2325 unregister_virtio_driver(&virtio_console);
2326 unregister_virtio_driver(&virtio_rproc_serial);
2327
2328 class_destroy(pdrvdata.class);
2329 debugfs_remove_recursive(pdrvdata.debugfs_dir);
2330}
2331module_init(init);
2332module_exit(fini);
2333
2334MODULE_DEVICE_TABLE(virtio, id_table);
2335MODULE_DESCRIPTION("Virtio console driver");
2336MODULE_LICENSE("GPL");
2337