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
26
27
28
29
30
31
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
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79#define HDLC_MAGIC 0x239e
80
81#include <linux/module.h>
82#include <linux/init.h>
83#include <linux/kernel.h>
84#include <linux/sched.h>
85#include <linux/types.h>
86#include <linux/fcntl.h>
87#include <linux/interrupt.h>
88#include <linux/ptrace.h>
89
90#include <linux/poll.h>
91#include <linux/in.h>
92#include <linux/ioctl.h>
93#include <linux/slab.h>
94#include <linux/tty.h>
95#include <linux/errno.h>
96#include <linux/string.h>
97#include <linux/signal.h>
98#include <linux/if.h>
99#include <linux/bitops.h>
100
101#include <asm/termios.h>
102#include <linux/uaccess.h>
103#include "tty.h"
104
105
106
107
108#define MAX_HDLC_FRAME_SIZE 65535
109#define DEFAULT_RX_BUF_COUNT 10
110#define MAX_RX_BUF_COUNT 60
111#define DEFAULT_TX_BUF_COUNT 3
112
113struct n_hdlc_buf {
114 struct list_head list_item;
115 int count;
116 char buf[];
117};
118
119struct n_hdlc_buf_list {
120 struct list_head list;
121 int count;
122 spinlock_t spinlock;
123};
124
125
126
127
128
129
130
131
132
133
134
135struct n_hdlc {
136 int magic;
137 bool tbusy;
138 bool woke_up;
139 struct n_hdlc_buf_list tx_buf_list;
140 struct n_hdlc_buf_list rx_buf_list;
141 struct n_hdlc_buf_list tx_free_buf_list;
142 struct n_hdlc_buf_list rx_free_buf_list;
143 struct work_struct write_work;
144 struct tty_struct *tty_for_write_work;
145};
146
147
148
149
150static void n_hdlc_buf_return(struct n_hdlc_buf_list *buf_list,
151 struct n_hdlc_buf *buf);
152static void n_hdlc_buf_put(struct n_hdlc_buf_list *list,
153 struct n_hdlc_buf *buf);
154static struct n_hdlc_buf *n_hdlc_buf_get(struct n_hdlc_buf_list *list);
155
156
157
158static struct n_hdlc *n_hdlc_alloc(void);
159static void n_hdlc_tty_write_work(struct work_struct *work);
160
161
162static int maxframe = 4096;
163
164static void flush_rx_queue(struct tty_struct *tty)
165{
166 struct n_hdlc *n_hdlc = tty->disc_data;
167 struct n_hdlc_buf *buf;
168
169 while ((buf = n_hdlc_buf_get(&n_hdlc->rx_buf_list)))
170 n_hdlc_buf_put(&n_hdlc->rx_free_buf_list, buf);
171}
172
173static void flush_tx_queue(struct tty_struct *tty)
174{
175 struct n_hdlc *n_hdlc = tty->disc_data;
176 struct n_hdlc_buf *buf;
177
178 while ((buf = n_hdlc_buf_get(&n_hdlc->tx_buf_list)))
179 n_hdlc_buf_put(&n_hdlc->tx_free_buf_list, buf);
180}
181
182static void n_hdlc_free_buf_list(struct n_hdlc_buf_list *list)
183{
184 struct n_hdlc_buf *buf;
185
186 do {
187 buf = n_hdlc_buf_get(list);
188 kfree(buf);
189 } while (buf);
190}
191
192
193
194
195
196
197
198
199static void n_hdlc_tty_close(struct tty_struct *tty)
200{
201 struct n_hdlc *n_hdlc = tty->disc_data;
202
203 if (n_hdlc->magic != HDLC_MAGIC) {
204 pr_warn("n_hdlc: trying to close unopened tty!\n");
205 return;
206 }
207#if defined(TTY_NO_WRITE_SPLIT)
208 clear_bit(TTY_NO_WRITE_SPLIT, &tty->flags);
209#endif
210 tty->disc_data = NULL;
211
212
213 wake_up_interruptible(&tty->read_wait);
214 wake_up_interruptible(&tty->write_wait);
215
216 cancel_work_sync(&n_hdlc->write_work);
217
218 n_hdlc_free_buf_list(&n_hdlc->rx_free_buf_list);
219 n_hdlc_free_buf_list(&n_hdlc->tx_free_buf_list);
220 n_hdlc_free_buf_list(&n_hdlc->rx_buf_list);
221 n_hdlc_free_buf_list(&n_hdlc->tx_buf_list);
222 kfree(n_hdlc);
223}
224
225
226
227
228
229
230
231static int n_hdlc_tty_open(struct tty_struct *tty)
232{
233 struct n_hdlc *n_hdlc = tty->disc_data;
234
235 pr_debug("%s() called (device=%s)\n", __func__, tty->name);
236
237
238 if (n_hdlc) {
239 pr_err("%s: tty already associated!\n", __func__);
240 return -EEXIST;
241 }
242
243 n_hdlc = n_hdlc_alloc();
244 if (!n_hdlc) {
245 pr_err("%s: n_hdlc_alloc failed\n", __func__);
246 return -ENFILE;
247 }
248
249 INIT_WORK(&n_hdlc->write_work, n_hdlc_tty_write_work);
250 n_hdlc->tty_for_write_work = tty;
251 tty->disc_data = n_hdlc;
252 tty->receive_room = 65536;
253
254
255 set_bit(TTY_NO_WRITE_SPLIT, &tty->flags);
256
257
258 tty_driver_flush_buffer(tty);
259
260 return 0;
261
262}
263
264
265
266
267
268
269
270
271
272
273static void n_hdlc_send_frames(struct n_hdlc *n_hdlc, struct tty_struct *tty)
274{
275 register int actual;
276 unsigned long flags;
277 struct n_hdlc_buf *tbuf;
278
279check_again:
280
281 spin_lock_irqsave(&n_hdlc->tx_buf_list.spinlock, flags);
282 if (n_hdlc->tbusy) {
283 n_hdlc->woke_up = true;
284 spin_unlock_irqrestore(&n_hdlc->tx_buf_list.spinlock, flags);
285 return;
286 }
287 n_hdlc->tbusy = true;
288 n_hdlc->woke_up = false;
289 spin_unlock_irqrestore(&n_hdlc->tx_buf_list.spinlock, flags);
290
291 tbuf = n_hdlc_buf_get(&n_hdlc->tx_buf_list);
292 while (tbuf) {
293 pr_debug("sending frame %p, count=%d\n", tbuf, tbuf->count);
294
295
296 set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
297 actual = tty->ops->write(tty, tbuf->buf, tbuf->count);
298
299
300 if (actual == -ERESTARTSYS) {
301 n_hdlc_buf_return(&n_hdlc->tx_buf_list, tbuf);
302 break;
303 }
304
305
306 if (actual < 0)
307 actual = tbuf->count;
308
309 if (actual == tbuf->count) {
310 pr_debug("frame %p completed\n", tbuf);
311
312
313 n_hdlc_buf_put(&n_hdlc->tx_free_buf_list, tbuf);
314
315
316 wake_up_interruptible(&tty->write_wait);
317
318
319 tbuf = n_hdlc_buf_get(&n_hdlc->tx_buf_list);
320 } else {
321 pr_debug("frame %p pending\n", tbuf);
322
323
324
325
326
327 n_hdlc_buf_return(&n_hdlc->tx_buf_list, tbuf);
328 break;
329 }
330 }
331
332 if (!tbuf)
333 clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
334
335
336 spin_lock_irqsave(&n_hdlc->tx_buf_list.spinlock, flags);
337 n_hdlc->tbusy = false;
338 spin_unlock_irqrestore(&n_hdlc->tx_buf_list.spinlock, flags);
339
340 if (n_hdlc->woke_up)
341 goto check_again;
342}
343
344
345
346
347
348
349
350static void n_hdlc_tty_write_work(struct work_struct *work)
351{
352 struct n_hdlc *n_hdlc = container_of(work, struct n_hdlc, write_work);
353 struct tty_struct *tty = n_hdlc->tty_for_write_work;
354
355 n_hdlc_send_frames(n_hdlc, tty);
356}
357
358
359
360
361
362
363
364static void n_hdlc_tty_wakeup(struct tty_struct *tty)
365{
366 struct n_hdlc *n_hdlc = tty->disc_data;
367
368 schedule_work(&n_hdlc->write_work);
369}
370
371
372
373
374
375
376
377
378
379
380
381static void n_hdlc_tty_receive(struct tty_struct *tty, const __u8 *data,
382 const char *flags, int count)
383{
384 register struct n_hdlc *n_hdlc = tty->disc_data;
385 register struct n_hdlc_buf *buf;
386
387 pr_debug("%s() called count=%d\n", __func__, count);
388
389
390 if (n_hdlc->magic != HDLC_MAGIC) {
391 pr_err("line not using HDLC discipline\n");
392 return;
393 }
394
395 if (count > maxframe) {
396 pr_debug("rx count>maxframesize, data discarded\n");
397 return;
398 }
399
400
401 buf = n_hdlc_buf_get(&n_hdlc->rx_free_buf_list);
402 if (!buf) {
403
404
405
406
407 if (n_hdlc->rx_buf_list.count < MAX_RX_BUF_COUNT)
408 buf = kmalloc(struct_size(buf, buf, maxframe),
409 GFP_ATOMIC);
410 }
411
412 if (!buf) {
413 pr_debug("no more rx buffers, data discarded\n");
414 return;
415 }
416
417
418 memcpy(buf->buf, data, count);
419 buf->count = count;
420
421
422 n_hdlc_buf_put(&n_hdlc->rx_buf_list, buf);
423
424
425 wake_up_interruptible(&tty->read_wait);
426 if (tty->fasync != NULL)
427 kill_fasync(&tty->fasync, SIGIO, POLL_IN);
428
429}
430
431
432
433
434
435
436
437
438
439
440
441
442static ssize_t n_hdlc_tty_read(struct tty_struct *tty, struct file *file,
443 __u8 *kbuf, size_t nr,
444 void **cookie, unsigned long offset)
445{
446 struct n_hdlc *n_hdlc = tty->disc_data;
447 int ret = 0;
448 struct n_hdlc_buf *rbuf;
449 DECLARE_WAITQUEUE(wait, current);
450
451
452 rbuf = *cookie;
453 if (rbuf)
454 goto have_rbuf;
455
456 add_wait_queue(&tty->read_wait, &wait);
457
458 for (;;) {
459 if (test_bit(TTY_OTHER_CLOSED, &tty->flags)) {
460 ret = -EIO;
461 break;
462 }
463 if (tty_hung_up_p(file))
464 break;
465
466 set_current_state(TASK_INTERRUPTIBLE);
467
468 rbuf = n_hdlc_buf_get(&n_hdlc->rx_buf_list);
469 if (rbuf)
470 break;
471
472
473 if (tty_io_nonblock(tty, file)) {
474 ret = -EAGAIN;
475 break;
476 }
477
478 schedule();
479
480 if (signal_pending(current)) {
481 ret = -EINTR;
482 break;
483 }
484 }
485
486 remove_wait_queue(&tty->read_wait, &wait);
487 __set_current_state(TASK_RUNNING);
488
489 if (!rbuf)
490 return ret;
491 *cookie = rbuf;
492
493have_rbuf:
494
495 if (offset >= rbuf->count)
496 goto done_with_rbuf;
497
498
499 ret = -EOVERFLOW;
500 if (!nr)
501 goto done_with_rbuf;
502
503
504 ret = rbuf->count - offset;
505 if (ret > nr)
506 ret = nr;
507 memcpy(kbuf, rbuf->buf+offset, ret);
508 offset += ret;
509
510
511 if (offset < rbuf->count)
512 return ret;
513
514done_with_rbuf:
515 *cookie = NULL;
516
517 if (n_hdlc->rx_free_buf_list.count > DEFAULT_RX_BUF_COUNT)
518 kfree(rbuf);
519 else
520 n_hdlc_buf_put(&n_hdlc->rx_free_buf_list, rbuf);
521
522 return ret;
523
524}
525
526
527
528
529
530
531
532
533
534
535static ssize_t n_hdlc_tty_write(struct tty_struct *tty, struct file *file,
536 const unsigned char *data, size_t count)
537{
538 struct n_hdlc *n_hdlc = tty->disc_data;
539 int error = 0;
540 DECLARE_WAITQUEUE(wait, current);
541 struct n_hdlc_buf *tbuf;
542
543 pr_debug("%s() called count=%zd\n", __func__, count);
544
545 if (n_hdlc->magic != HDLC_MAGIC)
546 return -EIO;
547
548
549 if (count > maxframe) {
550 pr_debug("%s: truncating user packet from %zu to %d\n",
551 __func__, count, maxframe);
552 count = maxframe;
553 }
554
555 add_wait_queue(&tty->write_wait, &wait);
556
557 for (;;) {
558 set_current_state(TASK_INTERRUPTIBLE);
559
560 tbuf = n_hdlc_buf_get(&n_hdlc->tx_free_buf_list);
561 if (tbuf)
562 break;
563
564 if (tty_io_nonblock(tty, file)) {
565 error = -EAGAIN;
566 break;
567 }
568 schedule();
569
570 if (signal_pending(current)) {
571 error = -EINTR;
572 break;
573 }
574 }
575
576 __set_current_state(TASK_RUNNING);
577 remove_wait_queue(&tty->write_wait, &wait);
578
579 if (!error) {
580
581 memcpy(tbuf->buf, data, count);
582
583
584 tbuf->count = error = count;
585 n_hdlc_buf_put(&n_hdlc->tx_buf_list, tbuf);
586 n_hdlc_send_frames(n_hdlc, tty);
587 }
588
589 return error;
590
591}
592
593
594
595
596
597
598
599
600
601static int n_hdlc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
602 unsigned long arg)
603{
604 struct n_hdlc *n_hdlc = tty->disc_data;
605 int error = 0;
606 int count;
607 unsigned long flags;
608 struct n_hdlc_buf *buf = NULL;
609
610 pr_debug("%s() called %d\n", __func__, cmd);
611
612
613 if (n_hdlc->magic != HDLC_MAGIC)
614 return -EBADF;
615
616 switch (cmd) {
617 case FIONREAD:
618
619
620 spin_lock_irqsave(&n_hdlc->rx_buf_list.spinlock, flags);
621 buf = list_first_entry_or_null(&n_hdlc->rx_buf_list.list,
622 struct n_hdlc_buf, list_item);
623 if (buf)
624 count = buf->count;
625 else
626 count = 0;
627 spin_unlock_irqrestore(&n_hdlc->rx_buf_list.spinlock, flags);
628 error = put_user(count, (int __user *)arg);
629 break;
630
631 case TIOCOUTQ:
632
633 count = tty_chars_in_buffer(tty);
634
635 spin_lock_irqsave(&n_hdlc->tx_buf_list.spinlock, flags);
636 buf = list_first_entry_or_null(&n_hdlc->tx_buf_list.list,
637 struct n_hdlc_buf, list_item);
638 if (buf)
639 count += buf->count;
640 spin_unlock_irqrestore(&n_hdlc->tx_buf_list.spinlock, flags);
641 error = put_user(count, (int __user *)arg);
642 break;
643
644 case TCFLSH:
645 switch (arg) {
646 case TCIOFLUSH:
647 case TCOFLUSH:
648 flush_tx_queue(tty);
649 }
650 fallthrough;
651
652 default:
653 error = n_tty_ioctl_helper(tty, cmd, arg);
654 break;
655 }
656 return error;
657
658}
659
660
661
662
663
664
665
666
667
668
669
670static __poll_t n_hdlc_tty_poll(struct tty_struct *tty, struct file *filp,
671 poll_table *wait)
672{
673 struct n_hdlc *n_hdlc = tty->disc_data;
674 __poll_t mask = 0;
675
676 if (n_hdlc->magic != HDLC_MAGIC)
677 return 0;
678
679
680
681
682
683 poll_wait(filp, &tty->read_wait, wait);
684 poll_wait(filp, &tty->write_wait, wait);
685
686
687 if (!list_empty(&n_hdlc->rx_buf_list.list))
688 mask |= EPOLLIN | EPOLLRDNORM;
689 if (test_bit(TTY_OTHER_CLOSED, &tty->flags))
690 mask |= EPOLLHUP;
691 if (tty_hung_up_p(filp))
692 mask |= EPOLLHUP;
693 if (!tty_is_writelocked(tty) &&
694 !list_empty(&n_hdlc->tx_free_buf_list.list))
695 mask |= EPOLLOUT | EPOLLWRNORM;
696
697 return mask;
698}
699
700static void n_hdlc_alloc_buf(struct n_hdlc_buf_list *list, unsigned int count,
701 const char *name)
702{
703 struct n_hdlc_buf *buf;
704 unsigned int i;
705
706 for (i = 0; i < count; i++) {
707 buf = kmalloc(struct_size(buf, buf, maxframe), GFP_KERNEL);
708 if (!buf) {
709 pr_debug("%s(), kmalloc() failed for %s buffer %u\n",
710 __func__, name, i);
711 return;
712 }
713 n_hdlc_buf_put(list, buf);
714 }
715}
716
717
718
719
720
721
722static struct n_hdlc *n_hdlc_alloc(void)
723{
724 struct n_hdlc *n_hdlc = kzalloc(sizeof(*n_hdlc), GFP_KERNEL);
725
726 if (!n_hdlc)
727 return NULL;
728
729 spin_lock_init(&n_hdlc->rx_free_buf_list.spinlock);
730 spin_lock_init(&n_hdlc->tx_free_buf_list.spinlock);
731 spin_lock_init(&n_hdlc->rx_buf_list.spinlock);
732 spin_lock_init(&n_hdlc->tx_buf_list.spinlock);
733
734 INIT_LIST_HEAD(&n_hdlc->rx_free_buf_list.list);
735 INIT_LIST_HEAD(&n_hdlc->tx_free_buf_list.list);
736 INIT_LIST_HEAD(&n_hdlc->rx_buf_list.list);
737 INIT_LIST_HEAD(&n_hdlc->tx_buf_list.list);
738
739 n_hdlc_alloc_buf(&n_hdlc->rx_free_buf_list, DEFAULT_RX_BUF_COUNT, "rx");
740 n_hdlc_alloc_buf(&n_hdlc->tx_free_buf_list, DEFAULT_TX_BUF_COUNT, "tx");
741
742
743 n_hdlc->magic = HDLC_MAGIC;
744
745 return n_hdlc;
746
747}
748
749
750
751
752
753
754static void n_hdlc_buf_return(struct n_hdlc_buf_list *buf_list,
755 struct n_hdlc_buf *buf)
756{
757 unsigned long flags;
758
759 spin_lock_irqsave(&buf_list->spinlock, flags);
760
761 list_add(&buf->list_item, &buf_list->list);
762 buf_list->count++;
763
764 spin_unlock_irqrestore(&buf_list->spinlock, flags);
765}
766
767
768
769
770
771
772static void n_hdlc_buf_put(struct n_hdlc_buf_list *buf_list,
773 struct n_hdlc_buf *buf)
774{
775 unsigned long flags;
776
777 spin_lock_irqsave(&buf_list->spinlock, flags);
778
779 list_add_tail(&buf->list_item, &buf_list->list);
780 buf_list->count++;
781
782 spin_unlock_irqrestore(&buf_list->spinlock, flags);
783}
784
785
786
787
788
789
790
791
792
793static struct n_hdlc_buf *n_hdlc_buf_get(struct n_hdlc_buf_list *buf_list)
794{
795 unsigned long flags;
796 struct n_hdlc_buf *buf;
797
798 spin_lock_irqsave(&buf_list->spinlock, flags);
799
800 buf = list_first_entry_or_null(&buf_list->list,
801 struct n_hdlc_buf, list_item);
802 if (buf) {
803 list_del(&buf->list_item);
804 buf_list->count--;
805 }
806
807 spin_unlock_irqrestore(&buf_list->spinlock, flags);
808 return buf;
809}
810
811static struct tty_ldisc_ops n_hdlc_ldisc = {
812 .owner = THIS_MODULE,
813 .num = N_HDLC,
814 .name = "hdlc",
815 .open = n_hdlc_tty_open,
816 .close = n_hdlc_tty_close,
817 .read = n_hdlc_tty_read,
818 .write = n_hdlc_tty_write,
819 .ioctl = n_hdlc_tty_ioctl,
820 .poll = n_hdlc_tty_poll,
821 .receive_buf = n_hdlc_tty_receive,
822 .write_wakeup = n_hdlc_tty_wakeup,
823 .flush_buffer = flush_rx_queue,
824};
825
826static int __init n_hdlc_init(void)
827{
828 int status;
829
830
831 maxframe = clamp(maxframe, 4096, MAX_HDLC_FRAME_SIZE);
832
833 status = tty_register_ldisc(&n_hdlc_ldisc);
834 if (!status)
835 pr_info("N_HDLC line discipline registered with maxframe=%d\n",
836 maxframe);
837 else
838 pr_err("N_HDLC: error registering line discipline: %d\n",
839 status);
840
841 return status;
842
843}
844
845static void __exit n_hdlc_exit(void)
846{
847 tty_unregister_ldisc(&n_hdlc_ldisc);
848}
849
850module_init(n_hdlc_init);
851module_exit(n_hdlc_exit);
852
853MODULE_LICENSE("GPL");
854MODULE_AUTHOR("Paul Fulghum paulkf@microgate.com");
855module_param(maxframe, int, 0);
856MODULE_ALIAS_LDISC(N_HDLC);
857