1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25#include <linux/console.h>
26#include <linux/cpumask.h>
27#include <linux/init.h>
28#include <linux/kbd_kern.h>
29#include <linux/kernel.h>
30#include <linux/kthread.h>
31#include <linux/list.h>
32#include <linux/module.h>
33#include <linux/major.h>
34#include <linux/sysrq.h>
35#include <linux/tty.h>
36#include <linux/tty_flip.h>
37#include <linux/sched.h>
38#include <linux/spinlock.h>
39#include <linux/delay.h>
40#include <linux/freezer.h>
41
42#include <asm/uaccess.h>
43
44#include "hvc_console.h"
45
46#define HVC_MAJOR 229
47#define HVC_MINOR 0
48
49
50
51
52
53#define HVC_CLOSE_WAIT (HZ/100)
54
55
56
57
58
59
60#define N_OUTBUF 16
61#define N_INBUF 16
62
63#define __ALIGNED__ __attribute__((__aligned__(sizeof(long))))
64
65static struct tty_driver *hvc_driver;
66static struct task_struct *hvc_task;
67
68
69static int hvc_kicked;
70
71static int hvc_init(void);
72
73#ifdef CONFIG_MAGIC_SYSRQ
74static int sysrq_pressed;
75#endif
76
77
78static LIST_HEAD(hvc_structs);
79
80
81
82
83
84static DEFINE_SPINLOCK(hvc_structs_lock);
85
86
87
88
89
90
91static int last_hvc = -1;
92
93
94
95
96
97
98static struct hvc_struct *hvc_get_by_index(int index)
99{
100 struct hvc_struct *hp;
101 unsigned long flags;
102
103 spin_lock(&hvc_structs_lock);
104
105 list_for_each_entry(hp, &hvc_structs, next) {
106 spin_lock_irqsave(&hp->lock, flags);
107 if (hp->index == index) {
108 kref_get(&hp->kref);
109 spin_unlock_irqrestore(&hp->lock, flags);
110 spin_unlock(&hvc_structs_lock);
111 return hp;
112 }
113 spin_unlock_irqrestore(&hp->lock, flags);
114 }
115 hp = NULL;
116
117 spin_unlock(&hvc_structs_lock);
118 return hp;
119}
120
121
122
123
124
125
126
127
128static struct hv_ops *cons_ops[MAX_NR_HVC_CONSOLES];
129static uint32_t vtermnos[MAX_NR_HVC_CONSOLES] =
130 {[0 ... MAX_NR_HVC_CONSOLES - 1] = -1};
131
132
133
134
135
136
137static void hvc_console_print(struct console *co, const char *b,
138 unsigned count)
139{
140 char c[N_OUTBUF] __ALIGNED__;
141 unsigned i = 0, n = 0;
142 int r, donecr = 0, index = co->index;
143
144
145 if (index >= MAX_NR_HVC_CONSOLES)
146 return;
147
148
149 if (vtermnos[index] < 0)
150 return;
151
152 while (count > 0 || i > 0) {
153 if (count > 0 && i < sizeof(c)) {
154 if (b[n] == '\n' && !donecr) {
155 c[i++] = '\r';
156 donecr = 1;
157 } else {
158 c[i++] = b[n++];
159 donecr = 0;
160 --count;
161 }
162 } else {
163 r = cons_ops[index]->put_chars(vtermnos[index], c, i);
164 if (r <= 0) {
165
166 i = 0;
167 } else if (r > 0) {
168 i -= r;
169 if (i > 0)
170 memmove(c, c+r, i);
171 }
172 }
173 }
174}
175
176static struct tty_driver *hvc_console_device(struct console *c, int *index)
177{
178 if (vtermnos[c->index] == -1)
179 return NULL;
180
181 *index = c->index;
182 return hvc_driver;
183}
184
185static int __init hvc_console_setup(struct console *co, char *options)
186{
187 if (co->index < 0 || co->index >= MAX_NR_HVC_CONSOLES)
188 return -ENODEV;
189
190 if (vtermnos[co->index] == -1)
191 return -ENODEV;
192
193 return 0;
194}
195
196static struct console hvc_con_driver = {
197 .name = "hvc",
198 .write = hvc_console_print,
199 .device = hvc_console_device,
200 .setup = hvc_console_setup,
201 .flags = CON_PRINTBUFFER,
202 .index = -1,
203};
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220static int __init hvc_console_init(void)
221{
222 register_console(&hvc_con_driver);
223 return 0;
224}
225console_initcall(hvc_console_init);
226
227
228static void destroy_hvc_struct(struct kref *kref)
229{
230 struct hvc_struct *hp = container_of(kref, struct hvc_struct, kref);
231 unsigned long flags;
232
233 spin_lock(&hvc_structs_lock);
234
235 spin_lock_irqsave(&hp->lock, flags);
236 list_del(&(hp->next));
237 spin_unlock_irqrestore(&hp->lock, flags);
238
239 spin_unlock(&hvc_structs_lock);
240
241 kfree(hp);
242}
243
244
245
246
247
248
249
250int hvc_instantiate(uint32_t vtermno, int index, struct hv_ops *ops)
251{
252 struct hvc_struct *hp;
253
254 if (index < 0 || index >= MAX_NR_HVC_CONSOLES)
255 return -1;
256
257 if (vtermnos[index] != -1)
258 return -1;
259
260
261 hp = hvc_get_by_index(index);
262 if (hp) {
263 kref_put(&hp->kref, destroy_hvc_struct);
264 return -1;
265 }
266
267 vtermnos[index] = vtermno;
268 cons_ops[index] = ops;
269
270
271 if (last_hvc < index)
272 last_hvc = index;
273
274
275
276
277
278 if (index == hvc_con_driver.index)
279 register_console(&hvc_con_driver);
280
281 return 0;
282}
283EXPORT_SYMBOL_GPL(hvc_instantiate);
284
285
286void hvc_kick(void)
287{
288 hvc_kicked = 1;
289 wake_up_process(hvc_task);
290}
291EXPORT_SYMBOL_GPL(hvc_kick);
292
293static void hvc_unthrottle(struct tty_struct *tty)
294{
295 hvc_kick();
296}
297
298
299
300
301
302static int hvc_open(struct tty_struct *tty, struct file * filp)
303{
304 struct hvc_struct *hp;
305 unsigned long flags;
306 int rc = 0;
307
308
309 if (!(hp = hvc_get_by_index(tty->index)))
310 return -ENODEV;
311
312 spin_lock_irqsave(&hp->lock, flags);
313
314 if (hp->count++ > 0) {
315 spin_unlock_irqrestore(&hp->lock, flags);
316 hvc_kick();
317 return 0;
318 }
319
320 tty->driver_data = hp;
321
322 hp->tty = tty;
323
324 spin_unlock_irqrestore(&hp->lock, flags);
325
326 if (hp->ops->notifier_add)
327 rc = hp->ops->notifier_add(hp, hp->data);
328
329
330
331
332
333
334
335 if (rc) {
336 spin_lock_irqsave(&hp->lock, flags);
337 hp->tty = NULL;
338 spin_unlock_irqrestore(&hp->lock, flags);
339 tty->driver_data = NULL;
340 kref_put(&hp->kref, destroy_hvc_struct);
341 printk(KERN_ERR "hvc_open: request_irq failed with rc %d.\n", rc);
342 }
343
344 hvc_kick();
345
346 return rc;
347}
348
349static void hvc_close(struct tty_struct *tty, struct file * filp)
350{
351 struct hvc_struct *hp;
352 unsigned long flags;
353
354 if (tty_hung_up_p(filp))
355 return;
356
357
358
359
360
361
362 if (!tty->driver_data)
363 return;
364
365 hp = tty->driver_data;
366 spin_lock_irqsave(&hp->lock, flags);
367
368 if (--hp->count == 0) {
369
370 hp->tty = NULL;
371 spin_unlock_irqrestore(&hp->lock, flags);
372
373 if (hp->ops->notifier_del)
374 hp->ops->notifier_del(hp, hp->data);
375
376
377 cancel_work_sync(&hp->tty_resize);
378
379
380
381
382
383
384 tty_wait_until_sent(tty, HVC_CLOSE_WAIT);
385 } else {
386 if (hp->count < 0)
387 printk(KERN_ERR "hvc_close %X: oops, count is %d\n",
388 hp->vtermno, hp->count);
389 spin_unlock_irqrestore(&hp->lock, flags);
390 }
391
392 kref_put(&hp->kref, destroy_hvc_struct);
393}
394
395static void hvc_hangup(struct tty_struct *tty)
396{
397 struct hvc_struct *hp = tty->driver_data;
398 unsigned long flags;
399 int temp_open_count;
400
401 if (!hp)
402 return;
403
404
405 cancel_work_sync(&hp->tty_resize);
406
407 spin_lock_irqsave(&hp->lock, flags);
408
409
410
411
412
413
414 if (hp->count <= 0) {
415 spin_unlock_irqrestore(&hp->lock, flags);
416 return;
417 }
418
419 temp_open_count = hp->count;
420 hp->count = 0;
421 hp->n_outbuf = 0;
422 hp->tty = NULL;
423
424 spin_unlock_irqrestore(&hp->lock, flags);
425
426 if (hp->ops->notifier_hangup)
427 hp->ops->notifier_hangup(hp, hp->data);
428
429 while(temp_open_count) {
430 --temp_open_count;
431 kref_put(&hp->kref, destroy_hvc_struct);
432 }
433}
434
435
436
437
438
439static int hvc_push(struct hvc_struct *hp)
440{
441 int n;
442
443 n = hp->ops->put_chars(hp->vtermno, hp->outbuf, hp->n_outbuf);
444 if (n <= 0) {
445 if (n == 0) {
446 hp->do_wakeup = 1;
447 return 0;
448 }
449
450
451 hp->n_outbuf = 0;
452 } else
453 hp->n_outbuf -= n;
454 if (hp->n_outbuf > 0)
455 memmove(hp->outbuf, hp->outbuf + n, hp->n_outbuf);
456 else
457 hp->do_wakeup = 1;
458
459 return n;
460}
461
462static int hvc_write(struct tty_struct *tty, const unsigned char *buf, int count)
463{
464 struct hvc_struct *hp = tty->driver_data;
465 unsigned long flags;
466 int rsize, written = 0;
467
468
469 if (!hp)
470 return -EPIPE;
471
472 if (hp->count <= 0)
473 return -EIO;
474
475 spin_lock_irqsave(&hp->lock, flags);
476
477
478 if (hp->n_outbuf > 0)
479 hvc_push(hp);
480
481 while (count > 0 && (rsize = hp->outbuf_size - hp->n_outbuf) > 0) {
482 if (rsize > count)
483 rsize = count;
484 memcpy(hp->outbuf + hp->n_outbuf, buf, rsize);
485 count -= rsize;
486 buf += rsize;
487 hp->n_outbuf += rsize;
488 written += rsize;
489 hvc_push(hp);
490 }
491 spin_unlock_irqrestore(&hp->lock, flags);
492
493
494
495
496 if (hp->n_outbuf)
497 hvc_kick();
498
499 return written;
500}
501
502
503
504
505
506
507
508
509
510
511static void hvc_set_winsz(struct work_struct *work)
512{
513 struct hvc_struct *hp;
514 unsigned long hvc_flags;
515 struct tty_struct *tty;
516 struct winsize ws;
517
518 hp = container_of(work, struct hvc_struct, tty_resize);
519
520 spin_lock_irqsave(&hp->lock, hvc_flags);
521 if (!hp->tty) {
522 spin_unlock_irqrestore(&hp->lock, hvc_flags);
523 return;
524 }
525 ws = hp->ws;
526 tty = tty_kref_get(hp->tty);
527 spin_unlock_irqrestore(&hp->lock, hvc_flags);
528
529 tty_do_resize(tty, &ws);
530 tty_kref_put(tty);
531}
532
533
534
535
536
537
538static int hvc_write_room(struct tty_struct *tty)
539{
540 struct hvc_struct *hp = tty->driver_data;
541
542 if (!hp)
543 return -1;
544
545 return hp->outbuf_size - hp->n_outbuf;
546}
547
548static int hvc_chars_in_buffer(struct tty_struct *tty)
549{
550 struct hvc_struct *hp = tty->driver_data;
551
552 if (!hp)
553 return 0;
554 return hp->n_outbuf;
555}
556
557
558
559
560
561
562
563
564
565#define MIN_TIMEOUT (10)
566#define MAX_TIMEOUT (2000)
567static u32 timeout = MIN_TIMEOUT;
568
569#define HVC_POLL_READ 0x00000001
570#define HVC_POLL_WRITE 0x00000002
571
572int hvc_poll(struct hvc_struct *hp)
573{
574 struct tty_struct *tty;
575 int i, n, poll_mask = 0;
576 char buf[N_INBUF] __ALIGNED__;
577 unsigned long flags;
578 int read_total = 0;
579 int written_total = 0;
580
581 spin_lock_irqsave(&hp->lock, flags);
582
583
584 if (hp->n_outbuf > 0)
585 written_total = hvc_push(hp);
586
587
588 if (hp->n_outbuf > 0) {
589 poll_mask |= HVC_POLL_WRITE;
590
591 timeout = (written_total) ? 0 : MIN_TIMEOUT;
592 }
593
594
595 tty = hp->tty;
596 if (tty == NULL)
597 goto bail;
598
599
600 if (test_bit(TTY_THROTTLED, &tty->flags))
601 goto throttled;
602
603
604
605
606 if (!hp->irq_requested)
607 poll_mask |= HVC_POLL_READ;
608
609
610 for (;;) {
611 int count = tty_buffer_request_room(tty, N_INBUF);
612
613
614 if (count == 0) {
615 poll_mask |= HVC_POLL_READ;
616 break;
617 }
618
619 n = hp->ops->get_chars(hp->vtermno, buf, count);
620 if (n <= 0) {
621
622 if (n == -EPIPE) {
623 spin_unlock_irqrestore(&hp->lock, flags);
624 tty_hangup(tty);
625 spin_lock_irqsave(&hp->lock, flags);
626 } else if ( n == -EAGAIN ) {
627
628
629
630
631
632 poll_mask |= HVC_POLL_READ;
633 }
634 break;
635 }
636 for (i = 0; i < n; ++i) {
637#ifdef CONFIG_MAGIC_SYSRQ
638 if (hp->index == hvc_con_driver.index) {
639
640
641 if (buf[i] == '\x0f') {
642
643
644 sysrq_pressed = !sysrq_pressed;
645 if (sysrq_pressed)
646 continue;
647 } else if (sysrq_pressed) {
648 handle_sysrq(buf[i], tty);
649 sysrq_pressed = 0;
650 continue;
651 }
652 }
653#endif
654 tty_insert_flip_char(tty, buf[i], 0);
655 }
656
657 read_total += n;
658 }
659 throttled:
660
661 if (hp->do_wakeup) {
662 hp->do_wakeup = 0;
663 tty_wakeup(tty);
664 }
665 bail:
666 spin_unlock_irqrestore(&hp->lock, flags);
667
668 if (read_total) {
669
670
671 timeout = MIN_TIMEOUT;
672
673 tty_flip_buffer_push(tty);
674 }
675
676 return poll_mask;
677}
678EXPORT_SYMBOL_GPL(hvc_poll);
679
680
681
682
683
684
685
686
687
688
689
690void __hvc_resize(struct hvc_struct *hp, struct winsize ws)
691{
692 hp->ws = ws;
693 schedule_work(&hp->tty_resize);
694}
695EXPORT_SYMBOL_GPL(__hvc_resize);
696
697
698
699
700
701
702static int khvcd(void *unused)
703{
704 int poll_mask;
705 struct hvc_struct *hp;
706
707 set_freezable();
708 __set_current_state(TASK_RUNNING);
709 do {
710 poll_mask = 0;
711 hvc_kicked = 0;
712 try_to_freeze();
713 wmb();
714 if (!cpus_are_in_xmon()) {
715 spin_lock(&hvc_structs_lock);
716 list_for_each_entry(hp, &hvc_structs, next) {
717 poll_mask |= hvc_poll(hp);
718 }
719 spin_unlock(&hvc_structs_lock);
720 } else
721 poll_mask |= HVC_POLL_READ;
722 if (hvc_kicked)
723 continue;
724 set_current_state(TASK_INTERRUPTIBLE);
725 if (!hvc_kicked) {
726 if (poll_mask == 0)
727 schedule();
728 else {
729 if (timeout < MAX_TIMEOUT)
730 timeout += (timeout >> 6) + 1;
731
732 msleep_interruptible(timeout);
733 }
734 }
735 __set_current_state(TASK_RUNNING);
736 } while (!kthread_should_stop());
737
738 return 0;
739}
740
741static const struct tty_operations hvc_ops = {
742 .open = hvc_open,
743 .close = hvc_close,
744 .write = hvc_write,
745 .hangup = hvc_hangup,
746 .unthrottle = hvc_unthrottle,
747 .write_room = hvc_write_room,
748 .chars_in_buffer = hvc_chars_in_buffer,
749};
750
751struct hvc_struct __devinit *hvc_alloc(uint32_t vtermno, int data,
752 struct hv_ops *ops, int outbuf_size)
753{
754 struct hvc_struct *hp;
755 int i;
756
757
758 if (!hvc_driver) {
759 int err = hvc_init();
760 if (err)
761 return ERR_PTR(err);
762 }
763
764 hp = kzalloc(ALIGN(sizeof(*hp), sizeof(long)) + outbuf_size,
765 GFP_KERNEL);
766 if (!hp)
767 return ERR_PTR(-ENOMEM);
768
769 hp->vtermno = vtermno;
770 hp->data = data;
771 hp->ops = ops;
772 hp->outbuf_size = outbuf_size;
773 hp->outbuf = &((char *)hp)[ALIGN(sizeof(*hp), sizeof(long))];
774
775 kref_init(&hp->kref);
776
777 INIT_WORK(&hp->tty_resize, hvc_set_winsz);
778 spin_lock_init(&hp->lock);
779 spin_lock(&hvc_structs_lock);
780
781
782
783
784
785 for (i=0; i < MAX_NR_HVC_CONSOLES; i++)
786 if (vtermnos[i] == hp->vtermno &&
787 cons_ops[i] == hp->ops)
788 break;
789
790
791 if (i >= MAX_NR_HVC_CONSOLES)
792 i = ++last_hvc;
793
794 hp->index = i;
795
796 list_add_tail(&(hp->next), &hvc_structs);
797 spin_unlock(&hvc_structs_lock);
798
799 return hp;
800}
801EXPORT_SYMBOL_GPL(hvc_alloc);
802
803int hvc_remove(struct hvc_struct *hp)
804{
805 unsigned long flags;
806 struct tty_struct *tty;
807
808 spin_lock_irqsave(&hp->lock, flags);
809 tty = hp->tty;
810
811 if (hp->index < MAX_NR_HVC_CONSOLES)
812 vtermnos[hp->index] = -1;
813
814
815
816 spin_unlock_irqrestore(&hp->lock, flags);
817
818
819
820
821
822
823
824 kref_put(&hp->kref, destroy_hvc_struct);
825
826
827
828
829
830
831 if (tty)
832 tty_hangup(tty);
833 return 0;
834}
835
836
837static int hvc_init(void)
838{
839 struct tty_driver *drv;
840 int err;
841
842
843 drv = alloc_tty_driver(HVC_ALLOC_TTY_ADAPTERS);
844 if (!drv) {
845 err = -ENOMEM;
846 goto out;
847 }
848
849 drv->owner = THIS_MODULE;
850 drv->driver_name = "hvc";
851 drv->name = "hvc";
852 drv->major = HVC_MAJOR;
853 drv->minor_start = HVC_MINOR;
854 drv->type = TTY_DRIVER_TYPE_SYSTEM;
855 drv->init_termios = tty_std_termios;
856 drv->flags = TTY_DRIVER_REAL_RAW | TTY_DRIVER_RESET_TERMIOS;
857 tty_set_operations(drv, &hvc_ops);
858
859
860
861 hvc_task = kthread_run(khvcd, NULL, "khvcd");
862 if (IS_ERR(hvc_task)) {
863 printk(KERN_ERR "Couldn't create kthread for console.\n");
864 err = PTR_ERR(hvc_task);
865 goto put_tty;
866 }
867
868 err = tty_register_driver(drv);
869 if (err) {
870 printk(KERN_ERR "Couldn't register hvc console driver\n");
871 goto stop_thread;
872 }
873
874
875
876
877
878 smp_mb();
879 hvc_driver = drv;
880 return 0;
881
882stop_thread:
883 kthread_stop(hvc_task);
884 hvc_task = NULL;
885put_tty:
886 put_tty_driver(drv);
887out:
888 return err;
889}
890
891
892
893
894static void __exit hvc_exit(void)
895{
896 if (hvc_driver) {
897 kthread_stop(hvc_task);
898
899 tty_unregister_driver(hvc_driver);
900
901 put_tty_driver(hvc_driver);
902 unregister_console(&hvc_con_driver);
903 }
904}
905module_exit(hvc_exit);
906