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/types.h>
26#include <linux/major.h>
27#include <linux/errno.h>
28#include <linux/signal.h>
29#include <linux/fcntl.h>
30#include <linux/sched/signal.h>
31#include <linux/interrupt.h>
32#include <linux/tty.h>
33#include <linux/ctype.h>
34#include <linux/mm.h>
35#include <linux/string.h>
36#include <linux/slab.h>
37#include <linux/poll.h>
38#include <linux/bitops.h>
39#include <linux/file.h>
40#include <linux/uaccess.h>
41#include <linux/module.h>
42#include <linux/timer.h>
43#include <linux/tty_flip.h>
44#include <linux/tty_driver.h>
45#include <linux/serial.h>
46#include <linux/kfifo.h>
47#include <linux/skbuff.h>
48#include <net/arp.h>
49#include <linux/ip.h>
50#include <linux/netdevice.h>
51#include <linux/etherdevice.h>
52#include <linux/gsmmux.h>
53#include "tty.h"
54
55static int debug;
56module_param(debug, int, 0600);
57
58
59
60#define T1 10
61#define T2 34
62#define N2 3
63
64
65#ifdef DEBUG_TIMING
66#define T1 100
67#define T2 200
68#endif
69
70
71
72
73
74#define MAX_MRU 1500
75#define MAX_MTU 1500
76#define GSM_NET_TX_TIMEOUT (HZ*10)
77
78
79
80
81
82
83struct gsm_mux_net {
84 struct kref ref;
85 struct gsm_dlci *dlci;
86};
87
88
89
90
91
92
93
94struct gsm_msg {
95 struct list_head list;
96 u8 addr;
97 u8 ctrl;
98 unsigned int len;
99 unsigned char *data;
100 unsigned char buffer[];
101};
102
103enum gsm_dlci_state {
104 DLCI_CLOSED,
105 DLCI_OPENING,
106 DLCI_OPEN,
107 DLCI_CLOSING,
108};
109
110enum gsm_dlci_mode {
111 DLCI_MODE_ABM,
112 DLCI_MODE_ADM,
113};
114
115
116
117
118
119
120
121
122
123
124
125struct gsm_dlci {
126 struct gsm_mux *gsm;
127 int addr;
128 enum gsm_dlci_state state;
129 struct mutex mutex;
130
131
132 enum gsm_dlci_mode mode;
133 spinlock_t lock;
134 struct timer_list t1;
135 int retries;
136
137 struct tty_port port;
138 struct kfifo fifo;
139 int adaption;
140 int prev_adaption;
141 u32 modem_rx;
142 u32 modem_tx;
143 bool dead;
144
145 bool throttled;
146 bool constipated;
147
148 struct sk_buff *skb;
149 struct sk_buff_head skb_list;
150
151 void (*data)(struct gsm_dlci *dlci, const u8 *data, int len);
152 void (*prev_data)(struct gsm_dlci *dlci, const u8 *data, int len);
153 struct net_device *net;
154};
155
156
157
158#define NUM_DLCI 64
159
160
161
162
163
164
165
166
167
168struct gsm_control {
169 u8 cmd;
170 u8 *data;
171 int len;
172 int done;
173 int error;
174};
175
176enum gsm_mux_state {
177 GSM_SEARCH,
178 GSM_START,
179 GSM_ADDRESS,
180 GSM_CONTROL,
181 GSM_LEN,
182 GSM_DATA,
183 GSM_FCS,
184 GSM_OVERRUN,
185 GSM_LEN0,
186 GSM_LEN1,
187 GSM_SSOF,
188};
189
190
191
192
193
194
195
196
197
198
199
200struct gsm_mux {
201 struct tty_struct *tty;
202 spinlock_t lock;
203 struct mutex mutex;
204 unsigned int num;
205 struct kref ref;
206
207
208 wait_queue_head_t event;
209
210
211
212
213 unsigned char *buf;
214 enum gsm_mux_state state;
215 unsigned int len;
216 unsigned int address;
217 unsigned int count;
218 bool escape;
219 int encoding;
220 u8 control;
221 u8 fcs;
222 u8 received_fcs;
223 u8 *txframe;
224
225
226 void (*receive)(struct gsm_mux *gsm, u8 ch);
227
228
229 unsigned int mru;
230 unsigned int mtu;
231 int initiator;
232 bool dead;
233 struct gsm_dlci *dlci[NUM_DLCI];
234 bool constipated;
235
236 spinlock_t tx_lock;
237 unsigned int tx_bytes;
238#define TX_THRESH_HI 8192
239#define TX_THRESH_LO 2048
240 struct list_head tx_list;
241
242
243 struct timer_list t2_timer;
244 int cretries;
245 struct gsm_control *pending_cmd;
246 spinlock_t control_lock;
247
248
249 int adaption;
250 u8 ftype;
251 int t1, t2;
252 int n2;
253
254
255 unsigned long bad_fcs;
256 unsigned long malformed;
257 unsigned long io_error;
258 unsigned long bad_size;
259 unsigned long unsupported;
260};
261
262
263
264
265
266
267
268#define MAX_MUX 4
269static struct gsm_mux *gsm_mux[MAX_MUX];
270static DEFINE_SPINLOCK(gsm_mux_lock);
271
272static struct tty_driver *gsm_tty_driver;
273
274
275
276
277
278
279
280#define CR 0x02
281#define EA 0x01
282#define PF 0x10
283
284
285#define RR 0x01
286#define UI 0x03
287#define RNR 0x05
288#define REJ 0x09
289#define DM 0x0F
290#define SABM 0x2F
291#define DISC 0x43
292#define UA 0x63
293#define UIH 0xEF
294
295
296#define CMD_NSC 0x09
297#define CMD_TEST 0x11
298#define CMD_PSC 0x21
299#define CMD_RLS 0x29
300#define CMD_FCOFF 0x31
301#define CMD_PN 0x41
302#define CMD_RPN 0x49
303#define CMD_FCON 0x51
304#define CMD_CLD 0x61
305#define CMD_SNC 0x69
306#define CMD_MSC 0x71
307
308
309#define MDM_FC 0x01
310#define MDM_RTC 0x02
311#define MDM_RTR 0x04
312#define MDM_IC 0x20
313#define MDM_DV 0x40
314
315#define GSM0_SOF 0xF9
316#define GSM1_SOF 0x7E
317#define GSM1_ESCAPE 0x7D
318#define GSM1_ESCAPE_BITS 0x20
319#define XON 0x11
320#define XOFF 0x13
321
322static const struct tty_port_operations gsm_port_ops;
323
324
325
326
327
328static const u8 gsm_fcs8[256] = {
329 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75,
330 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
331 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69,
332 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
333 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D,
334 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
335 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51,
336 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
337 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05,
338 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
339 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19,
340 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
341 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D,
342 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
343 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21,
344 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
345 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95,
346 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
347 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89,
348 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
349 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD,
350 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
351 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1,
352 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
353 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5,
354 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
355 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9,
356 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
357 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD,
358 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
359 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1,
360 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF
361};
362
363#define INIT_FCS 0xFF
364#define GOOD_FCS 0xCF
365
366static int gsmld_output(struct gsm_mux *gsm, u8 *data, int len);
367
368
369
370
371
372
373
374
375
376
377static inline u8 gsm_fcs_add(u8 fcs, u8 c)
378{
379 return gsm_fcs8[fcs ^ c];
380}
381
382
383
384
385
386
387
388
389
390
391
392static inline u8 gsm_fcs_add_block(u8 fcs, u8 *c, int len)
393{
394 while (len--)
395 fcs = gsm_fcs8[fcs ^ *c++];
396 return fcs;
397}
398
399
400
401
402
403
404
405
406
407
408static int gsm_read_ea(unsigned int *val, u8 c)
409{
410
411 *val <<= 7;
412 *val |= c >> 1;
413
414 return c & EA;
415}
416
417
418
419
420
421
422
423
424
425static u8 gsm_encode_modem(const struct gsm_dlci *dlci)
426{
427 u8 modembits = 0;
428
429 if (dlci->throttled)
430 modembits |= MDM_FC;
431 if (dlci->modem_tx & TIOCM_DTR)
432 modembits |= MDM_RTC;
433 if (dlci->modem_tx & TIOCM_RTS)
434 modembits |= MDM_RTR;
435 if (dlci->modem_tx & TIOCM_RI)
436 modembits |= MDM_IC;
437 if (dlci->modem_tx & TIOCM_CD)
438 modembits |= MDM_DV;
439 return modembits;
440}
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455static void gsm_print_packet(const char *hdr, int addr, int cr,
456 u8 control, const u8 *data, int dlen)
457{
458 if (!(debug & 1))
459 return;
460
461 pr_info("%s %d) %c: ", hdr, addr, "RC"[cr]);
462
463 switch (control & ~PF) {
464 case SABM:
465 pr_cont("SABM");
466 break;
467 case UA:
468 pr_cont("UA");
469 break;
470 case DISC:
471 pr_cont("DISC");
472 break;
473 case DM:
474 pr_cont("DM");
475 break;
476 case UI:
477 pr_cont("UI");
478 break;
479 case UIH:
480 pr_cont("UIH");
481 break;
482 default:
483 if (!(control & 0x01)) {
484 pr_cont("I N(S)%d N(R)%d",
485 (control & 0x0E) >> 1, (control & 0xE0) >> 5);
486 } else switch (control & 0x0F) {
487 case RR:
488 pr_cont("RR(%d)", (control & 0xE0) >> 5);
489 break;
490 case RNR:
491 pr_cont("RNR(%d)", (control & 0xE0) >> 5);
492 break;
493 case REJ:
494 pr_cont("REJ(%d)", (control & 0xE0) >> 5);
495 break;
496 default:
497 pr_cont("[%02X]", control);
498 }
499 }
500
501 if (control & PF)
502 pr_cont("(P)");
503 else
504 pr_cont("(F)");
505
506 print_hex_dump_bytes("", DUMP_PREFIX_NONE, data, dlen);
507}
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525static int gsm_stuff_frame(const u8 *input, u8 *output, int len)
526{
527 int olen = 0;
528 while (len--) {
529 if (*input == GSM1_SOF || *input == GSM1_ESCAPE
530 || *input == XON || *input == XOFF) {
531 *output++ = GSM1_ESCAPE;
532 *output++ = *input++ ^ GSM1_ESCAPE_BITS;
533 olen++;
534 } else
535 *output++ = *input++;
536 olen++;
537 }
538 return olen;
539}
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555static void gsm_send(struct gsm_mux *gsm, int addr, int cr, int control)
556{
557 int len;
558 u8 cbuf[10];
559 u8 ibuf[3];
560
561 switch (gsm->encoding) {
562 case 0:
563 cbuf[0] = GSM0_SOF;
564 cbuf[1] = (addr << 2) | (cr << 1) | EA;
565 cbuf[2] = control;
566 cbuf[3] = EA;
567 cbuf[4] = 0xFF - gsm_fcs_add_block(INIT_FCS, cbuf + 1, 3);
568 cbuf[5] = GSM0_SOF;
569 len = 6;
570 break;
571 case 1:
572 case 2:
573
574 ibuf[0] = (addr << 2) | (cr << 1) | EA;
575 ibuf[1] = control;
576 ibuf[2] = 0xFF - gsm_fcs_add_block(INIT_FCS, ibuf, 2);
577
578 len = gsm_stuff_frame(ibuf, cbuf + 1, 3);
579
580 cbuf[0] = GSM1_SOF;
581 cbuf[len + 1] = GSM1_SOF;
582
583 len += 2;
584 break;
585 default:
586 WARN_ON(1);
587 return;
588 }
589 gsmld_output(gsm, cbuf, len);
590 gsm_print_packet("-->", addr, cr, control, NULL, 0);
591}
592
593
594
595
596
597
598
599
600
601
602static inline void gsm_response(struct gsm_mux *gsm, int addr, int control)
603{
604 gsm_send(gsm, addr, 0, control);
605}
606
607
608
609
610
611
612
613
614
615
616static inline void gsm_command(struct gsm_mux *gsm, int addr, int control)
617{
618 gsm_send(gsm, addr, 1, control);
619}
620
621
622
623#define HDR_LEN 6
624
625
626
627
628
629
630
631
632
633
634
635
636
637static struct gsm_msg *gsm_data_alloc(struct gsm_mux *gsm, u8 addr, int len,
638 u8 ctrl)
639{
640 struct gsm_msg *m = kmalloc(sizeof(struct gsm_msg) + len + HDR_LEN,
641 GFP_ATOMIC);
642 if (m == NULL)
643 return NULL;
644 m->data = m->buffer + HDR_LEN - 1;
645 m->len = len;
646 m->addr = addr;
647 m->ctrl = ctrl;
648 INIT_LIST_HEAD(&m->list);
649 return m;
650}
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665static void gsm_data_kick(struct gsm_mux *gsm, struct gsm_dlci *dlci)
666{
667 struct gsm_msg *msg, *nmsg;
668 int len;
669
670 list_for_each_entry_safe(msg, nmsg, &gsm->tx_list, list) {
671 if (gsm->constipated && msg->addr)
672 continue;
673 if (gsm->encoding != 0) {
674 gsm->txframe[0] = GSM1_SOF;
675 len = gsm_stuff_frame(msg->data,
676 gsm->txframe + 1, msg->len);
677 gsm->txframe[len + 1] = GSM1_SOF;
678 len += 2;
679 } else {
680 gsm->txframe[0] = GSM0_SOF;
681 memcpy(gsm->txframe + 1 , msg->data, msg->len);
682 gsm->txframe[msg->len + 1] = GSM0_SOF;
683 len = msg->len + 2;
684 }
685
686 if (debug & 4)
687 print_hex_dump_bytes("gsm_data_kick: ",
688 DUMP_PREFIX_OFFSET,
689 gsm->txframe, len);
690 if (gsmld_output(gsm, gsm->txframe, len) < 0)
691 break;
692
693 gsm->tx_bytes -= msg->len;
694
695 list_del(&msg->list);
696 kfree(msg);
697
698 if (dlci) {
699 tty_port_tty_wakeup(&dlci->port);
700 } else {
701 int i = 0;
702
703 for (i = 0; i < NUM_DLCI; i++)
704 if (gsm->dlci[i])
705 tty_port_tty_wakeup(&gsm->dlci[i]->port);
706 }
707 }
708}
709
710
711
712
713
714
715
716
717
718
719
720static void __gsm_data_queue(struct gsm_dlci *dlci, struct gsm_msg *msg)
721{
722 struct gsm_mux *gsm = dlci->gsm;
723 u8 *dp = msg->data;
724 u8 *fcs = dp + msg->len;
725
726
727 if (gsm->encoding == 0) {
728 if (msg->len < 128)
729 *--dp = (msg->len << 1) | EA;
730 else {
731 *--dp = (msg->len >> 7);
732 *--dp = (msg->len & 127) << 1;
733 }
734 }
735
736 *--dp = msg->ctrl;
737 if (gsm->initiator)
738 *--dp = (msg->addr << 2) | 2 | EA;
739 else
740 *--dp = (msg->addr << 2) | EA;
741 *fcs = gsm_fcs_add_block(INIT_FCS, dp , msg->data - dp);
742
743 if (msg->ctrl == UI || msg->ctrl == (UI|PF))
744 *fcs = gsm_fcs_add_block(*fcs, msg->data, msg->len);
745 *fcs = 0xFF - *fcs;
746
747 gsm_print_packet("Q> ", msg->addr, gsm->initiator, msg->ctrl,
748 msg->data, msg->len);
749
750
751
752 msg->len += (msg->data - dp) + 1;
753 msg->data = dp;
754
755
756 list_add_tail(&msg->list, &gsm->tx_list);
757 gsm->tx_bytes += msg->len;
758 gsm_data_kick(gsm, dlci);
759}
760
761
762
763
764
765
766
767
768
769
770
771static void gsm_data_queue(struct gsm_dlci *dlci, struct gsm_msg *msg)
772{
773 unsigned long flags;
774 spin_lock_irqsave(&dlci->gsm->tx_lock, flags);
775 __gsm_data_queue(dlci, msg);
776 spin_unlock_irqrestore(&dlci->gsm->tx_lock, flags);
777}
778
779
780
781
782
783
784
785
786
787
788
789
790
791static int gsm_dlci_data_output(struct gsm_mux *gsm, struct gsm_dlci *dlci)
792{
793 struct gsm_msg *msg;
794 u8 *dp;
795 int len, total_size, size;
796 int h = dlci->adaption - 1;
797
798 total_size = 0;
799 while (1) {
800 len = kfifo_len(&dlci->fifo);
801 if (len == 0)
802 return total_size;
803
804
805 if (len > gsm->mtu)
806 len = gsm->mtu;
807
808 size = len + h;
809
810 msg = gsm_data_alloc(gsm, dlci->addr, size, gsm->ftype);
811
812
813 if (msg == NULL)
814 return -ENOMEM;
815 dp = msg->data;
816 switch (dlci->adaption) {
817 case 1:
818 break;
819 case 2:
820
821 *dp++ = gsm_encode_modem(dlci);
822 break;
823 }
824 WARN_ON(kfifo_out_locked(&dlci->fifo, dp , len, &dlci->lock) != len);
825 __gsm_data_queue(dlci, msg);
826 total_size += size;
827 }
828
829 return total_size;
830}
831
832
833
834
835
836
837
838
839
840
841
842
843
844static int gsm_dlci_data_output_framed(struct gsm_mux *gsm,
845 struct gsm_dlci *dlci)
846{
847 struct gsm_msg *msg;
848 u8 *dp;
849 int len, size;
850 int last = 0, first = 0;
851 int overhead = 0;
852
853
854 if (dlci->adaption == 4)
855 overhead = 1;
856
857
858 if (dlci->skb == NULL) {
859 dlci->skb = skb_dequeue_tail(&dlci->skb_list);
860 if (dlci->skb == NULL)
861 return 0;
862 first = 1;
863 }
864 len = dlci->skb->len + overhead;
865
866
867 if (len > gsm->mtu) {
868 if (dlci->adaption == 3) {
869
870 dev_kfree_skb_any(dlci->skb);
871 dlci->skb = NULL;
872 return 0;
873 }
874 len = gsm->mtu;
875 } else
876 last = 1;
877
878 size = len + overhead;
879 msg = gsm_data_alloc(gsm, dlci->addr, size, gsm->ftype);
880
881
882
883 if (msg == NULL) {
884 skb_queue_tail(&dlci->skb_list, dlci->skb);
885 dlci->skb = NULL;
886 return -ENOMEM;
887 }
888 dp = msg->data;
889
890 if (dlci->adaption == 4) {
891
892 *dp++ = last << 7 | first << 6 | 1;
893 len--;
894 }
895 memcpy(dp, dlci->skb->data, len);
896 skb_pull(dlci->skb, len);
897 __gsm_data_queue(dlci, msg);
898 if (last) {
899 dev_kfree_skb_any(dlci->skb);
900 dlci->skb = NULL;
901 }
902 return size;
903}
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918static void gsm_dlci_data_sweep(struct gsm_mux *gsm)
919{
920 int len;
921
922 int i = 1;
923
924 while (i < NUM_DLCI) {
925 struct gsm_dlci *dlci;
926
927 if (gsm->tx_bytes > TX_THRESH_HI)
928 break;
929 dlci = gsm->dlci[i];
930 if (dlci == NULL || dlci->constipated) {
931 i++;
932 continue;
933 }
934 if (dlci->adaption < 3 && !dlci->net)
935 len = gsm_dlci_data_output(gsm, dlci);
936 else
937 len = gsm_dlci_data_output_framed(gsm, dlci);
938 if (len < 0)
939 break;
940
941 if (len == 0)
942 i++;
943 }
944}
945
946
947
948
949
950
951
952
953
954
955static void gsm_dlci_data_kick(struct gsm_dlci *dlci)
956{
957 unsigned long flags;
958 int sweep;
959
960 if (dlci->constipated)
961 return;
962
963 spin_lock_irqsave(&dlci->gsm->tx_lock, flags);
964
965 sweep = (dlci->gsm->tx_bytes < TX_THRESH_LO);
966 if (dlci->gsm->tx_bytes == 0) {
967 if (dlci->net)
968 gsm_dlci_data_output_framed(dlci->gsm, dlci);
969 else
970 gsm_dlci_data_output(dlci->gsm, dlci);
971 }
972 if (sweep)
973 gsm_dlci_data_sweep(dlci->gsm);
974 spin_unlock_irqrestore(&dlci->gsm->tx_lock, flags);
975}
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992static void gsm_control_reply(struct gsm_mux *gsm, int cmd, const u8 *data,
993 int dlen)
994{
995 struct gsm_msg *msg;
996 msg = gsm_data_alloc(gsm, 0, dlen + 2, gsm->ftype);
997 if (msg == NULL)
998 return;
999 msg->data[0] = (cmd & 0xFE) << 1 | EA;
1000 msg->data[1] = (dlen << 1) | EA;
1001 memcpy(msg->data + 2, data, dlen);
1002 gsm_data_queue(gsm->dlci[0], msg);
1003}
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016static void gsm_process_modem(struct tty_struct *tty, struct gsm_dlci *dlci,
1017 u32 modem, int clen)
1018{
1019 int mlines = 0;
1020 u8 brk = 0;
1021 int fc;
1022
1023
1024
1025
1026
1027
1028 if (clen == 2)
1029 modem = modem & 0x7f;
1030 else {
1031 brk = modem & 0x7f;
1032 modem = (modem >> 7) & 0x7f;
1033 }
1034
1035
1036 fc = (modem & MDM_FC) || !(modem & MDM_RTR);
1037 if (fc && !dlci->constipated) {
1038
1039 dlci->constipated = true;
1040 } else if (!fc && dlci->constipated) {
1041 dlci->constipated = false;
1042 gsm_dlci_data_kick(dlci);
1043 }
1044
1045
1046 if (modem & MDM_RTC)
1047 mlines |= TIOCM_DSR | TIOCM_DTR;
1048 if (modem & MDM_RTR)
1049 mlines |= TIOCM_RTS | TIOCM_CTS;
1050 if (modem & MDM_IC)
1051 mlines |= TIOCM_RI;
1052 if (modem & MDM_DV)
1053 mlines |= TIOCM_CD;
1054
1055
1056 if (tty) {
1057 if ((mlines & TIOCM_CD) == 0 && (dlci->modem_rx & TIOCM_CD))
1058 if (!C_CLOCAL(tty))
1059 tty_hangup(tty);
1060 }
1061 if (brk & 0x01)
1062 tty_insert_flip_char(&dlci->port, 0, TTY_BREAK);
1063 dlci->modem_rx = mlines;
1064}
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078static void gsm_control_modem(struct gsm_mux *gsm, const u8 *data, int clen)
1079{
1080 unsigned int addr = 0;
1081 unsigned int modem = 0;
1082 unsigned int brk = 0;
1083 struct gsm_dlci *dlci;
1084 int len = clen;
1085 const u8 *dp = data;
1086 struct tty_struct *tty;
1087
1088 while (gsm_read_ea(&addr, *dp++) == 0) {
1089 len--;
1090 if (len == 0)
1091 return;
1092 }
1093
1094 len--;
1095 if (len <= 0)
1096 return;
1097
1098 addr >>= 1;
1099
1100 if (addr == 0 || addr >= NUM_DLCI || gsm->dlci[addr] == NULL)
1101 return;
1102 dlci = gsm->dlci[addr];
1103
1104 while (gsm_read_ea(&modem, *dp++) == 0) {
1105 len--;
1106 if (len == 0)
1107 return;
1108 }
1109 len--;
1110 if (len > 0) {
1111 while (gsm_read_ea(&brk, *dp++) == 0) {
1112 len--;
1113 if (len == 0)
1114 return;
1115 }
1116 modem <<= 7;
1117 modem |= (brk & 0x7f);
1118 }
1119 tty = tty_port_tty_get(&dlci->port);
1120 gsm_process_modem(tty, dlci, modem, clen);
1121 if (tty) {
1122 tty_wakeup(tty);
1123 tty_kref_put(tty);
1124 }
1125 gsm_control_reply(gsm, CMD_MSC, data, clen);
1126}
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139static void gsm_control_rls(struct gsm_mux *gsm, const u8 *data, int clen)
1140{
1141 struct tty_port *port;
1142 unsigned int addr = 0;
1143 u8 bits;
1144 int len = clen;
1145 const u8 *dp = data;
1146
1147 while (gsm_read_ea(&addr, *dp++) == 0) {
1148 len--;
1149 if (len == 0)
1150 return;
1151 }
1152
1153 len--;
1154 if (len <= 0)
1155 return;
1156 addr >>= 1;
1157
1158 if (addr == 0 || addr >= NUM_DLCI || gsm->dlci[addr] == NULL)
1159 return;
1160
1161 bits = *dp;
1162 if ((bits & 1) == 0)
1163 return;
1164
1165 port = &gsm->dlci[addr]->port;
1166
1167 if (bits & 2)
1168 tty_insert_flip_char(port, 0, TTY_OVERRUN);
1169 if (bits & 4)
1170 tty_insert_flip_char(port, 0, TTY_PARITY);
1171 if (bits & 8)
1172 tty_insert_flip_char(port, 0, TTY_FRAME);
1173
1174 tty_flip_buffer_push(port);
1175
1176 gsm_control_reply(gsm, CMD_RLS, data, clen);
1177}
1178
1179static void gsm_dlci_begin_close(struct gsm_dlci *dlci);
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193static void gsm_control_message(struct gsm_mux *gsm, unsigned int command,
1194 const u8 *data, int clen)
1195{
1196 u8 buf[1];
1197 unsigned long flags;
1198
1199 switch (command) {
1200 case CMD_CLD: {
1201 struct gsm_dlci *dlci = gsm->dlci[0];
1202
1203 if (dlci) {
1204 dlci->dead = true;
1205 gsm->dead = true;
1206 gsm_dlci_begin_close(dlci);
1207 }
1208 }
1209 break;
1210 case CMD_TEST:
1211
1212 gsm_control_reply(gsm, CMD_TEST, data, clen);
1213 break;
1214 case CMD_FCON:
1215
1216 gsm->constipated = false;
1217 gsm_control_reply(gsm, CMD_FCON, NULL, 0);
1218
1219 spin_lock_irqsave(&gsm->tx_lock, flags);
1220 gsm_data_kick(gsm, NULL);
1221 spin_unlock_irqrestore(&gsm->tx_lock, flags);
1222 break;
1223 case CMD_FCOFF:
1224
1225 gsm->constipated = true;
1226 gsm_control_reply(gsm, CMD_FCOFF, NULL, 0);
1227 break;
1228 case CMD_MSC:
1229
1230 gsm_control_modem(gsm, data, clen);
1231 break;
1232 case CMD_RLS:
1233
1234 gsm_control_rls(gsm, data, clen);
1235 break;
1236 case CMD_PSC:
1237
1238 gsm_control_reply(gsm, CMD_PSC, NULL, 0);
1239 break;
1240
1241 case CMD_PN:
1242 case CMD_RPN:
1243 case CMD_SNC:
1244 default:
1245
1246 buf[0] = command;
1247 gsm_control_reply(gsm, CMD_NSC, buf, 1);
1248 break;
1249 }
1250}
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265static void gsm_control_response(struct gsm_mux *gsm, unsigned int command,
1266 const u8 *data, int clen)
1267{
1268 struct gsm_control *ctrl;
1269 unsigned long flags;
1270
1271 spin_lock_irqsave(&gsm->control_lock, flags);
1272
1273 ctrl = gsm->pending_cmd;
1274
1275 command |= 1;
1276 if (ctrl != NULL && (command == ctrl->cmd || command == CMD_NSC)) {
1277
1278 del_timer(&gsm->t2_timer);
1279 gsm->pending_cmd = NULL;
1280
1281 if (command == CMD_NSC)
1282 ctrl->error = -EOPNOTSUPP;
1283 ctrl->done = 1;
1284 wake_up(&gsm->event);
1285 }
1286 spin_unlock_irqrestore(&gsm->control_lock, flags);
1287}
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297static void gsm_control_transmit(struct gsm_mux *gsm, struct gsm_control *ctrl)
1298{
1299 struct gsm_msg *msg = gsm_data_alloc(gsm, 0, ctrl->len + 1, gsm->ftype);
1300 if (msg == NULL)
1301 return;
1302 msg->data[0] = (ctrl->cmd << 1) | 2 | EA;
1303 memcpy(msg->data + 1, ctrl->data, ctrl->len);
1304 gsm_data_queue(gsm->dlci[0], msg);
1305}
1306
1307
1308
1309
1310
1311
1312
1313
1314
1315
1316
1317
1318static void gsm_control_retransmit(struct timer_list *t)
1319{
1320 struct gsm_mux *gsm = from_timer(gsm, t, t2_timer);
1321 struct gsm_control *ctrl;
1322 unsigned long flags;
1323 spin_lock_irqsave(&gsm->control_lock, flags);
1324 ctrl = gsm->pending_cmd;
1325 if (ctrl) {
1326 gsm->cretries--;
1327 if (gsm->cretries == 0) {
1328 gsm->pending_cmd = NULL;
1329 ctrl->error = -ETIMEDOUT;
1330 ctrl->done = 1;
1331 spin_unlock_irqrestore(&gsm->control_lock, flags);
1332 wake_up(&gsm->event);
1333 return;
1334 }
1335 gsm_control_transmit(gsm, ctrl);
1336 mod_timer(&gsm->t2_timer, jiffies + gsm->t2 * HZ / 100);
1337 }
1338 spin_unlock_irqrestore(&gsm->control_lock, flags);
1339}
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351
1352
1353static struct gsm_control *gsm_control_send(struct gsm_mux *gsm,
1354 unsigned int command, u8 *data, int clen)
1355{
1356 struct gsm_control *ctrl = kzalloc(sizeof(struct gsm_control),
1357 GFP_KERNEL);
1358 unsigned long flags;
1359 if (ctrl == NULL)
1360 return NULL;
1361retry:
1362 wait_event(gsm->event, gsm->pending_cmd == NULL);
1363 spin_lock_irqsave(&gsm->control_lock, flags);
1364 if (gsm->pending_cmd != NULL) {
1365 spin_unlock_irqrestore(&gsm->control_lock, flags);
1366 goto retry;
1367 }
1368 ctrl->cmd = command;
1369 ctrl->data = data;
1370 ctrl->len = clen;
1371 gsm->pending_cmd = ctrl;
1372
1373
1374 if (gsm->dlci[0]->mode == DLCI_MODE_ADM)
1375 gsm->cretries = 1;
1376 else
1377 gsm->cretries = gsm->n2;
1378
1379 mod_timer(&gsm->t2_timer, jiffies + gsm->t2 * HZ / 100);
1380 gsm_control_transmit(gsm, ctrl);
1381 spin_unlock_irqrestore(&gsm->control_lock, flags);
1382 return ctrl;
1383}
1384
1385
1386
1387
1388
1389
1390
1391
1392
1393
1394
1395static int gsm_control_wait(struct gsm_mux *gsm, struct gsm_control *control)
1396{
1397 int err;
1398 wait_event(gsm->event, control->done == 1);
1399 err = control->error;
1400 kfree(control);
1401 return err;
1402}
1403
1404
1405
1406
1407
1408
1409
1410
1411
1412
1413
1414
1415
1416
1417
1418
1419
1420
1421static void gsm_dlci_close(struct gsm_dlci *dlci)
1422{
1423 del_timer(&dlci->t1);
1424 if (debug & 8)
1425 pr_debug("DLCI %d goes closed.\n", dlci->addr);
1426 dlci->state = DLCI_CLOSED;
1427 if (dlci->addr != 0) {
1428 tty_port_tty_hangup(&dlci->port, false);
1429 kfifo_reset(&dlci->fifo);
1430 } else
1431 dlci->gsm->dead = true;
1432 wake_up(&dlci->gsm->event);
1433
1434
1435}
1436
1437
1438
1439
1440
1441
1442
1443
1444static void gsm_dlci_open(struct gsm_dlci *dlci)
1445{
1446
1447
1448 del_timer(&dlci->t1);
1449
1450 dlci->state = DLCI_OPEN;
1451 if (debug & 8)
1452 pr_debug("DLCI %d goes open.\n", dlci->addr);
1453 wake_up(&dlci->gsm->event);
1454}
1455
1456
1457
1458
1459
1460
1461
1462
1463
1464
1465
1466
1467
1468
1469
1470
1471static void gsm_dlci_t1(struct timer_list *t)
1472{
1473 struct gsm_dlci *dlci = from_timer(dlci, t, t1);
1474 struct gsm_mux *gsm = dlci->gsm;
1475
1476 switch (dlci->state) {
1477 case DLCI_OPENING:
1478 dlci->retries--;
1479 if (dlci->retries) {
1480 gsm_command(dlci->gsm, dlci->addr, SABM|PF);
1481 mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
1482 } else if (!dlci->addr && gsm->control == (DM | PF)) {
1483 if (debug & 8)
1484 pr_info("DLCI %d opening in ADM mode.\n",
1485 dlci->addr);
1486 dlci->mode = DLCI_MODE_ADM;
1487 gsm_dlci_open(dlci);
1488 } else {
1489 gsm_dlci_close(dlci);
1490 }
1491
1492 break;
1493 case DLCI_CLOSING:
1494 dlci->retries--;
1495 if (dlci->retries) {
1496 gsm_command(dlci->gsm, dlci->addr, DISC|PF);
1497 mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
1498 } else
1499 gsm_dlci_close(dlci);
1500 break;
1501 default:
1502 pr_debug("%s: unhandled state: %d\n", __func__, dlci->state);
1503 break;
1504 }
1505}
1506
1507
1508
1509
1510
1511
1512
1513
1514
1515
1516
1517static void gsm_dlci_begin_open(struct gsm_dlci *dlci)
1518{
1519 struct gsm_mux *gsm = dlci->gsm;
1520 if (dlci->state == DLCI_OPEN || dlci->state == DLCI_OPENING)
1521 return;
1522 dlci->retries = gsm->n2;
1523 dlci->state = DLCI_OPENING;
1524 gsm_command(dlci->gsm, dlci->addr, SABM|PF);
1525 mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
1526}
1527
1528
1529
1530
1531
1532
1533
1534
1535
1536
1537
1538
1539static void gsm_dlci_begin_close(struct gsm_dlci *dlci)
1540{
1541 struct gsm_mux *gsm = dlci->gsm;
1542 if (dlci->state == DLCI_CLOSED || dlci->state == DLCI_CLOSING)
1543 return;
1544 dlci->retries = gsm->n2;
1545 dlci->state = DLCI_CLOSING;
1546 gsm_command(dlci->gsm, dlci->addr, DISC|PF);
1547 mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
1548}
1549
1550
1551
1552
1553
1554
1555
1556
1557
1558
1559
1560
1561static void gsm_dlci_data(struct gsm_dlci *dlci, const u8 *data, int clen)
1562{
1563
1564 struct tty_port *port = &dlci->port;
1565 struct tty_struct *tty;
1566 unsigned int modem = 0;
1567 int len = clen;
1568
1569 if (debug & 16)
1570 pr_debug("%d bytes for tty\n", len);
1571 switch (dlci->adaption) {
1572
1573 case 4:
1574 break;
1575 case 3:
1576 break;
1577 case 2:
1578 while (gsm_read_ea(&modem, *data++) == 0) {
1579 len--;
1580 if (len == 0)
1581 return;
1582 }
1583 tty = tty_port_tty_get(port);
1584 if (tty) {
1585 gsm_process_modem(tty, dlci, modem, clen);
1586 tty_kref_put(tty);
1587 }
1588 fallthrough;
1589 case 1:
1590 default:
1591 tty_insert_flip_string(port, data, len);
1592 tty_flip_buffer_push(port);
1593 }
1594}
1595
1596
1597
1598
1599
1600
1601
1602
1603
1604
1605
1606
1607
1608static void gsm_dlci_command(struct gsm_dlci *dlci, const u8 *data, int len)
1609{
1610
1611 unsigned int command = 0;
1612 while (len-- > 0) {
1613 if (gsm_read_ea(&command, *data++) == 1) {
1614 int clen = *data++;
1615 len--;
1616
1617 clen >>= 1;
1618
1619 if (clen > len)
1620 return;
1621 if (command & 1)
1622 gsm_control_message(dlci->gsm, command,
1623 data, clen);
1624 else
1625 gsm_control_response(dlci->gsm, command,
1626 data, clen);
1627 return;
1628 }
1629 }
1630}
1631
1632
1633
1634
1635
1636
1637
1638
1639
1640
1641
1642
1643
1644
1645
1646static struct gsm_dlci *gsm_dlci_alloc(struct gsm_mux *gsm, int addr)
1647{
1648 struct gsm_dlci *dlci = kzalloc(sizeof(struct gsm_dlci), GFP_ATOMIC);
1649 if (dlci == NULL)
1650 return NULL;
1651 spin_lock_init(&dlci->lock);
1652 mutex_init(&dlci->mutex);
1653 if (kfifo_alloc(&dlci->fifo, 4096, GFP_KERNEL) < 0) {
1654 kfree(dlci);
1655 return NULL;
1656 }
1657
1658 skb_queue_head_init(&dlci->skb_list);
1659 timer_setup(&dlci->t1, gsm_dlci_t1, 0);
1660 tty_port_init(&dlci->port);
1661 dlci->port.ops = &gsm_port_ops;
1662 dlci->gsm = gsm;
1663 dlci->addr = addr;
1664 dlci->adaption = gsm->adaption;
1665 dlci->state = DLCI_CLOSED;
1666 if (addr)
1667 dlci->data = gsm_dlci_data;
1668 else
1669 dlci->data = gsm_dlci_command;
1670 gsm->dlci[addr] = dlci;
1671 return dlci;
1672}
1673
1674
1675
1676
1677
1678
1679
1680
1681
1682static void gsm_dlci_free(struct tty_port *port)
1683{
1684 struct gsm_dlci *dlci = container_of(port, struct gsm_dlci, port);
1685
1686 del_timer_sync(&dlci->t1);
1687 dlci->gsm->dlci[dlci->addr] = NULL;
1688 kfifo_free(&dlci->fifo);
1689 while ((dlci->skb = skb_dequeue(&dlci->skb_list)))
1690 dev_kfree_skb(dlci->skb);
1691 kfree(dlci);
1692}
1693
1694static inline void dlci_get(struct gsm_dlci *dlci)
1695{
1696 tty_port_get(&dlci->port);
1697}
1698
1699static inline void dlci_put(struct gsm_dlci *dlci)
1700{
1701 tty_port_put(&dlci->port);
1702}
1703
1704static void gsm_destroy_network(struct gsm_dlci *dlci);
1705
1706
1707
1708
1709
1710
1711
1712
1713
1714
1715static void gsm_dlci_release(struct gsm_dlci *dlci)
1716{
1717 struct tty_struct *tty = tty_port_tty_get(&dlci->port);
1718 if (tty) {
1719 mutex_lock(&dlci->mutex);
1720 gsm_destroy_network(dlci);
1721 mutex_unlock(&dlci->mutex);
1722
1723 tty_hangup(tty);
1724
1725 tty_port_tty_set(&dlci->port, NULL);
1726 tty_kref_put(tty);
1727 }
1728 dlci->state = DLCI_CLOSED;
1729 dlci_put(dlci);
1730}
1731
1732
1733
1734
1735
1736
1737
1738
1739
1740
1741
1742
1743
1744
1745
1746static void gsm_queue(struct gsm_mux *gsm)
1747{
1748 struct gsm_dlci *dlci;
1749 u8 cr;
1750 int address;
1751
1752
1753
1754 if ((gsm->control & ~PF) == UI)
1755 gsm->fcs = gsm_fcs_add_block(gsm->fcs, gsm->buf, gsm->len);
1756 if (gsm->encoding == 0) {
1757
1758
1759
1760
1761 gsm->fcs = gsm_fcs_add(gsm->fcs, gsm->received_fcs);
1762 }
1763 if (gsm->fcs != GOOD_FCS) {
1764 gsm->bad_fcs++;
1765 if (debug & 4)
1766 pr_debug("BAD FCS %02x\n", gsm->fcs);
1767 return;
1768 }
1769 address = gsm->address >> 1;
1770 if (address >= NUM_DLCI)
1771 goto invalid;
1772
1773 cr = gsm->address & 1;
1774
1775 gsm_print_packet("<--", address, cr, gsm->control, gsm->buf, gsm->len);
1776
1777 cr ^= 1 - gsm->initiator;
1778 dlci = gsm->dlci[address];
1779
1780 switch (gsm->control) {
1781 case SABM|PF:
1782 if (cr == 0)
1783 goto invalid;
1784 if (dlci == NULL)
1785 dlci = gsm_dlci_alloc(gsm, address);
1786 if (dlci == NULL)
1787 return;
1788 if (dlci->dead)
1789 gsm_response(gsm, address, DM);
1790 else {
1791 gsm_response(gsm, address, UA);
1792 gsm_dlci_open(dlci);
1793 }
1794 break;
1795 case DISC|PF:
1796 if (cr == 0)
1797 goto invalid;
1798 if (dlci == NULL || dlci->state == DLCI_CLOSED) {
1799 gsm_response(gsm, address, DM);
1800 return;
1801 }
1802
1803 gsm_response(gsm, address, UA);
1804 gsm_dlci_close(dlci);
1805 break;
1806 case UA:
1807 case UA|PF:
1808 if (cr == 0 || dlci == NULL)
1809 break;
1810 switch (dlci->state) {
1811 case DLCI_CLOSING:
1812 gsm_dlci_close(dlci);
1813 break;
1814 case DLCI_OPENING:
1815 gsm_dlci_open(dlci);
1816 break;
1817 default:
1818 pr_debug("%s: unhandled state: %d\n", __func__,
1819 dlci->state);
1820 break;
1821 }
1822 break;
1823 case DM:
1824 case DM|PF:
1825 if (cr)
1826 goto invalid;
1827 if (dlci == NULL)
1828 return;
1829 gsm_dlci_close(dlci);
1830 break;
1831 case UI:
1832 case UI|PF:
1833 case UIH:
1834 case UIH|PF:
1835#if 0
1836 if (cr)
1837 goto invalid;
1838#endif
1839 if (dlci == NULL || dlci->state != DLCI_OPEN) {
1840 gsm_command(gsm, address, DM|PF);
1841 return;
1842 }
1843 dlci->data(dlci, gsm->buf, gsm->len);
1844 break;
1845 default:
1846 goto invalid;
1847 }
1848 return;
1849invalid:
1850 gsm->malformed++;
1851 return;
1852}
1853
1854
1855
1856
1857
1858
1859
1860
1861
1862
1863static void gsm0_receive(struct gsm_mux *gsm, unsigned char c)
1864{
1865 unsigned int len;
1866
1867 switch (gsm->state) {
1868 case GSM_SEARCH:
1869 if (c == GSM0_SOF) {
1870 gsm->state = GSM_ADDRESS;
1871 gsm->address = 0;
1872 gsm->len = 0;
1873 gsm->fcs = INIT_FCS;
1874 }
1875 break;
1876 case GSM_ADDRESS:
1877 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1878 if (gsm_read_ea(&gsm->address, c))
1879 gsm->state = GSM_CONTROL;
1880 break;
1881 case GSM_CONTROL:
1882 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1883 gsm->control = c;
1884 gsm->state = GSM_LEN0;
1885 break;
1886 case GSM_LEN0:
1887 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1888 if (gsm_read_ea(&gsm->len, c)) {
1889 if (gsm->len > gsm->mru) {
1890 gsm->bad_size++;
1891 gsm->state = GSM_SEARCH;
1892 break;
1893 }
1894 gsm->count = 0;
1895 if (!gsm->len)
1896 gsm->state = GSM_FCS;
1897 else
1898 gsm->state = GSM_DATA;
1899 break;
1900 }
1901 gsm->state = GSM_LEN1;
1902 break;
1903 case GSM_LEN1:
1904 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1905 len = c;
1906 gsm->len |= len << 7;
1907 if (gsm->len > gsm->mru) {
1908 gsm->bad_size++;
1909 gsm->state = GSM_SEARCH;
1910 break;
1911 }
1912 gsm->count = 0;
1913 if (!gsm->len)
1914 gsm->state = GSM_FCS;
1915 else
1916 gsm->state = GSM_DATA;
1917 break;
1918 case GSM_DATA:
1919 gsm->buf[gsm->count++] = c;
1920 if (gsm->count == gsm->len)
1921 gsm->state = GSM_FCS;
1922 break;
1923 case GSM_FCS:
1924 gsm->received_fcs = c;
1925 gsm_queue(gsm);
1926 gsm->state = GSM_SSOF;
1927 break;
1928 case GSM_SSOF:
1929 if (c == GSM0_SOF) {
1930 gsm->state = GSM_SEARCH;
1931 break;
1932 }
1933 break;
1934 default:
1935 pr_debug("%s: unhandled state: %d\n", __func__, gsm->state);
1936 break;
1937 }
1938}
1939
1940
1941
1942
1943
1944
1945
1946
1947
1948static void gsm1_receive(struct gsm_mux *gsm, unsigned char c)
1949{
1950 if (c == GSM1_SOF) {
1951
1952
1953 if (gsm->state == GSM_DATA && gsm->count) {
1954
1955 gsm->count--;
1956 gsm->fcs = gsm_fcs_add(gsm->fcs, gsm->buf[gsm->count]);
1957 gsm->len = gsm->count;
1958 gsm_queue(gsm);
1959 gsm->state = GSM_START;
1960 return;
1961 }
1962
1963 if (gsm->state != GSM_START) {
1964 gsm->malformed++;
1965 gsm->state = GSM_START;
1966 }
1967
1968
1969 return;
1970 }
1971
1972 if (c == GSM1_ESCAPE) {
1973 gsm->escape = true;
1974 return;
1975 }
1976
1977
1978 if (gsm->state == GSM_SEARCH)
1979 return;
1980
1981 if (gsm->escape) {
1982 c ^= GSM1_ESCAPE_BITS;
1983 gsm->escape = false;
1984 }
1985 switch (gsm->state) {
1986 case GSM_START:
1987 gsm->address = 0;
1988 gsm->state = GSM_ADDRESS;
1989 gsm->fcs = INIT_FCS;
1990 fallthrough;
1991 case GSM_ADDRESS:
1992 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1993 if (gsm_read_ea(&gsm->address, c))
1994 gsm->state = GSM_CONTROL;
1995 break;
1996 case GSM_CONTROL:
1997 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1998 gsm->control = c;
1999 gsm->count = 0;
2000 gsm->state = GSM_DATA;
2001 break;
2002 case GSM_DATA:
2003 if (gsm->count > gsm->mru) {
2004 gsm->state = GSM_OVERRUN;
2005 gsm->bad_size++;
2006 } else
2007 gsm->buf[gsm->count++] = c;
2008 break;
2009 case GSM_OVERRUN:
2010 break;
2011 default:
2012 pr_debug("%s: unhandled state: %d\n", __func__, gsm->state);
2013 break;
2014 }
2015}
2016
2017
2018
2019
2020
2021
2022
2023
2024
2025
2026
2027
2028
2029static void gsm_error(struct gsm_mux *gsm,
2030 unsigned char data, unsigned char flag)
2031{
2032 gsm->state = GSM_SEARCH;
2033 gsm->io_error++;
2034}
2035
2036static int gsm_disconnect(struct gsm_mux *gsm)
2037{
2038 struct gsm_dlci *dlci = gsm->dlci[0];
2039 struct gsm_control *gc;
2040
2041 if (!dlci)
2042 return 0;
2043
2044
2045
2046 gc = gsm_control_send(gsm, CMD_CLD, NULL, 0);
2047 if (gc)
2048 gsm_control_wait(gsm, gc);
2049
2050 del_timer_sync(&gsm->t2_timer);
2051
2052
2053 gsm_dlci_begin_close(dlci);
2054 wait_event_interruptible(gsm->event,
2055 dlci->state == DLCI_CLOSED);
2056
2057 if (signal_pending(current))
2058 return -EINTR;
2059
2060 return 0;
2061}
2062
2063
2064
2065
2066
2067
2068
2069
2070
2071
2072static void gsm_cleanup_mux(struct gsm_mux *gsm)
2073{
2074 int i;
2075 struct gsm_dlci *dlci = gsm->dlci[0];
2076 struct gsm_msg *txq, *ntxq;
2077
2078 gsm->dead = true;
2079
2080 spin_lock(&gsm_mux_lock);
2081 for (i = 0; i < MAX_MUX; i++) {
2082 if (gsm_mux[i] == gsm) {
2083 gsm_mux[i] = NULL;
2084 break;
2085 }
2086 }
2087 spin_unlock(&gsm_mux_lock);
2088
2089 if (i == MAX_MUX)
2090 return;
2091
2092 del_timer_sync(&gsm->t2_timer);
2093
2094 if (dlci)
2095 dlci->dead = true;
2096
2097
2098 mutex_lock(&gsm->mutex);
2099 for (i = 0; i < NUM_DLCI; i++)
2100 if (gsm->dlci[i])
2101 gsm_dlci_release(gsm->dlci[i]);
2102 mutex_unlock(&gsm->mutex);
2103
2104 list_for_each_entry_safe(txq, ntxq, &gsm->tx_list, list)
2105 kfree(txq);
2106 INIT_LIST_HEAD(&gsm->tx_list);
2107}
2108
2109
2110
2111
2112
2113
2114
2115
2116
2117
2118static int gsm_activate_mux(struct gsm_mux *gsm)
2119{
2120 struct gsm_dlci *dlci;
2121 int i = 0;
2122
2123 timer_setup(&gsm->t2_timer, gsm_control_retransmit, 0);
2124 init_waitqueue_head(&gsm->event);
2125 spin_lock_init(&gsm->control_lock);
2126 spin_lock_init(&gsm->tx_lock);
2127
2128 if (gsm->encoding == 0)
2129 gsm->receive = gsm0_receive;
2130 else
2131 gsm->receive = gsm1_receive;
2132
2133 spin_lock(&gsm_mux_lock);
2134 for (i = 0; i < MAX_MUX; i++) {
2135 if (gsm_mux[i] == NULL) {
2136 gsm->num = i;
2137 gsm_mux[i] = gsm;
2138 break;
2139 }
2140 }
2141 spin_unlock(&gsm_mux_lock);
2142 if (i == MAX_MUX)
2143 return -EBUSY;
2144
2145 dlci = gsm_dlci_alloc(gsm, 0);
2146 if (dlci == NULL)
2147 return -ENOMEM;
2148 gsm->dead = false;
2149 return 0;
2150}
2151
2152
2153
2154
2155
2156
2157
2158static void gsm_free_mux(struct gsm_mux *gsm)
2159{
2160 kfree(gsm->txframe);
2161 kfree(gsm->buf);
2162 kfree(gsm);
2163}
2164
2165
2166
2167
2168
2169
2170
2171static void gsm_free_muxr(struct kref *ref)
2172{
2173 struct gsm_mux *gsm = container_of(ref, struct gsm_mux, ref);
2174 gsm_free_mux(gsm);
2175}
2176
2177static inline void mux_get(struct gsm_mux *gsm)
2178{
2179 kref_get(&gsm->ref);
2180}
2181
2182static inline void mux_put(struct gsm_mux *gsm)
2183{
2184 kref_put(&gsm->ref, gsm_free_muxr);
2185}
2186
2187static inline unsigned int mux_num_to_base(struct gsm_mux *gsm)
2188{
2189 return gsm->num * NUM_DLCI;
2190}
2191
2192static inline unsigned int mux_line_to_num(unsigned int line)
2193{
2194 return line / NUM_DLCI;
2195}
2196
2197
2198
2199
2200
2201
2202
2203static struct gsm_mux *gsm_alloc_mux(void)
2204{
2205 struct gsm_mux *gsm = kzalloc(sizeof(struct gsm_mux), GFP_KERNEL);
2206 if (gsm == NULL)
2207 return NULL;
2208 gsm->buf = kmalloc(MAX_MRU + 1, GFP_KERNEL);
2209 if (gsm->buf == NULL) {
2210 kfree(gsm);
2211 return NULL;
2212 }
2213 gsm->txframe = kmalloc(2 * MAX_MRU + 2, GFP_KERNEL);
2214 if (gsm->txframe == NULL) {
2215 kfree(gsm->buf);
2216 kfree(gsm);
2217 return NULL;
2218 }
2219 spin_lock_init(&gsm->lock);
2220 mutex_init(&gsm->mutex);
2221 kref_init(&gsm->ref);
2222 INIT_LIST_HEAD(&gsm->tx_list);
2223
2224 gsm->t1 = T1;
2225 gsm->t2 = T2;
2226 gsm->n2 = N2;
2227 gsm->ftype = UIH;
2228 gsm->adaption = 1;
2229 gsm->encoding = 1;
2230 gsm->mru = 64;
2231 gsm->mtu = 64;
2232 gsm->dead = true;
2233
2234 return gsm;
2235}
2236
2237static void gsm_copy_config_values(struct gsm_mux *gsm,
2238 struct gsm_config *c)
2239{
2240 memset(c, 0, sizeof(*c));
2241 c->adaption = gsm->adaption;
2242 c->encapsulation = gsm->encoding;
2243 c->initiator = gsm->initiator;
2244 c->t1 = gsm->t1;
2245 c->t2 = gsm->t2;
2246 c->t3 = 0;
2247 c->n2 = gsm->n2;
2248 if (gsm->ftype == UIH)
2249 c->i = 1;
2250 else
2251 c->i = 2;
2252 pr_debug("Ftype %d i %d\n", gsm->ftype, c->i);
2253 c->mru = gsm->mru;
2254 c->mtu = gsm->mtu;
2255 c->k = 0;
2256}
2257
2258static int gsm_config(struct gsm_mux *gsm, struct gsm_config *c)
2259{
2260 int need_close = 0;
2261 int need_restart = 0;
2262
2263
2264 if ((c->adaption != 1 && c->adaption != 2) || c->k)
2265 return -EOPNOTSUPP;
2266
2267 if (c->mru > MAX_MRU || c->mtu > MAX_MTU || c->mru < 8 || c->mtu < 8)
2268 return -EINVAL;
2269 if (c->n2 < 3)
2270 return -EINVAL;
2271 if (c->encapsulation > 1)
2272 return -EINVAL;
2273 if (c->initiator > 1)
2274 return -EINVAL;
2275 if (c->i == 0 || c->i > 2)
2276 return -EINVAL;
2277
2278
2279
2280
2281
2282 if (c->t1 != 0 && c->t1 != gsm->t1)
2283 need_restart = 1;
2284 if (c->t2 != 0 && c->t2 != gsm->t2)
2285 need_restart = 1;
2286 if (c->encapsulation != gsm->encoding)
2287 need_restart = 1;
2288 if (c->adaption != gsm->adaption)
2289 need_restart = 1;
2290
2291 if (c->initiator != gsm->initiator)
2292 need_close = 1;
2293 if (c->mru != gsm->mru)
2294 need_restart = 1;
2295 if (c->mtu != gsm->mtu)
2296 need_restart = 1;
2297
2298
2299
2300
2301
2302
2303 if (need_close || need_restart) {
2304 int ret;
2305
2306 ret = gsm_disconnect(gsm);
2307
2308 if (ret)
2309 return ret;
2310 }
2311 if (need_restart)
2312 gsm_cleanup_mux(gsm);
2313
2314 gsm->initiator = c->initiator;
2315 gsm->mru = c->mru;
2316 gsm->mtu = c->mtu;
2317 gsm->encoding = c->encapsulation;
2318 gsm->adaption = c->adaption;
2319 gsm->n2 = c->n2;
2320
2321 if (c->i == 1)
2322 gsm->ftype = UIH;
2323 else if (c->i == 2)
2324 gsm->ftype = UI;
2325
2326 if (c->t1)
2327 gsm->t1 = c->t1;
2328 if (c->t2)
2329 gsm->t2 = c->t2;
2330
2331
2332
2333
2334
2335 if (need_restart)
2336 gsm_activate_mux(gsm);
2337 if (gsm->initiator && need_close)
2338 gsm_dlci_begin_open(gsm->dlci[0]);
2339 return 0;
2340}
2341
2342
2343
2344
2345
2346
2347
2348
2349
2350
2351
2352static int gsmld_output(struct gsm_mux *gsm, u8 *data, int len)
2353{
2354 if (tty_write_room(gsm->tty) < len) {
2355 set_bit(TTY_DO_WRITE_WAKEUP, &gsm->tty->flags);
2356 return -ENOSPC;
2357 }
2358 if (debug & 4)
2359 print_hex_dump_bytes("gsmld_output: ", DUMP_PREFIX_OFFSET,
2360 data, len);
2361 gsm->tty->ops->write(gsm->tty, data, len);
2362 return len;
2363}
2364
2365
2366
2367
2368
2369
2370
2371
2372
2373
2374
2375static int gsmld_attach_gsm(struct tty_struct *tty, struct gsm_mux *gsm)
2376{
2377 unsigned int base;
2378 int ret, i;
2379
2380 gsm->tty = tty_kref_get(tty);
2381 ret = gsm_activate_mux(gsm);
2382 if (ret != 0)
2383 tty_kref_put(gsm->tty);
2384 else {
2385
2386
2387 base = mux_num_to_base(gsm);
2388 for (i = 1; i < NUM_DLCI; i++) {
2389 struct device *dev;
2390
2391 dev = tty_register_device(gsm_tty_driver,
2392 base + i, NULL);
2393 if (IS_ERR(dev)) {
2394 for (i--; i >= 1; i--)
2395 tty_unregister_device(gsm_tty_driver,
2396 base + i);
2397 return PTR_ERR(dev);
2398 }
2399 }
2400 }
2401 return ret;
2402}
2403
2404
2405
2406
2407
2408
2409
2410
2411
2412
2413static void gsmld_detach_gsm(struct tty_struct *tty, struct gsm_mux *gsm)
2414{
2415 unsigned int base = mux_num_to_base(gsm);
2416 int i;
2417
2418 WARN_ON(tty != gsm->tty);
2419 for (i = 1; i < NUM_DLCI; i++)
2420 tty_unregister_device(gsm_tty_driver, base + i);
2421 gsm_cleanup_mux(gsm);
2422 tty_kref_put(gsm->tty);
2423 gsm->tty = NULL;
2424}
2425
2426static void gsmld_receive_buf(struct tty_struct *tty, const unsigned char *cp,
2427 const char *fp, int count)
2428{
2429 struct gsm_mux *gsm = tty->disc_data;
2430 char flags = TTY_NORMAL;
2431
2432 if (debug & 4)
2433 print_hex_dump_bytes("gsmld_receive: ", DUMP_PREFIX_OFFSET,
2434 cp, count);
2435
2436 for (; count; count--, cp++) {
2437 if (fp)
2438 flags = *fp++;
2439 switch (flags) {
2440 case TTY_NORMAL:
2441 gsm->receive(gsm, *cp);
2442 break;
2443 case TTY_OVERRUN:
2444 case TTY_BREAK:
2445 case TTY_PARITY:
2446 case TTY_FRAME:
2447 gsm_error(gsm, *cp, flags);
2448 break;
2449 default:
2450 WARN_ONCE(1, "%s: unknown flag %d\n",
2451 tty_name(tty), flags);
2452 break;
2453 }
2454 }
2455
2456
2457}
2458
2459
2460
2461
2462
2463
2464
2465
2466
2467
2468static void gsmld_flush_buffer(struct tty_struct *tty)
2469{
2470}
2471
2472
2473
2474
2475
2476
2477
2478
2479
2480
2481
2482static void gsmld_close(struct tty_struct *tty)
2483{
2484 struct gsm_mux *gsm = tty->disc_data;
2485
2486 gsmld_detach_gsm(tty, gsm);
2487
2488 gsmld_flush_buffer(tty);
2489
2490 mux_put(gsm);
2491}
2492
2493
2494
2495
2496
2497
2498
2499
2500
2501
2502
2503static int gsmld_open(struct tty_struct *tty)
2504{
2505 struct gsm_mux *gsm;
2506 int ret;
2507
2508 if (tty->ops->write == NULL)
2509 return -EINVAL;
2510
2511
2512 gsm = gsm_alloc_mux();
2513 if (gsm == NULL)
2514 return -ENOMEM;
2515
2516 tty->disc_data = gsm;
2517 tty->receive_room = 65536;
2518
2519
2520 gsm->encoding = 1;
2521
2522 ret = gsmld_attach_gsm(tty, gsm);
2523 if (ret != 0) {
2524 gsm_cleanup_mux(gsm);
2525 mux_put(gsm);
2526 }
2527 return ret;
2528}
2529
2530
2531
2532
2533
2534
2535
2536
2537
2538
2539static void gsmld_write_wakeup(struct tty_struct *tty)
2540{
2541 struct gsm_mux *gsm = tty->disc_data;
2542 unsigned long flags;
2543
2544
2545 clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
2546 spin_lock_irqsave(&gsm->tx_lock, flags);
2547 gsm_data_kick(gsm, NULL);
2548 if (gsm->tx_bytes < TX_THRESH_LO) {
2549 gsm_dlci_data_sweep(gsm);
2550 }
2551 spin_unlock_irqrestore(&gsm->tx_lock, flags);
2552}
2553
2554
2555
2556
2557
2558
2559
2560
2561
2562
2563
2564
2565
2566
2567
2568
2569
2570
2571static ssize_t gsmld_read(struct tty_struct *tty, struct file *file,
2572 unsigned char *buf, size_t nr,
2573 void **cookie, unsigned long offset)
2574{
2575 return -EOPNOTSUPP;
2576}
2577
2578
2579
2580
2581
2582
2583
2584
2585
2586
2587
2588
2589
2590
2591
2592static ssize_t gsmld_write(struct tty_struct *tty, struct file *file,
2593 const unsigned char *buf, size_t nr)
2594{
2595 int space = tty_write_room(tty);
2596 if (space >= nr)
2597 return tty->ops->write(tty, buf, nr);
2598 set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
2599 return -ENOBUFS;
2600}
2601
2602
2603
2604
2605
2606
2607
2608
2609
2610
2611
2612
2613
2614
2615
2616static __poll_t gsmld_poll(struct tty_struct *tty, struct file *file,
2617 poll_table *wait)
2618{
2619 __poll_t mask = 0;
2620 struct gsm_mux *gsm = tty->disc_data;
2621
2622 poll_wait(file, &tty->read_wait, wait);
2623 poll_wait(file, &tty->write_wait, wait);
2624 if (tty_hung_up_p(file))
2625 mask |= EPOLLHUP;
2626 if (!tty_is_writelocked(tty) && tty_write_room(tty) > 0)
2627 mask |= EPOLLOUT | EPOLLWRNORM;
2628 if (gsm->dead)
2629 mask |= EPOLLHUP;
2630 return mask;
2631}
2632
2633static int gsmld_ioctl(struct tty_struct *tty, struct file *file,
2634 unsigned int cmd, unsigned long arg)
2635{
2636 struct gsm_config c;
2637 struct gsm_mux *gsm = tty->disc_data;
2638 unsigned int base;
2639
2640 switch (cmd) {
2641 case GSMIOC_GETCONF:
2642 gsm_copy_config_values(gsm, &c);
2643 if (copy_to_user((void __user *)arg, &c, sizeof(c)))
2644 return -EFAULT;
2645 return 0;
2646 case GSMIOC_SETCONF:
2647 if (copy_from_user(&c, (void __user *)arg, sizeof(c)))
2648 return -EFAULT;
2649 return gsm_config(gsm, &c);
2650 case GSMIOC_GETFIRST:
2651 base = mux_num_to_base(gsm);
2652 return put_user(base + 1, (__u32 __user *)arg);
2653 default:
2654 return n_tty_ioctl_helper(tty, file, cmd, arg);
2655 }
2656}
2657
2658
2659
2660
2661
2662
2663static int gsm_mux_net_open(struct net_device *net)
2664{
2665 pr_debug("%s called\n", __func__);
2666 netif_start_queue(net);
2667 return 0;
2668}
2669
2670static int gsm_mux_net_close(struct net_device *net)
2671{
2672 netif_stop_queue(net);
2673 return 0;
2674}
2675
2676static void dlci_net_free(struct gsm_dlci *dlci)
2677{
2678 if (!dlci->net) {
2679 WARN_ON(1);
2680 return;
2681 }
2682 dlci->adaption = dlci->prev_adaption;
2683 dlci->data = dlci->prev_data;
2684 free_netdev(dlci->net);
2685 dlci->net = NULL;
2686}
2687static void net_free(struct kref *ref)
2688{
2689 struct gsm_mux_net *mux_net;
2690 struct gsm_dlci *dlci;
2691
2692 mux_net = container_of(ref, struct gsm_mux_net, ref);
2693 dlci = mux_net->dlci;
2694
2695 if (dlci->net) {
2696 unregister_netdev(dlci->net);
2697 dlci_net_free(dlci);
2698 }
2699}
2700
2701static inline void muxnet_get(struct gsm_mux_net *mux_net)
2702{
2703 kref_get(&mux_net->ref);
2704}
2705
2706static inline void muxnet_put(struct gsm_mux_net *mux_net)
2707{
2708 kref_put(&mux_net->ref, net_free);
2709}
2710
2711static netdev_tx_t gsm_mux_net_start_xmit(struct sk_buff *skb,
2712 struct net_device *net)
2713{
2714 struct gsm_mux_net *mux_net = netdev_priv(net);
2715 struct gsm_dlci *dlci = mux_net->dlci;
2716 muxnet_get(mux_net);
2717
2718 skb_queue_head(&dlci->skb_list, skb);
2719 net->stats.tx_packets++;
2720 net->stats.tx_bytes += skb->len;
2721 gsm_dlci_data_kick(dlci);
2722
2723 netif_trans_update(net);
2724 muxnet_put(mux_net);
2725 return NETDEV_TX_OK;
2726}
2727
2728
2729static void gsm_mux_net_tx_timeout(struct net_device *net, unsigned int txqueue)
2730{
2731
2732 dev_dbg(&net->dev, "Tx timed out.\n");
2733
2734
2735 net->stats.tx_errors++;
2736}
2737
2738static void gsm_mux_rx_netchar(struct gsm_dlci *dlci,
2739 const unsigned char *in_buf, int size)
2740{
2741 struct net_device *net = dlci->net;
2742 struct sk_buff *skb;
2743 struct gsm_mux_net *mux_net = netdev_priv(net);
2744 muxnet_get(mux_net);
2745
2746
2747 skb = dev_alloc_skb(size + NET_IP_ALIGN);
2748 if (!skb) {
2749
2750 net->stats.rx_dropped++;
2751 muxnet_put(mux_net);
2752 return;
2753 }
2754 skb_reserve(skb, NET_IP_ALIGN);
2755 skb_put_data(skb, in_buf, size);
2756
2757 skb->dev = net;
2758 skb->protocol = htons(ETH_P_IP);
2759
2760
2761 netif_rx(skb);
2762
2763
2764 net->stats.rx_packets++;
2765 net->stats.rx_bytes += size;
2766 muxnet_put(mux_net);
2767 return;
2768}
2769
2770static void gsm_mux_net_init(struct net_device *net)
2771{
2772 static const struct net_device_ops gsm_netdev_ops = {
2773 .ndo_open = gsm_mux_net_open,
2774 .ndo_stop = gsm_mux_net_close,
2775 .ndo_start_xmit = gsm_mux_net_start_xmit,
2776 .ndo_tx_timeout = gsm_mux_net_tx_timeout,
2777 };
2778
2779 net->netdev_ops = &gsm_netdev_ops;
2780
2781
2782 net->watchdog_timeo = GSM_NET_TX_TIMEOUT;
2783 net->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
2784 net->type = ARPHRD_NONE;
2785 net->tx_queue_len = 10;
2786}
2787
2788
2789
2790static void gsm_destroy_network(struct gsm_dlci *dlci)
2791{
2792 struct gsm_mux_net *mux_net;
2793
2794 pr_debug("destroy network interface\n");
2795 if (!dlci->net)
2796 return;
2797 mux_net = netdev_priv(dlci->net);
2798 muxnet_put(mux_net);
2799}
2800
2801
2802
2803static int gsm_create_network(struct gsm_dlci *dlci, struct gsm_netconfig *nc)
2804{
2805 char *netname;
2806 int retval = 0;
2807 struct net_device *net;
2808 struct gsm_mux_net *mux_net;
2809
2810 if (!capable(CAP_NET_ADMIN))
2811 return -EPERM;
2812
2813
2814 if (dlci->adaption > 2)
2815 return -EBUSY;
2816
2817 if (nc->protocol != htons(ETH_P_IP))
2818 return -EPROTONOSUPPORT;
2819
2820 if (nc->adaption != 3 && nc->adaption != 4)
2821 return -EPROTONOSUPPORT;
2822
2823 pr_debug("create network interface\n");
2824
2825 netname = "gsm%d";
2826 if (nc->if_name[0] != '\0')
2827 netname = nc->if_name;
2828 net = alloc_netdev(sizeof(struct gsm_mux_net), netname,
2829 NET_NAME_UNKNOWN, gsm_mux_net_init);
2830 if (!net) {
2831 pr_err("alloc_netdev failed\n");
2832 return -ENOMEM;
2833 }
2834 net->mtu = dlci->gsm->mtu;
2835 net->min_mtu = 8;
2836 net->max_mtu = dlci->gsm->mtu;
2837 mux_net = netdev_priv(net);
2838 mux_net->dlci = dlci;
2839 kref_init(&mux_net->ref);
2840 strncpy(nc->if_name, net->name, IFNAMSIZ);
2841
2842
2843 dlci->prev_adaption = dlci->adaption;
2844 dlci->prev_data = dlci->data;
2845 dlci->adaption = nc->adaption;
2846 dlci->data = gsm_mux_rx_netchar;
2847 dlci->net = net;
2848
2849 pr_debug("register netdev\n");
2850 retval = register_netdev(net);
2851 if (retval) {
2852 pr_err("network register fail %d\n", retval);
2853 dlci_net_free(dlci);
2854 return retval;
2855 }
2856 return net->ifindex;
2857}
2858
2859
2860static struct tty_ldisc_ops tty_ldisc_packet = {
2861 .owner = THIS_MODULE,
2862 .num = N_GSM0710,
2863 .name = "n_gsm",
2864 .open = gsmld_open,
2865 .close = gsmld_close,
2866 .flush_buffer = gsmld_flush_buffer,
2867 .read = gsmld_read,
2868 .write = gsmld_write,
2869 .ioctl = gsmld_ioctl,
2870 .poll = gsmld_poll,
2871 .receive_buf = gsmld_receive_buf,
2872 .write_wakeup = gsmld_write_wakeup
2873};
2874
2875
2876
2877
2878
2879#define TX_SIZE 512
2880
2881static int gsmtty_modem_update(struct gsm_dlci *dlci, u8 brk)
2882{
2883 u8 modembits[5];
2884 struct gsm_control *ctrl;
2885 int len = 2;
2886
2887 if (brk)
2888 len++;
2889
2890 modembits[0] = len << 1 | EA;
2891 modembits[1] = dlci->addr << 2 | 3;
2892 modembits[2] = gsm_encode_modem(dlci) << 1 | EA;
2893 if (brk)
2894 modembits[3] = brk << 4 | 2 | EA;
2895 ctrl = gsm_control_send(dlci->gsm, CMD_MSC, modembits, len + 1);
2896 if (ctrl == NULL)
2897 return -ENOMEM;
2898 return gsm_control_wait(dlci->gsm, ctrl);
2899}
2900
2901static int gsm_carrier_raised(struct tty_port *port)
2902{
2903 struct gsm_dlci *dlci = container_of(port, struct gsm_dlci, port);
2904 struct gsm_mux *gsm = dlci->gsm;
2905
2906
2907 if (dlci->state != DLCI_OPEN)
2908 return 0;
2909 if (debug & 2)
2910 return 1;
2911
2912
2913
2914
2915
2916 if (gsm->encoding == 0 && gsm->dlci[0]->mode == DLCI_MODE_ADM &&
2917 !dlci->modem_rx)
2918 return 1;
2919
2920 return dlci->modem_rx & TIOCM_CD;
2921}
2922
2923static void gsm_dtr_rts(struct tty_port *port, int onoff)
2924{
2925 struct gsm_dlci *dlci = container_of(port, struct gsm_dlci, port);
2926 unsigned int modem_tx = dlci->modem_tx;
2927 if (onoff)
2928 modem_tx |= TIOCM_DTR | TIOCM_RTS;
2929 else
2930 modem_tx &= ~(TIOCM_DTR | TIOCM_RTS);
2931 if (modem_tx != dlci->modem_tx) {
2932 dlci->modem_tx = modem_tx;
2933 gsmtty_modem_update(dlci, 0);
2934 }
2935}
2936
2937static const struct tty_port_operations gsm_port_ops = {
2938 .carrier_raised = gsm_carrier_raised,
2939 .dtr_rts = gsm_dtr_rts,
2940 .destruct = gsm_dlci_free,
2941};
2942
2943static int gsmtty_install(struct tty_driver *driver, struct tty_struct *tty)
2944{
2945 struct gsm_mux *gsm;
2946 struct gsm_dlci *dlci;
2947 unsigned int line = tty->index;
2948 unsigned int mux = mux_line_to_num(line);
2949 bool alloc = false;
2950 int ret;
2951
2952 line = line & 0x3F;
2953
2954 if (mux >= MAX_MUX)
2955 return -ENXIO;
2956
2957 if (gsm_mux[mux] == NULL)
2958 return -EUNATCH;
2959 if (line == 0 || line > 61)
2960 return -ECHRNG;
2961 gsm = gsm_mux[mux];
2962 if (gsm->dead)
2963 return -EL2HLT;
2964
2965
2966
2967
2968 mutex_lock(&gsm->mutex);
2969 if (gsm->dlci[0] && gsm->dlci[0]->state != DLCI_OPEN) {
2970 mutex_unlock(&gsm->mutex);
2971 return -EL2NSYNC;
2972 }
2973 dlci = gsm->dlci[line];
2974 if (dlci == NULL) {
2975 alloc = true;
2976 dlci = gsm_dlci_alloc(gsm, line);
2977 }
2978 if (dlci == NULL) {
2979 mutex_unlock(&gsm->mutex);
2980 return -ENOMEM;
2981 }
2982 ret = tty_port_install(&dlci->port, driver, tty);
2983 if (ret) {
2984 if (alloc)
2985 dlci_put(dlci);
2986 mutex_unlock(&gsm->mutex);
2987 return ret;
2988 }
2989
2990 dlci_get(dlci);
2991 dlci_get(gsm->dlci[0]);
2992 mux_get(gsm);
2993 tty->driver_data = dlci;
2994 mutex_unlock(&gsm->mutex);
2995
2996 return 0;
2997}
2998
2999static int gsmtty_open(struct tty_struct *tty, struct file *filp)
3000{
3001 struct gsm_dlci *dlci = tty->driver_data;
3002 struct tty_port *port = &dlci->port;
3003
3004 port->count++;
3005 tty_port_tty_set(port, tty);
3006
3007 dlci->modem_rx = 0;
3008
3009
3010 tty_port_set_initialized(port, 1);
3011
3012 gsm_dlci_begin_open(dlci);
3013
3014 return tty_port_block_til_ready(port, tty, filp);
3015}
3016
3017static void gsmtty_close(struct tty_struct *tty, struct file *filp)
3018{
3019 struct gsm_dlci *dlci = tty->driver_data;
3020
3021 if (dlci == NULL)
3022 return;
3023 if (dlci->state == DLCI_CLOSED)
3024 return;
3025 mutex_lock(&dlci->mutex);
3026 gsm_destroy_network(dlci);
3027 mutex_unlock(&dlci->mutex);
3028 if (tty_port_close_start(&dlci->port, tty, filp) == 0)
3029 return;
3030 gsm_dlci_begin_close(dlci);
3031 if (tty_port_initialized(&dlci->port) && C_HUPCL(tty))
3032 tty_port_lower_dtr_rts(&dlci->port);
3033 tty_port_close_end(&dlci->port, tty);
3034 tty_port_tty_set(&dlci->port, NULL);
3035 return;
3036}
3037
3038static void gsmtty_hangup(struct tty_struct *tty)
3039{
3040 struct gsm_dlci *dlci = tty->driver_data;
3041 if (dlci->state == DLCI_CLOSED)
3042 return;
3043 tty_port_hangup(&dlci->port);
3044 gsm_dlci_begin_close(dlci);
3045}
3046
3047static int gsmtty_write(struct tty_struct *tty, const unsigned char *buf,
3048 int len)
3049{
3050 int sent;
3051 struct gsm_dlci *dlci = tty->driver_data;
3052 if (dlci->state == DLCI_CLOSED)
3053 return -EINVAL;
3054
3055 sent = kfifo_in_locked(&dlci->fifo, buf, len, &dlci->lock);
3056
3057 gsm_dlci_data_kick(dlci);
3058 return sent;
3059}
3060
3061static unsigned int gsmtty_write_room(struct tty_struct *tty)
3062{
3063 struct gsm_dlci *dlci = tty->driver_data;
3064 if (dlci->state == DLCI_CLOSED)
3065 return 0;
3066 return TX_SIZE - kfifo_len(&dlci->fifo);
3067}
3068
3069static unsigned int gsmtty_chars_in_buffer(struct tty_struct *tty)
3070{
3071 struct gsm_dlci *dlci = tty->driver_data;
3072 if (dlci->state == DLCI_CLOSED)
3073 return 0;
3074 return kfifo_len(&dlci->fifo);
3075}
3076
3077static void gsmtty_flush_buffer(struct tty_struct *tty)
3078{
3079 struct gsm_dlci *dlci = tty->driver_data;
3080 if (dlci->state == DLCI_CLOSED)
3081 return;
3082
3083
3084
3085
3086 kfifo_reset(&dlci->fifo);
3087
3088}
3089
3090static void gsmtty_wait_until_sent(struct tty_struct *tty, int timeout)
3091{
3092
3093
3094
3095}
3096
3097static int gsmtty_tiocmget(struct tty_struct *tty)
3098{
3099 struct gsm_dlci *dlci = tty->driver_data;
3100 if (dlci->state == DLCI_CLOSED)
3101 return -EINVAL;
3102 return dlci->modem_rx;
3103}
3104
3105static int gsmtty_tiocmset(struct tty_struct *tty,
3106 unsigned int set, unsigned int clear)
3107{
3108 struct gsm_dlci *dlci = tty->driver_data;
3109 unsigned int modem_tx = dlci->modem_tx;
3110
3111 if (dlci->state == DLCI_CLOSED)
3112 return -EINVAL;
3113 modem_tx &= ~clear;
3114 modem_tx |= set;
3115
3116 if (modem_tx != dlci->modem_tx) {
3117 dlci->modem_tx = modem_tx;
3118 return gsmtty_modem_update(dlci, 0);
3119 }
3120 return 0;
3121}
3122
3123
3124static int gsmtty_ioctl(struct tty_struct *tty,
3125 unsigned int cmd, unsigned long arg)
3126{
3127 struct gsm_dlci *dlci = tty->driver_data;
3128 struct gsm_netconfig nc;
3129 int index;
3130
3131 if (dlci->state == DLCI_CLOSED)
3132 return -EINVAL;
3133 switch (cmd) {
3134 case GSMIOC_ENABLE_NET:
3135 if (copy_from_user(&nc, (void __user *)arg, sizeof(nc)))
3136 return -EFAULT;
3137 nc.if_name[IFNAMSIZ-1] = '\0';
3138
3139 mutex_lock(&dlci->mutex);
3140 index = gsm_create_network(dlci, &nc);
3141 mutex_unlock(&dlci->mutex);
3142 if (copy_to_user((void __user *)arg, &nc, sizeof(nc)))
3143 return -EFAULT;
3144 return index;
3145 case GSMIOC_DISABLE_NET:
3146 if (!capable(CAP_NET_ADMIN))
3147 return -EPERM;
3148 mutex_lock(&dlci->mutex);
3149 gsm_destroy_network(dlci);
3150 mutex_unlock(&dlci->mutex);
3151 return 0;
3152 default:
3153 return -ENOIOCTLCMD;
3154 }
3155}
3156
3157static void gsmtty_set_termios(struct tty_struct *tty, struct ktermios *old)
3158{
3159 struct gsm_dlci *dlci = tty->driver_data;
3160 if (dlci->state == DLCI_CLOSED)
3161 return;
3162
3163
3164
3165
3166
3167 tty_termios_copy_hw(&tty->termios, old);
3168}
3169
3170static void gsmtty_throttle(struct tty_struct *tty)
3171{
3172 struct gsm_dlci *dlci = tty->driver_data;
3173 if (dlci->state == DLCI_CLOSED)
3174 return;
3175 if (C_CRTSCTS(tty))
3176 dlci->modem_tx &= ~TIOCM_DTR;
3177 dlci->throttled = true;
3178
3179 gsmtty_modem_update(dlci, 0);
3180}
3181
3182static void gsmtty_unthrottle(struct tty_struct *tty)
3183{
3184 struct gsm_dlci *dlci = tty->driver_data;
3185 if (dlci->state == DLCI_CLOSED)
3186 return;
3187 if (C_CRTSCTS(tty))
3188 dlci->modem_tx |= TIOCM_DTR;
3189 dlci->throttled = false;
3190
3191 gsmtty_modem_update(dlci, 0);
3192}
3193
3194static int gsmtty_break_ctl(struct tty_struct *tty, int state)
3195{
3196 struct gsm_dlci *dlci = tty->driver_data;
3197 int encode = 0;
3198 if (dlci->state == DLCI_CLOSED)
3199 return -EINVAL;
3200
3201 if (state == -1)
3202
3203 encode = 0x0F;
3204 else if (state > 0) {
3205 encode = state / 200;
3206 if (encode > 0x0F)
3207 encode = 0x0F;
3208 }
3209 return gsmtty_modem_update(dlci, encode);
3210}
3211
3212static void gsmtty_cleanup(struct tty_struct *tty)
3213{
3214 struct gsm_dlci *dlci = tty->driver_data;
3215 struct gsm_mux *gsm = dlci->gsm;
3216
3217 dlci_put(dlci);
3218 dlci_put(gsm->dlci[0]);
3219 mux_put(gsm);
3220}
3221
3222
3223static const struct tty_operations gsmtty_ops = {
3224 .install = gsmtty_install,
3225 .open = gsmtty_open,
3226 .close = gsmtty_close,
3227 .write = gsmtty_write,
3228 .write_room = gsmtty_write_room,
3229 .chars_in_buffer = gsmtty_chars_in_buffer,
3230 .flush_buffer = gsmtty_flush_buffer,
3231 .ioctl = gsmtty_ioctl,
3232 .throttle = gsmtty_throttle,
3233 .unthrottle = gsmtty_unthrottle,
3234 .set_termios = gsmtty_set_termios,
3235 .hangup = gsmtty_hangup,
3236 .wait_until_sent = gsmtty_wait_until_sent,
3237 .tiocmget = gsmtty_tiocmget,
3238 .tiocmset = gsmtty_tiocmset,
3239 .break_ctl = gsmtty_break_ctl,
3240 .cleanup = gsmtty_cleanup,
3241};
3242
3243
3244
3245static int __init gsm_init(void)
3246{
3247
3248 int status = tty_register_ldisc(&tty_ldisc_packet);
3249 if (status != 0) {
3250 pr_err("n_gsm: can't register line discipline (err = %d)\n",
3251 status);
3252 return status;
3253 }
3254
3255 gsm_tty_driver = tty_alloc_driver(256, TTY_DRIVER_REAL_RAW |
3256 TTY_DRIVER_DYNAMIC_DEV | TTY_DRIVER_HARDWARE_BREAK);
3257 if (IS_ERR(gsm_tty_driver)) {
3258 pr_err("gsm_init: tty allocation failed.\n");
3259 status = PTR_ERR(gsm_tty_driver);
3260 goto err_unreg_ldisc;
3261 }
3262 gsm_tty_driver->driver_name = "gsmtty";
3263 gsm_tty_driver->name = "gsmtty";
3264 gsm_tty_driver->major = 0;
3265 gsm_tty_driver->minor_start = 0;
3266 gsm_tty_driver->type = TTY_DRIVER_TYPE_SERIAL;
3267 gsm_tty_driver->subtype = SERIAL_TYPE_NORMAL;
3268 gsm_tty_driver->init_termios = tty_std_termios;
3269
3270 gsm_tty_driver->init_termios.c_lflag &= ~ECHO;
3271 tty_set_operations(gsm_tty_driver, &gsmtty_ops);
3272
3273 if (tty_register_driver(gsm_tty_driver)) {
3274 pr_err("gsm_init: tty registration failed.\n");
3275 status = -EBUSY;
3276 goto err_put_driver;
3277 }
3278 pr_debug("gsm_init: loaded as %d,%d.\n",
3279 gsm_tty_driver->major, gsm_tty_driver->minor_start);
3280 return 0;
3281err_put_driver:
3282 tty_driver_kref_put(gsm_tty_driver);
3283err_unreg_ldisc:
3284 tty_unregister_ldisc(&tty_ldisc_packet);
3285 return status;
3286}
3287
3288static void __exit gsm_exit(void)
3289{
3290 tty_unregister_ldisc(&tty_ldisc_packet);
3291 tty_unregister_driver(gsm_tty_driver);
3292 tty_driver_kref_put(gsm_tty_driver);
3293}
3294
3295module_init(gsm_init);
3296module_exit(gsm_exit);
3297
3298
3299MODULE_LICENSE("GPL");
3300MODULE_ALIAS_LDISC(N_GSM0710);
3301