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