tty: ldops: unify to u8
[linux-2.6-block.git] / drivers / tty / n_gsm.c
1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * n_gsm.c GSM 0710 tty multiplexor
4  * Copyright (c) 2009/10 Intel Corporation
5  *
6  *      * THIS IS A DEVELOPMENT SNAPSHOT IT IS NOT A FINAL RELEASE *
7  *
8  * Outgoing path:
9  * tty -> DLCI fifo -> scheduler -> GSM MUX data queue    ---o-> ldisc
10  * control message               -> GSM MUX control queue --ยด
11  *
12  * Incoming path:
13  * ldisc -> gsm_queue() -o--> tty
14  *                        `-> gsm_control_response()
15  *
16  * TO DO:
17  *      Mostly done:    ioctls for setting modes/timing
18  *      Partly done:    hooks so you can pull off frames to non tty devs
19  *      Restart DLCI 0 when it closes ?
20  *      Improve the tx engine
21  *      Resolve tx side locking by adding a queue_head and routing
22  *              all control traffic via it
23  *      General tidy/document
24  *      Review the locking/move to refcounts more (mux now moved to an
25  *              alloc/free model ready)
26  *      Use newest tty open/close port helpers and install hooks
27  *      What to do about power functions ?
28  *      Termios setting and negotiation
29  *      Do we need a 'which mux are you' ioctl to correlate mux and tty sets
30  *
31  */
32
33 #include <linux/types.h>
34 #include <linux/major.h>
35 #include <linux/errno.h>
36 #include <linux/signal.h>
37 #include <linux/fcntl.h>
38 #include <linux/sched/signal.h>
39 #include <linux/interrupt.h>
40 #include <linux/tty.h>
41 #include <linux/bitfield.h>
42 #include <linux/ctype.h>
43 #include <linux/mm.h>
44 #include <linux/math.h>
45 #include <linux/nospec.h>
46 #include <linux/string.h>
47 #include <linux/slab.h>
48 #include <linux/poll.h>
49 #include <linux/bitops.h>
50 #include <linux/file.h>
51 #include <linux/uaccess.h>
52 #include <linux/module.h>
53 #include <linux/timer.h>
54 #include <linux/tty_flip.h>
55 #include <linux/tty_driver.h>
56 #include <linux/serial.h>
57 #include <linux/kfifo.h>
58 #include <linux/skbuff.h>
59 #include <net/arp.h>
60 #include <linux/ip.h>
61 #include <linux/netdevice.h>
62 #include <linux/etherdevice.h>
63 #include <linux/gsmmux.h>
64 #include "tty.h"
65
66 static int debug;
67 module_param(debug, int, 0600);
68
69 /* Module debug bits */
70 #define DBG_DUMP        BIT(0) /* Data transmission dump. */
71 #define DBG_CD_ON       BIT(1) /* Always assume CD line on. */
72 #define DBG_DATA        BIT(2) /* Data transmission details. */
73 #define DBG_ERRORS      BIT(3) /* Details for fail conditions. */
74 #define DBG_TTY         BIT(4) /* Transmission statistics for DLCI TTYs. */
75 #define DBG_PAYLOAD     BIT(5) /* Limits DBG_DUMP to payload frames. */
76
77 /* Defaults: these are from the specification */
78
79 #define T1      10              /* 100mS */
80 #define T2      34              /* 333mS */
81 #define T3      10              /* 10s */
82 #define N2      3               /* Retry 3 times */
83 #define K       2               /* outstanding I frames */
84
85 #define MAX_T3 255              /* In seconds. */
86 #define MAX_WINDOW_SIZE 7       /* Limit of K in error recovery mode. */
87
88 /* Use long timers for testing at low speed with debug on */
89 #ifdef DEBUG_TIMING
90 #define T1      100
91 #define T2      200
92 #endif
93
94 /*
95  * Semi-arbitrary buffer size limits. 0710 is normally run with 32-64 byte
96  * limits so this is plenty
97  */
98 #define MAX_MRU 1500
99 #define MAX_MTU 1500
100 #define MIN_MTU (PROT_OVERHEAD + 1)
101 /* SOF, ADDR, CTRL, LEN1, LEN2, ..., FCS, EOF */
102 #define PROT_OVERHEAD 7
103 #define GSM_NET_TX_TIMEOUT (HZ*10)
104
105 /*
106  *      struct gsm_mux_net      -       network interface
107  *
108  *      Created when net interface is initialized.
109  */
110 struct gsm_mux_net {
111         struct kref ref;
112         struct gsm_dlci *dlci;
113 };
114
115 /*
116  *      Each block of data we have queued to go out is in the form of
117  *      a gsm_msg which holds everything we need in a link layer independent
118  *      format
119  */
120
121 struct gsm_msg {
122         struct list_head list;
123         u8 addr;                /* DLCI address + flags */
124         u8 ctrl;                /* Control byte + flags */
125         unsigned int len;       /* Length of data block (can be zero) */
126         unsigned char *data;    /* Points into buffer but not at the start */
127         unsigned char buffer[];
128 };
129
130 enum gsm_dlci_state {
131         DLCI_CLOSED,
132         DLCI_WAITING_CONFIG,    /* Waiting for DLCI configuration from user */
133         DLCI_CONFIGURE,         /* Sending PN (for adaption > 1) */
134         DLCI_OPENING,           /* Sending SABM not seen UA */
135         DLCI_OPEN,              /* SABM/UA complete */
136         DLCI_CLOSING,           /* Sending DISC not seen UA/DM */
137 };
138
139 enum gsm_dlci_mode {
140         DLCI_MODE_ABM,          /* Normal Asynchronous Balanced Mode */
141         DLCI_MODE_ADM,          /* Asynchronous Disconnected Mode */
142 };
143
144 /*
145  *      Each active data link has a gsm_dlci structure associated which ties
146  *      the link layer to an optional tty (if the tty side is open). To avoid
147  *      complexity right now these are only ever freed up when the mux is
148  *      shut down.
149  *
150  *      At the moment we don't free DLCI objects until the mux is torn down
151  *      this avoid object life time issues but might be worth review later.
152  */
153
154 struct gsm_dlci {
155         struct gsm_mux *gsm;
156         int addr;
157         enum gsm_dlci_state state;
158         struct mutex mutex;
159
160         /* Link layer */
161         enum gsm_dlci_mode mode;
162         spinlock_t lock;        /* Protects the internal state */
163         struct timer_list t1;   /* Retransmit timer for SABM and UA */
164         int retries;
165         /* Uplink tty if active */
166         struct tty_port port;   /* The tty bound to this DLCI if there is one */
167 #define TX_SIZE         4096    /* Must be power of 2. */
168         struct kfifo fifo;      /* Queue fifo for the DLCI */
169         int adaption;           /* Adaption layer in use */
170         int prev_adaption;
171         u32 modem_rx;           /* Our incoming virtual modem lines */
172         u32 modem_tx;           /* Our outgoing modem lines */
173         unsigned int mtu;
174         bool dead;              /* Refuse re-open */
175         /* Configuration */
176         u8 prio;                /* Priority */
177         u8 ftype;               /* Frame type */
178         u8 k;                   /* Window size */
179         /* Flow control */
180         bool throttled;         /* Private copy of throttle state */
181         bool constipated;       /* Throttle status for outgoing */
182         /* Packetised I/O */
183         struct sk_buff *skb;    /* Frame being sent */
184         struct sk_buff_head skb_list;   /* Queued frames */
185         /* Data handling callback */
186         void (*data)(struct gsm_dlci *dlci, const u8 *data, int len);
187         void (*prev_data)(struct gsm_dlci *dlci, const u8 *data, int len);
188         struct net_device *net; /* network interface, if created */
189 };
190
191 /*
192  * Parameter bits used for parameter negotiation according to 3GPP 27.010
193  * chapter 5.4.6.3.1.
194  */
195
196 struct gsm_dlci_param_bits {
197         u8 d_bits;
198         u8 i_cl_bits;
199         u8 p_bits;
200         u8 t_bits;
201         __le16 n_bits;
202         u8 na_bits;
203         u8 k_bits;
204 };
205
206 static_assert(sizeof(struct gsm_dlci_param_bits) == 8);
207
208 #define PN_D_FIELD_DLCI         GENMASK(5, 0)
209 #define PN_I_CL_FIELD_FTYPE     GENMASK(3, 0)
210 #define PN_I_CL_FIELD_ADAPTION  GENMASK(7, 4)
211 #define PN_P_FIELD_PRIO         GENMASK(5, 0)
212 #define PN_T_FIELD_T1           GENMASK(7, 0)
213 #define PN_N_FIELD_N1           GENMASK(15, 0)
214 #define PN_NA_FIELD_N2          GENMASK(7, 0)
215 #define PN_K_FIELD_K            GENMASK(2, 0)
216
217 /* Total number of supported devices */
218 #define GSM_TTY_MINORS          256
219
220 /* DLCI 0, 62/63 are special or reserved see gsmtty_open */
221
222 #define NUM_DLCI                64
223
224 /*
225  *      DLCI 0 is used to pass control blocks out of band of the data
226  *      flow (and with a higher link priority). One command can be outstanding
227  *      at a time and we use this structure to manage them. They are created
228  *      and destroyed by the user context, and updated by the receive paths
229  *      and timers
230  */
231
232 struct gsm_control {
233         u8 cmd;         /* Command we are issuing */
234         u8 *data;       /* Data for the command in case we retransmit */
235         int len;        /* Length of block for retransmission */
236         int done;       /* Done flag */
237         int error;      /* Error if any */
238 };
239
240 enum gsm_encoding {
241         GSM_BASIC_OPT,
242         GSM_ADV_OPT,
243 };
244
245 enum gsm_mux_state {
246         GSM_SEARCH,
247         GSM_START,
248         GSM_ADDRESS,
249         GSM_CONTROL,
250         GSM_LEN,
251         GSM_DATA,
252         GSM_FCS,
253         GSM_OVERRUN,
254         GSM_LEN0,
255         GSM_LEN1,
256         GSM_SSOF,
257 };
258
259 /*
260  *      Each GSM mux we have is represented by this structure. If we are
261  *      operating as an ldisc then we use this structure as our ldisc
262  *      state. We need to sort out lifetimes and locking with respect
263  *      to the gsm mux array. For now we don't free DLCI objects that
264  *      have been instantiated until the mux itself is terminated.
265  *
266  *      To consider further: tty open versus mux shutdown.
267  */
268
269 struct gsm_mux {
270         struct tty_struct *tty;         /* The tty our ldisc is bound to */
271         spinlock_t lock;
272         struct mutex mutex;
273         unsigned int num;
274         struct kref ref;
275
276         /* Events on the GSM channel */
277         wait_queue_head_t event;
278
279         /* ldisc send work */
280         struct work_struct tx_work;
281
282         /* Bits for GSM mode decoding */
283
284         /* Framing Layer */
285         unsigned char *buf;
286         enum gsm_mux_state state;
287         unsigned int len;
288         unsigned int address;
289         unsigned int count;
290         bool escape;
291         enum gsm_encoding encoding;
292         u8 control;
293         u8 fcs;
294         u8 *txframe;                    /* TX framing buffer */
295
296         /* Method for the receiver side */
297         void (*receive)(struct gsm_mux *gsm, u8 ch);
298
299         /* Link Layer */
300         unsigned int mru;
301         unsigned int mtu;
302         int initiator;                  /* Did we initiate connection */
303         bool dead;                      /* Has the mux been shut down */
304         struct gsm_dlci *dlci[NUM_DLCI];
305         int old_c_iflag;                /* termios c_iflag value before attach */
306         bool constipated;               /* Asked by remote to shut up */
307         bool has_devices;               /* Devices were registered */
308
309         spinlock_t tx_lock;
310         unsigned int tx_bytes;          /* TX data outstanding */
311 #define TX_THRESH_HI            8192
312 #define TX_THRESH_LO            2048
313         struct list_head tx_ctrl_list;  /* Pending control packets */
314         struct list_head tx_data_list;  /* Pending data packets */
315
316         /* Control messages */
317         struct timer_list kick_timer;   /* Kick TX queuing on timeout */
318         struct timer_list t2_timer;     /* Retransmit timer for commands */
319         int cretries;                   /* Command retry counter */
320         struct gsm_control *pending_cmd;/* Our current pending command */
321         spinlock_t control_lock;        /* Protects the pending command */
322
323         /* Keep-alive */
324         struct timer_list ka_timer;     /* Keep-alive response timer */
325         u8 ka_num;                      /* Keep-alive match pattern */
326         signed int ka_retries;          /* Keep-alive retry counter, -1 if not yet initialized */
327
328         /* Configuration */
329         int adaption;           /* 1 or 2 supported */
330         u8 ftype;               /* UI or UIH */
331         int t1, t2;             /* Timers in 1/100th of a sec */
332         unsigned int t3;        /* Power wake-up timer in seconds. */
333         int n2;                 /* Retry count */
334         u8 k;                   /* Window size */
335         bool wait_config;       /* Wait for configuration by ioctl before DLCI open */
336         u32 keep_alive;         /* Control channel keep-alive in 10ms */
337
338         /* Statistics (not currently exposed) */
339         unsigned long bad_fcs;
340         unsigned long malformed;
341         unsigned long io_error;
342         unsigned long bad_size;
343         unsigned long unsupported;
344 };
345
346
347 /*
348  *      Mux objects - needed so that we can translate a tty index into the
349  *      relevant mux and DLCI.
350  */
351
352 #define MAX_MUX         4                       /* 256 minors */
353 static struct gsm_mux *gsm_mux[MAX_MUX];        /* GSM muxes */
354 static DEFINE_SPINLOCK(gsm_mux_lock);
355
356 static struct tty_driver *gsm_tty_driver;
357
358 /*
359  *      This section of the driver logic implements the GSM encodings
360  *      both the basic and the 'advanced'. Reliable transport is not
361  *      supported.
362  */
363
364 #define CR                      0x02
365 #define EA                      0x01
366 #define PF                      0x10
367
368 /* I is special: the rest are ..*/
369 #define RR                      0x01
370 #define UI                      0x03
371 #define RNR                     0x05
372 #define REJ                     0x09
373 #define DM                      0x0F
374 #define SABM                    0x2F
375 #define DISC                    0x43
376 #define UA                      0x63
377 #define UIH                     0xEF
378
379 /* Channel commands */
380 #define CMD_NSC                 0x09
381 #define CMD_TEST                0x11
382 #define CMD_PSC                 0x21
383 #define CMD_RLS                 0x29
384 #define CMD_FCOFF               0x31
385 #define CMD_PN                  0x41
386 #define CMD_RPN                 0x49
387 #define CMD_FCON                0x51
388 #define CMD_CLD                 0x61
389 #define CMD_SNC                 0x69
390 #define CMD_MSC                 0x71
391
392 /* Virtual modem bits */
393 #define MDM_FC                  0x01
394 #define MDM_RTC                 0x02
395 #define MDM_RTR                 0x04
396 #define MDM_IC                  0x20
397 #define MDM_DV                  0x40
398
399 #define GSM0_SOF                0xF9
400 #define GSM1_SOF                0x7E
401 #define GSM1_ESCAPE             0x7D
402 #define GSM1_ESCAPE_BITS        0x20
403 #define XON                     0x11
404 #define XOFF                    0x13
405 #define ISO_IEC_646_MASK        0x7F
406
407 static const struct tty_port_operations gsm_port_ops;
408
409 /*
410  *      CRC table for GSM 0710
411  */
412
413 static const u8 gsm_fcs8[256] = {
414         0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75,
415         0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
416         0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69,
417         0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
418         0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D,
419         0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
420         0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51,
421         0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
422         0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05,
423         0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
424         0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19,
425         0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
426         0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D,
427         0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
428         0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21,
429         0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
430         0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95,
431         0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
432         0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89,
433         0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
434         0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD,
435         0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
436         0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1,
437         0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
438         0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5,
439         0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
440         0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9,
441         0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
442         0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD,
443         0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
444         0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1,
445         0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF
446 };
447
448 #define INIT_FCS        0xFF
449 #define GOOD_FCS        0xCF
450
451 static void gsm_dlci_close(struct gsm_dlci *dlci);
452 static int gsmld_output(struct gsm_mux *gsm, u8 *data, int len);
453 static int gsm_modem_update(struct gsm_dlci *dlci, u8 brk);
454 static struct gsm_msg *gsm_data_alloc(struct gsm_mux *gsm, u8 addr, int len,
455                                                                 u8 ctrl);
456 static int gsm_send_packet(struct gsm_mux *gsm, struct gsm_msg *msg);
457 static struct gsm_dlci *gsm_dlci_alloc(struct gsm_mux *gsm, int addr);
458 static void gsmld_write_trigger(struct gsm_mux *gsm);
459 static void gsmld_write_task(struct work_struct *work);
460
461 /**
462  *      gsm_fcs_add     -       update FCS
463  *      @fcs: Current FCS
464  *      @c: Next data
465  *
466  *      Update the FCS to include c. Uses the algorithm in the specification
467  *      notes.
468  */
469
470 static inline u8 gsm_fcs_add(u8 fcs, u8 c)
471 {
472         return gsm_fcs8[fcs ^ c];
473 }
474
475 /**
476  *      gsm_fcs_add_block       -       update FCS for a block
477  *      @fcs: Current FCS
478  *      @c: buffer of data
479  *      @len: length of buffer
480  *
481  *      Update the FCS to include c. Uses the algorithm in the specification
482  *      notes.
483  */
484
485 static inline u8 gsm_fcs_add_block(u8 fcs, u8 *c, int len)
486 {
487         while (len--)
488                 fcs = gsm_fcs8[fcs ^ *c++];
489         return fcs;
490 }
491
492 /**
493  *      gsm_read_ea             -       read a byte into an EA
494  *      @val: variable holding value
495  *      @c: byte going into the EA
496  *
497  *      Processes one byte of an EA. Updates the passed variable
498  *      and returns 1 if the EA is now completely read
499  */
500
501 static int gsm_read_ea(unsigned int *val, u8 c)
502 {
503         /* Add the next 7 bits into the value */
504         *val <<= 7;
505         *val |= c >> 1;
506         /* Was this the last byte of the EA 1 = yes*/
507         return c & EA;
508 }
509
510 /**
511  *      gsm_read_ea_val -       read a value until EA
512  *      @val: variable holding value
513  *      @data: buffer of data
514  *      @dlen: length of data
515  *
516  *      Processes an EA value. Updates the passed variable and
517  *      returns the processed data length.
518  */
519 static unsigned int gsm_read_ea_val(unsigned int *val, const u8 *data, int dlen)
520 {
521         unsigned int len = 0;
522
523         for (; dlen > 0; dlen--) {
524                 len++;
525                 if (gsm_read_ea(val, *data++))
526                         break;
527         }
528         return len;
529 }
530
531 /**
532  *      gsm_encode_modem        -       encode modem data bits
533  *      @dlci: DLCI to encode from
534  *
535  *      Returns the correct GSM encoded modem status bits (6 bit field) for
536  *      the current status of the DLCI and attached tty object
537  */
538
539 static u8 gsm_encode_modem(const struct gsm_dlci *dlci)
540 {
541         u8 modembits = 0;
542         /* FC is true flow control not modem bits */
543         if (dlci->throttled)
544                 modembits |= MDM_FC;
545         if (dlci->modem_tx & TIOCM_DTR)
546                 modembits |= MDM_RTC;
547         if (dlci->modem_tx & TIOCM_RTS)
548                 modembits |= MDM_RTR;
549         if (dlci->modem_tx & TIOCM_RI)
550                 modembits |= MDM_IC;
551         if (dlci->modem_tx & TIOCM_CD || dlci->gsm->initiator)
552                 modembits |= MDM_DV;
553         /* special mappings for passive side to operate as UE */
554         if (dlci->modem_tx & TIOCM_OUT1)
555                 modembits |= MDM_IC;
556         if (dlci->modem_tx & TIOCM_OUT2)
557                 modembits |= MDM_DV;
558         return modembits;
559 }
560
561 static void gsm_hex_dump_bytes(const char *fname, const u8 *data,
562                                unsigned long len)
563 {
564         char *prefix;
565
566         if (!fname) {
567                 print_hex_dump(KERN_INFO, "", DUMP_PREFIX_NONE, 16, 1, data, len,
568                                true);
569                 return;
570         }
571
572         prefix = kasprintf(GFP_ATOMIC, "%s: ", fname);
573         if (!prefix)
574                 return;
575         print_hex_dump(KERN_INFO, prefix, DUMP_PREFIX_OFFSET, 16, 1, data, len,
576                        true);
577         kfree(prefix);
578 }
579
580 /**
581  * gsm_encode_params    -       encode DLCI parameters
582  * @dlci: DLCI to encode from
583  * @params: buffer to fill with the encoded parameters
584  *
585  * Encodes the parameters according to GSM 07.10 section 5.4.6.3.1
586  * table 3.
587  */
588 static int gsm_encode_params(const struct gsm_dlci *dlci,
589                              struct gsm_dlci_param_bits *params)
590 {
591         const struct gsm_mux *gsm = dlci->gsm;
592         unsigned int i, cl;
593
594         switch (dlci->ftype) {
595         case UIH:
596                 i = 0; /* UIH */
597                 break;
598         case UI:
599                 i = 1; /* UI */
600                 break;
601         default:
602                 pr_debug("unsupported frame type %d\n", dlci->ftype);
603                 return -EINVAL;
604         }
605
606         switch (dlci->adaption) {
607         case 1: /* Unstructured */
608                 cl = 0; /* convergence layer type 1 */
609                 break;
610         case 2: /* Unstructured with modem bits. */
611                 cl = 1; /* convergence layer type 2 */
612                 break;
613         default:
614                 pr_debug("unsupported adaption %d\n", dlci->adaption);
615                 return -EINVAL;
616         }
617
618         params->d_bits = FIELD_PREP(PN_D_FIELD_DLCI, dlci->addr);
619         /* UIH, convergence layer type 1 */
620         params->i_cl_bits = FIELD_PREP(PN_I_CL_FIELD_FTYPE, i) |
621                             FIELD_PREP(PN_I_CL_FIELD_ADAPTION, cl);
622         params->p_bits = FIELD_PREP(PN_P_FIELD_PRIO, dlci->prio);
623         params->t_bits = FIELD_PREP(PN_T_FIELD_T1, gsm->t1);
624         params->n_bits = cpu_to_le16(FIELD_PREP(PN_N_FIELD_N1, dlci->mtu));
625         params->na_bits = FIELD_PREP(PN_NA_FIELD_N2, gsm->n2);
626         params->k_bits = FIELD_PREP(PN_K_FIELD_K, dlci->k);
627
628         return 0;
629 }
630
631 /**
632  *      gsm_register_devices    -       register all tty devices for a given mux index
633  *
634  *      @driver: the tty driver that describes the tty devices
635  *      @index:  the mux number is used to calculate the minor numbers of the
636  *               ttys for this mux and may differ from the position in the
637  *               mux array.
638  */
639 static int gsm_register_devices(struct tty_driver *driver, unsigned int index)
640 {
641         struct device *dev;
642         int i;
643         unsigned int base;
644
645         if (!driver || index >= MAX_MUX)
646                 return -EINVAL;
647
648         base = index * NUM_DLCI; /* first minor for this index */
649         for (i = 1; i < NUM_DLCI; i++) {
650                 /* Don't register device 0 - this is the control channel
651                  * and not a usable tty interface
652                  */
653                 dev = tty_register_device(gsm_tty_driver, base + i, NULL);
654                 if (IS_ERR(dev)) {
655                         if (debug & DBG_ERRORS)
656                                 pr_info("%s failed to register device minor %u",
657                                         __func__, base + i);
658                         for (i--; i >= 1; i--)
659                                 tty_unregister_device(gsm_tty_driver, base + i);
660                         return PTR_ERR(dev);
661                 }
662         }
663
664         return 0;
665 }
666
667 /**
668  *      gsm_unregister_devices  -       unregister all tty devices for a given mux index
669  *
670  *      @driver: the tty driver that describes the tty devices
671  *      @index:  the mux number is used to calculate the minor numbers of the
672  *               ttys for this mux and may differ from the position in the
673  *               mux array.
674  */
675 static void gsm_unregister_devices(struct tty_driver *driver,
676                                    unsigned int index)
677 {
678         int i;
679         unsigned int base;
680
681         if (!driver || index >= MAX_MUX)
682                 return;
683
684         base = index * NUM_DLCI; /* first minor for this index */
685         for (i = 1; i < NUM_DLCI; i++) {
686                 /* Don't unregister device 0 - this is the control
687                  * channel and not a usable tty interface
688                  */
689                 tty_unregister_device(gsm_tty_driver, base + i);
690         }
691 }
692
693 /**
694  *      gsm_print_packet        -       display a frame for debug
695  *      @hdr: header to print before decode
696  *      @addr: address EA from the frame
697  *      @cr: C/R bit seen as initiator
698  *      @control: control including PF bit
699  *      @data: following data bytes
700  *      @dlen: length of data
701  *
702  *      Displays a packet in human readable format for debugging purposes. The
703  *      style is based on amateur radio LAP-B dump display.
704  */
705
706 static void gsm_print_packet(const char *hdr, int addr, int cr,
707                                         u8 control, const u8 *data, int dlen)
708 {
709         if (!(debug & DBG_DUMP))
710                 return;
711         /* Only show user payload frames if debug & DBG_PAYLOAD */
712         if (!(debug & DBG_PAYLOAD) && addr != 0)
713                 if ((control & ~PF) == UI || (control & ~PF) == UIH)
714                         return;
715
716         pr_info("%s %d) %c: ", hdr, addr, "RC"[cr]);
717
718         switch (control & ~PF) {
719         case SABM:
720                 pr_cont("SABM");
721                 break;
722         case UA:
723                 pr_cont("UA");
724                 break;
725         case DISC:
726                 pr_cont("DISC");
727                 break;
728         case DM:
729                 pr_cont("DM");
730                 break;
731         case UI:
732                 pr_cont("UI");
733                 break;
734         case UIH:
735                 pr_cont("UIH");
736                 break;
737         default:
738                 if (!(control & 0x01)) {
739                         pr_cont("I N(S)%d N(R)%d",
740                                 (control & 0x0E) >> 1, (control & 0xE0) >> 5);
741                 } else switch (control & 0x0F) {
742                         case RR:
743                                 pr_cont("RR(%d)", (control & 0xE0) >> 5);
744                                 break;
745                         case RNR:
746                                 pr_cont("RNR(%d)", (control & 0xE0) >> 5);
747                                 break;
748                         case REJ:
749                                 pr_cont("REJ(%d)", (control & 0xE0) >> 5);
750                                 break;
751                         default:
752                                 pr_cont("[%02X]", control);
753                 }
754         }
755
756         if (control & PF)
757                 pr_cont("(P)");
758         else
759                 pr_cont("(F)");
760
761         gsm_hex_dump_bytes(NULL, data, dlen);
762 }
763
764
765 /*
766  *      Link level transmission side
767  */
768
769 /**
770  *      gsm_stuff_frame -       bytestuff a packet
771  *      @input: input buffer
772  *      @output: output buffer
773  *      @len: length of input
774  *
775  *      Expand a buffer by bytestuffing it. The worst case size change
776  *      is doubling and the caller is responsible for handing out
777  *      suitable sized buffers.
778  */
779
780 static int gsm_stuff_frame(const u8 *input, u8 *output, int len)
781 {
782         int olen = 0;
783         while (len--) {
784                 if (*input == GSM1_SOF || *input == GSM1_ESCAPE
785                     || (*input & ISO_IEC_646_MASK) == XON
786                     || (*input & ISO_IEC_646_MASK) == XOFF) {
787                         *output++ = GSM1_ESCAPE;
788                         *output++ = *input++ ^ GSM1_ESCAPE_BITS;
789                         olen++;
790                 } else
791                         *output++ = *input++;
792                 olen++;
793         }
794         return olen;
795 }
796
797 /**
798  *      gsm_send        -       send a control frame
799  *      @gsm: our GSM mux
800  *      @addr: address for control frame
801  *      @cr: command/response bit seen as initiator
802  *      @control:  control byte including PF bit
803  *
804  *      Format up and transmit a control frame. These should be transmitted
805  *      ahead of data when they are needed.
806  */
807 static int gsm_send(struct gsm_mux *gsm, int addr, int cr, int control)
808 {
809         struct gsm_msg *msg;
810         u8 *dp;
811         int ocr;
812         unsigned long flags;
813
814         msg = gsm_data_alloc(gsm, addr, 0, control);
815         if (!msg)
816                 return -ENOMEM;
817
818         /* toggle C/R coding if not initiator */
819         ocr = cr ^ (gsm->initiator ? 0 : 1);
820
821         msg->data -= 3;
822         dp = msg->data;
823         *dp++ = (addr << 2) | (ocr << 1) | EA;
824         *dp++ = control;
825
826         if (gsm->encoding == GSM_BASIC_OPT)
827                 *dp++ = EA; /* Length of data = 0 */
828
829         *dp = 0xFF - gsm_fcs_add_block(INIT_FCS, msg->data, dp - msg->data);
830         msg->len = (dp - msg->data) + 1;
831
832         gsm_print_packet("Q->", addr, cr, control, NULL, 0);
833
834         spin_lock_irqsave(&gsm->tx_lock, flags);
835         list_add_tail(&msg->list, &gsm->tx_ctrl_list);
836         gsm->tx_bytes += msg->len;
837         spin_unlock_irqrestore(&gsm->tx_lock, flags);
838         gsmld_write_trigger(gsm);
839
840         return 0;
841 }
842
843 /**
844  *      gsm_dlci_clear_queues   -       remove outstanding data for a DLCI
845  *      @gsm: mux
846  *      @dlci: clear for this DLCI
847  *
848  *      Clears the data queues for a given DLCI.
849  */
850 static void gsm_dlci_clear_queues(struct gsm_mux *gsm, struct gsm_dlci *dlci)
851 {
852         struct gsm_msg *msg, *nmsg;
853         int addr = dlci->addr;
854         unsigned long flags;
855
856         /* Clear DLCI write fifo first */
857         spin_lock_irqsave(&dlci->lock, flags);
858         kfifo_reset(&dlci->fifo);
859         spin_unlock_irqrestore(&dlci->lock, flags);
860
861         /* Clear data packets in MUX write queue */
862         spin_lock_irqsave(&gsm->tx_lock, flags);
863         list_for_each_entry_safe(msg, nmsg, &gsm->tx_data_list, list) {
864                 if (msg->addr != addr)
865                         continue;
866                 gsm->tx_bytes -= msg->len;
867                 list_del(&msg->list);
868                 kfree(msg);
869         }
870         spin_unlock_irqrestore(&gsm->tx_lock, flags);
871 }
872
873 /**
874  *      gsm_response    -       send a control response
875  *      @gsm: our GSM mux
876  *      @addr: address for control frame
877  *      @control:  control byte including PF bit
878  *
879  *      Format up and transmit a link level response frame.
880  */
881
882 static inline void gsm_response(struct gsm_mux *gsm, int addr, int control)
883 {
884         gsm_send(gsm, addr, 0, control);
885 }
886
887 /**
888  *      gsm_command     -       send a control command
889  *      @gsm: our GSM mux
890  *      @addr: address for control frame
891  *      @control:  control byte including PF bit
892  *
893  *      Format up and transmit a link level command frame.
894  */
895
896 static inline void gsm_command(struct gsm_mux *gsm, int addr, int control)
897 {
898         gsm_send(gsm, addr, 1, control);
899 }
900
901 /* Data transmission */
902
903 #define HDR_LEN         6       /* ADDR CTRL [LEN.2] DATA FCS */
904
905 /**
906  *      gsm_data_alloc          -       allocate data frame
907  *      @gsm: GSM mux
908  *      @addr: DLCI address
909  *      @len: length excluding header and FCS
910  *      @ctrl: control byte
911  *
912  *      Allocate a new data buffer for sending frames with data. Space is left
913  *      at the front for header bytes but that is treated as an implementation
914  *      detail and not for the high level code to use
915  */
916
917 static struct gsm_msg *gsm_data_alloc(struct gsm_mux *gsm, u8 addr, int len,
918                                                                 u8 ctrl)
919 {
920         struct gsm_msg *m = kmalloc(sizeof(struct gsm_msg) + len + HDR_LEN,
921                                                                 GFP_ATOMIC);
922         if (m == NULL)
923                 return NULL;
924         m->data = m->buffer + HDR_LEN - 1;      /* Allow for FCS */
925         m->len = len;
926         m->addr = addr;
927         m->ctrl = ctrl;
928         INIT_LIST_HEAD(&m->list);
929         return m;
930 }
931
932 /**
933  *      gsm_send_packet -       sends a single packet
934  *      @gsm: GSM Mux
935  *      @msg: packet to send
936  *
937  *      The given packet is encoded and sent out. No memory is freed.
938  *      The caller must hold the gsm tx lock.
939  */
940 static int gsm_send_packet(struct gsm_mux *gsm, struct gsm_msg *msg)
941 {
942         int len, ret;
943
944
945         if (gsm->encoding == GSM_BASIC_OPT) {
946                 gsm->txframe[0] = GSM0_SOF;
947                 memcpy(gsm->txframe + 1, msg->data, msg->len);
948                 gsm->txframe[msg->len + 1] = GSM0_SOF;
949                 len = msg->len + 2;
950         } else {
951                 gsm->txframe[0] = GSM1_SOF;
952                 len = gsm_stuff_frame(msg->data, gsm->txframe + 1, msg->len);
953                 gsm->txframe[len + 1] = GSM1_SOF;
954                 len += 2;
955         }
956
957         if (debug & DBG_DATA)
958                 gsm_hex_dump_bytes(__func__, gsm->txframe, len);
959         gsm_print_packet("-->", msg->addr, gsm->initiator, msg->ctrl, msg->data,
960                          msg->len);
961
962         ret = gsmld_output(gsm, gsm->txframe, len);
963         if (ret <= 0)
964                 return ret;
965         /* FIXME: Can eliminate one SOF in many more cases */
966         gsm->tx_bytes -= msg->len;
967
968         return 0;
969 }
970
971 /**
972  *      gsm_is_flow_ctrl_msg    -       checks if flow control message
973  *      @msg: message to check
974  *
975  *      Returns true if the given message is a flow control command of the
976  *      control channel. False is returned in any other case.
977  */
978 static bool gsm_is_flow_ctrl_msg(struct gsm_msg *msg)
979 {
980         unsigned int cmd;
981
982         if (msg->addr > 0)
983                 return false;
984
985         switch (msg->ctrl & ~PF) {
986         case UI:
987         case UIH:
988                 cmd = 0;
989                 if (gsm_read_ea_val(&cmd, msg->data + 2, msg->len - 2) < 1)
990                         break;
991                 switch (cmd & ~PF) {
992                 case CMD_FCOFF:
993                 case CMD_FCON:
994                         return true;
995                 }
996                 break;
997         }
998
999         return false;
1000 }
1001
1002 /**
1003  *      gsm_data_kick   -       poke the queue
1004  *      @gsm: GSM Mux
1005  *
1006  *      The tty device has called us to indicate that room has appeared in
1007  *      the transmit queue. Ram more data into the pipe if we have any.
1008  *      If we have been flow-stopped by a CMD_FCOFF, then we can only
1009  *      send messages on DLCI0 until CMD_FCON. The caller must hold
1010  *      the gsm tx lock.
1011  */
1012 static int gsm_data_kick(struct gsm_mux *gsm)
1013 {
1014         struct gsm_msg *msg, *nmsg;
1015         struct gsm_dlci *dlci;
1016         int ret;
1017
1018         clear_bit(TTY_DO_WRITE_WAKEUP, &gsm->tty->flags);
1019
1020         /* Serialize control messages and control channel messages first */
1021         list_for_each_entry_safe(msg, nmsg, &gsm->tx_ctrl_list, list) {
1022                 if (gsm->constipated && !gsm_is_flow_ctrl_msg(msg))
1023                         continue;
1024                 ret = gsm_send_packet(gsm, msg);
1025                 switch (ret) {
1026                 case -ENOSPC:
1027                         return -ENOSPC;
1028                 case -ENODEV:
1029                         /* ldisc not open */
1030                         gsm->tx_bytes -= msg->len;
1031                         list_del(&msg->list);
1032                         kfree(msg);
1033                         continue;
1034                 default:
1035                         if (ret >= 0) {
1036                                 list_del(&msg->list);
1037                                 kfree(msg);
1038                         }
1039                         break;
1040                 }
1041         }
1042
1043         if (gsm->constipated)
1044                 return -EAGAIN;
1045
1046         /* Serialize other channels */
1047         if (list_empty(&gsm->tx_data_list))
1048                 return 0;
1049         list_for_each_entry_safe(msg, nmsg, &gsm->tx_data_list, list) {
1050                 dlci = gsm->dlci[msg->addr];
1051                 /* Send only messages for DLCIs with valid state */
1052                 if (dlci->state != DLCI_OPEN) {
1053                         gsm->tx_bytes -= msg->len;
1054                         list_del(&msg->list);
1055                         kfree(msg);
1056                         continue;
1057                 }
1058                 ret = gsm_send_packet(gsm, msg);
1059                 switch (ret) {
1060                 case -ENOSPC:
1061                         return -ENOSPC;
1062                 case -ENODEV:
1063                         /* ldisc not open */
1064                         gsm->tx_bytes -= msg->len;
1065                         list_del(&msg->list);
1066                         kfree(msg);
1067                         continue;
1068                 default:
1069                         if (ret >= 0) {
1070                                 list_del(&msg->list);
1071                                 kfree(msg);
1072                         }
1073                         break;
1074                 }
1075         }
1076
1077         return 1;
1078 }
1079
1080 /**
1081  *      __gsm_data_queue                -       queue a UI or UIH frame
1082  *      @dlci: DLCI sending the data
1083  *      @msg: message queued
1084  *
1085  *      Add data to the transmit queue and try and get stuff moving
1086  *      out of the mux tty if not already doing so. The Caller must hold
1087  *      the gsm tx lock.
1088  */
1089
1090 static void __gsm_data_queue(struct gsm_dlci *dlci, struct gsm_msg *msg)
1091 {
1092         struct gsm_mux *gsm = dlci->gsm;
1093         u8 *dp = msg->data;
1094         u8 *fcs = dp + msg->len;
1095
1096         /* Fill in the header */
1097         if (gsm->encoding == GSM_BASIC_OPT) {
1098                 if (msg->len < 128)
1099                         *--dp = (msg->len << 1) | EA;
1100                 else {
1101                         *--dp = (msg->len >> 7);        /* bits 7 - 15 */
1102                         *--dp = (msg->len & 127) << 1;  /* bits 0 - 6 */
1103                 }
1104         }
1105
1106         *--dp = msg->ctrl;
1107         if (gsm->initiator)
1108                 *--dp = (msg->addr << 2) | CR | EA;
1109         else
1110                 *--dp = (msg->addr << 2) | EA;
1111         *fcs = gsm_fcs_add_block(INIT_FCS, dp , msg->data - dp);
1112         /* Ugly protocol layering violation */
1113         if (msg->ctrl == UI || msg->ctrl == (UI|PF))
1114                 *fcs = gsm_fcs_add_block(*fcs, msg->data, msg->len);
1115         *fcs = 0xFF - *fcs;
1116
1117         gsm_print_packet("Q> ", msg->addr, gsm->initiator, msg->ctrl,
1118                                                         msg->data, msg->len);
1119
1120         /* Move the header back and adjust the length, also allow for the FCS
1121            now tacked on the end */
1122         msg->len += (msg->data - dp) + 1;
1123         msg->data = dp;
1124
1125         /* Add to the actual output queue */
1126         switch (msg->ctrl & ~PF) {
1127         case UI:
1128         case UIH:
1129                 if (msg->addr > 0) {
1130                         list_add_tail(&msg->list, &gsm->tx_data_list);
1131                         break;
1132                 }
1133                 fallthrough;
1134         default:
1135                 list_add_tail(&msg->list, &gsm->tx_ctrl_list);
1136                 break;
1137         }
1138         gsm->tx_bytes += msg->len;
1139
1140         gsmld_write_trigger(gsm);
1141         mod_timer(&gsm->kick_timer, jiffies + 10 * gsm->t1 * HZ / 100);
1142 }
1143
1144 /**
1145  *      gsm_data_queue          -       queue a UI or UIH frame
1146  *      @dlci: DLCI sending the data
1147  *      @msg: message queued
1148  *
1149  *      Add data to the transmit queue and try and get stuff moving
1150  *      out of the mux tty if not already doing so. Take the
1151  *      the gsm tx lock and dlci lock.
1152  */
1153
1154 static void gsm_data_queue(struct gsm_dlci *dlci, struct gsm_msg *msg)
1155 {
1156         unsigned long flags;
1157         spin_lock_irqsave(&dlci->gsm->tx_lock, flags);
1158         __gsm_data_queue(dlci, msg);
1159         spin_unlock_irqrestore(&dlci->gsm->tx_lock, flags);
1160 }
1161
1162 /**
1163  *      gsm_dlci_data_output    -       try and push data out of a DLCI
1164  *      @gsm: mux
1165  *      @dlci: the DLCI to pull data from
1166  *
1167  *      Pull data from a DLCI and send it into the transmit queue if there
1168  *      is data. Keep to the MRU of the mux. This path handles the usual tty
1169  *      interface which is a byte stream with optional modem data.
1170  *
1171  *      Caller must hold the tx_lock of the mux.
1172  */
1173
1174 static int gsm_dlci_data_output(struct gsm_mux *gsm, struct gsm_dlci *dlci)
1175 {
1176         struct gsm_msg *msg;
1177         u8 *dp;
1178         int h, len, size;
1179
1180         /* for modem bits without break data */
1181         h = ((dlci->adaption == 1) ? 0 : 1);
1182
1183         len = kfifo_len(&dlci->fifo);
1184         if (len == 0)
1185                 return 0;
1186
1187         /* MTU/MRU count only the data bits but watch adaption mode */
1188         if ((len + h) > dlci->mtu)
1189                 len = dlci->mtu - h;
1190
1191         size = len + h;
1192
1193         msg = gsm_data_alloc(gsm, dlci->addr, size, dlci->ftype);
1194         if (!msg)
1195                 return -ENOMEM;
1196         dp = msg->data;
1197         switch (dlci->adaption) {
1198         case 1: /* Unstructured */
1199                 break;
1200         case 2: /* Unstructured with modem bits.
1201                  * Always one byte as we never send inline break data
1202                  */
1203                 *dp++ = (gsm_encode_modem(dlci) << 1) | EA;
1204                 break;
1205         default:
1206                 pr_err("%s: unsupported adaption %d\n", __func__,
1207                        dlci->adaption);
1208                 break;
1209         }
1210
1211         WARN_ON(len != kfifo_out_locked(&dlci->fifo, dp, len,
1212                 &dlci->lock));
1213
1214         /* Notify upper layer about available send space. */
1215         tty_port_tty_wakeup(&dlci->port);
1216
1217         __gsm_data_queue(dlci, msg);
1218         /* Bytes of data we used up */
1219         return size;
1220 }
1221
1222 /**
1223  *      gsm_dlci_data_output_framed  -  try and push data out of a DLCI
1224  *      @gsm: mux
1225  *      @dlci: the DLCI to pull data from
1226  *
1227  *      Pull data from a DLCI and send it into the transmit queue if there
1228  *      is data. Keep to the MRU of the mux. This path handles framed data
1229  *      queued as skbuffs to the DLCI.
1230  *
1231  *      Caller must hold the tx_lock of the mux.
1232  */
1233
1234 static int gsm_dlci_data_output_framed(struct gsm_mux *gsm,
1235                                                 struct gsm_dlci *dlci)
1236 {
1237         struct gsm_msg *msg;
1238         u8 *dp;
1239         int len, size;
1240         int last = 0, first = 0;
1241         int overhead = 0;
1242
1243         /* One byte per frame is used for B/F flags */
1244         if (dlci->adaption == 4)
1245                 overhead = 1;
1246
1247         /* dlci->skb is locked by tx_lock */
1248         if (dlci->skb == NULL) {
1249                 dlci->skb = skb_dequeue_tail(&dlci->skb_list);
1250                 if (dlci->skb == NULL)
1251                         return 0;
1252                 first = 1;
1253         }
1254         len = dlci->skb->len + overhead;
1255
1256         /* MTU/MRU count only the data bits */
1257         if (len > dlci->mtu) {
1258                 if (dlci->adaption == 3) {
1259                         /* Over long frame, bin it */
1260                         dev_kfree_skb_any(dlci->skb);
1261                         dlci->skb = NULL;
1262                         return 0;
1263                 }
1264                 len = dlci->mtu;
1265         } else
1266                 last = 1;
1267
1268         size = len + overhead;
1269         msg = gsm_data_alloc(gsm, dlci->addr, size, dlci->ftype);
1270         if (msg == NULL) {
1271                 skb_queue_tail(&dlci->skb_list, dlci->skb);
1272                 dlci->skb = NULL;
1273                 return -ENOMEM;
1274         }
1275         dp = msg->data;
1276
1277         if (dlci->adaption == 4) { /* Interruptible framed (Packetised Data) */
1278                 /* Flag byte to carry the start/end info */
1279                 *dp++ = last << 7 | first << 6 | 1;     /* EA */
1280                 len--;
1281         }
1282         memcpy(dp, dlci->skb->data, len);
1283         skb_pull(dlci->skb, len);
1284         __gsm_data_queue(dlci, msg);
1285         if (last) {
1286                 dev_kfree_skb_any(dlci->skb);
1287                 dlci->skb = NULL;
1288         }
1289         return size;
1290 }
1291
1292 /**
1293  *      gsm_dlci_modem_output   -       try and push modem status out of a DLCI
1294  *      @gsm: mux
1295  *      @dlci: the DLCI to pull modem status from
1296  *      @brk: break signal
1297  *
1298  *      Push an empty frame in to the transmit queue to update the modem status
1299  *      bits and to transmit an optional break.
1300  *
1301  *      Caller must hold the tx_lock of the mux.
1302  */
1303
1304 static int gsm_dlci_modem_output(struct gsm_mux *gsm, struct gsm_dlci *dlci,
1305                                  u8 brk)
1306 {
1307         u8 *dp = NULL;
1308         struct gsm_msg *msg;
1309         int size = 0;
1310
1311         /* for modem bits without break data */
1312         switch (dlci->adaption) {
1313         case 1: /* Unstructured */
1314                 break;
1315         case 2: /* Unstructured with modem bits. */
1316                 size++;
1317                 if (brk > 0)
1318                         size++;
1319                 break;
1320         default:
1321                 pr_err("%s: unsupported adaption %d\n", __func__,
1322                        dlci->adaption);
1323                 return -EINVAL;
1324         }
1325
1326         msg = gsm_data_alloc(gsm, dlci->addr, size, dlci->ftype);
1327         if (!msg) {
1328                 pr_err("%s: gsm_data_alloc error", __func__);
1329                 return -ENOMEM;
1330         }
1331         dp = msg->data;
1332         switch (dlci->adaption) {
1333         case 1: /* Unstructured */
1334                 break;
1335         case 2: /* Unstructured with modem bits. */
1336                 if (brk == 0) {
1337                         *dp++ = (gsm_encode_modem(dlci) << 1) | EA;
1338                 } else {
1339                         *dp++ = gsm_encode_modem(dlci) << 1;
1340                         *dp++ = (brk << 4) | 2 | EA; /* Length, Break, EA */
1341                 }
1342                 break;
1343         default:
1344                 /* Handled above */
1345                 break;
1346         }
1347
1348         __gsm_data_queue(dlci, msg);
1349         return size;
1350 }
1351
1352 /**
1353  *      gsm_dlci_data_sweep             -       look for data to send
1354  *      @gsm: the GSM mux
1355  *
1356  *      Sweep the GSM mux channels in priority order looking for ones with
1357  *      data to send. We could do with optimising this scan a bit. We aim
1358  *      to fill the queue totally or up to TX_THRESH_HI bytes. Once we hit
1359  *      TX_THRESH_LO we get called again
1360  *
1361  *      FIXME: We should round robin between groups and in theory you can
1362  *      renegotiate DLCI priorities with optional stuff. Needs optimising.
1363  */
1364
1365 static int gsm_dlci_data_sweep(struct gsm_mux *gsm)
1366 {
1367         /* Priority ordering: We should do priority with RR of the groups */
1368         int i, len, ret = 0;
1369         bool sent;
1370         struct gsm_dlci *dlci;
1371
1372         while (gsm->tx_bytes < TX_THRESH_HI) {
1373                 for (sent = false, i = 1; i < NUM_DLCI; i++) {
1374                         dlci = gsm->dlci[i];
1375                         /* skip unused or blocked channel */
1376                         if (!dlci || dlci->constipated)
1377                                 continue;
1378                         /* skip channels with invalid state */
1379                         if (dlci->state != DLCI_OPEN)
1380                                 continue;
1381                         /* count the sent data per adaption */
1382                         if (dlci->adaption < 3 && !dlci->net)
1383                                 len = gsm_dlci_data_output(gsm, dlci);
1384                         else
1385                                 len = gsm_dlci_data_output_framed(gsm, dlci);
1386                         /* on error exit */
1387                         if (len < 0)
1388                                 return ret;
1389                         if (len > 0) {
1390                                 ret++;
1391                                 sent = true;
1392                                 /* The lower DLCs can starve the higher DLCs! */
1393                                 break;
1394                         }
1395                         /* try next */
1396                 }
1397                 if (!sent)
1398                         break;
1399         }
1400
1401         return ret;
1402 }
1403
1404 /**
1405  *      gsm_dlci_data_kick      -       transmit if possible
1406  *      @dlci: DLCI to kick
1407  *
1408  *      Transmit data from this DLCI if the queue is empty. We can't rely on
1409  *      a tty wakeup except when we filled the pipe so we need to fire off
1410  *      new data ourselves in other cases.
1411  */
1412
1413 static void gsm_dlci_data_kick(struct gsm_dlci *dlci)
1414 {
1415         unsigned long flags;
1416         int sweep;
1417
1418         if (dlci->constipated)
1419                 return;
1420
1421         spin_lock_irqsave(&dlci->gsm->tx_lock, flags);
1422         /* If we have nothing running then we need to fire up */
1423         sweep = (dlci->gsm->tx_bytes < TX_THRESH_LO);
1424         if (dlci->gsm->tx_bytes == 0) {
1425                 if (dlci->net)
1426                         gsm_dlci_data_output_framed(dlci->gsm, dlci);
1427                 else
1428                         gsm_dlci_data_output(dlci->gsm, dlci);
1429         }
1430         if (sweep)
1431                 gsm_dlci_data_sweep(dlci->gsm);
1432         spin_unlock_irqrestore(&dlci->gsm->tx_lock, flags);
1433 }
1434
1435 /*
1436  *      Control message processing
1437  */
1438
1439
1440 /**
1441  * gsm_control_command  -       send a command frame to a control
1442  * @gsm: gsm channel
1443  * @cmd: the command to use
1444  * @data: data to follow encoded info
1445  * @dlen: length of data
1446  *
1447  * Encode up and queue a UI/UIH frame containing our command.
1448  */
1449 static int gsm_control_command(struct gsm_mux *gsm, int cmd, const u8 *data,
1450                                int dlen)
1451 {
1452         struct gsm_msg *msg;
1453
1454         msg = gsm_data_alloc(gsm, 0, dlen + 2, gsm->dlci[0]->ftype);
1455         if (msg == NULL)
1456                 return -ENOMEM;
1457
1458         msg->data[0] = (cmd << 1) | CR | EA;    /* Set C/R */
1459         msg->data[1] = (dlen << 1) | EA;
1460         memcpy(msg->data + 2, data, dlen);
1461         gsm_data_queue(gsm->dlci[0], msg);
1462
1463         return 0;
1464 }
1465
1466 /**
1467  *      gsm_control_reply       -       send a response frame to a control
1468  *      @gsm: gsm channel
1469  *      @cmd: the command to use
1470  *      @data: data to follow encoded info
1471  *      @dlen: length of data
1472  *
1473  *      Encode up and queue a UI/UIH frame containing our response.
1474  */
1475
1476 static void gsm_control_reply(struct gsm_mux *gsm, int cmd, const u8 *data,
1477                                         int dlen)
1478 {
1479         struct gsm_msg *msg;
1480
1481         msg = gsm_data_alloc(gsm, 0, dlen + 2, gsm->dlci[0]->ftype);
1482         if (msg == NULL)
1483                 return;
1484         msg->data[0] = (cmd & 0xFE) << 1 | EA;  /* Clear C/R */
1485         msg->data[1] = (dlen << 1) | EA;
1486         memcpy(msg->data + 2, data, dlen);
1487         gsm_data_queue(gsm->dlci[0], msg);
1488 }
1489
1490 /**
1491  *      gsm_process_modem       -       process received modem status
1492  *      @tty: virtual tty bound to the DLCI
1493  *      @dlci: DLCI to affect
1494  *      @modem: modem bits (full EA)
1495  *      @slen: number of signal octets
1496  *
1497  *      Used when a modem control message or line state inline in adaption
1498  *      layer 2 is processed. Sort out the local modem state and throttles
1499  */
1500
1501 static void gsm_process_modem(struct tty_struct *tty, struct gsm_dlci *dlci,
1502                                                         u32 modem, int slen)
1503 {
1504         int  mlines = 0;
1505         u8 brk = 0;
1506         int fc;
1507
1508         /* The modem status command can either contain one octet (V.24 signals)
1509          * or two octets (V.24 signals + break signals). This is specified in
1510          * section 5.4.6.3.7 of the 07.10 mux spec.
1511          */
1512
1513         if (slen == 1)
1514                 modem = modem & 0x7f;
1515         else {
1516                 brk = modem & 0x7f;
1517                 modem = (modem >> 7) & 0x7f;
1518         }
1519
1520         /* Flow control/ready to communicate */
1521         fc = (modem & MDM_FC) || !(modem & MDM_RTR);
1522         if (fc && !dlci->constipated) {
1523                 /* Need to throttle our output on this device */
1524                 dlci->constipated = true;
1525         } else if (!fc && dlci->constipated) {
1526                 dlci->constipated = false;
1527                 gsm_dlci_data_kick(dlci);
1528         }
1529
1530         /* Map modem bits */
1531         if (modem & MDM_RTC)
1532                 mlines |= TIOCM_DSR | TIOCM_DTR;
1533         if (modem & MDM_RTR)
1534                 mlines |= TIOCM_RTS | TIOCM_CTS;
1535         if (modem & MDM_IC)
1536                 mlines |= TIOCM_RI;
1537         if (modem & MDM_DV)
1538                 mlines |= TIOCM_CD;
1539
1540         /* Carrier drop -> hangup */
1541         if (tty) {
1542                 if ((mlines & TIOCM_CD) == 0 && (dlci->modem_rx & TIOCM_CD))
1543                         if (!C_CLOCAL(tty))
1544                                 tty_hangup(tty);
1545         }
1546         if (brk & 0x01)
1547                 tty_insert_flip_char(&dlci->port, 0, TTY_BREAK);
1548         dlci->modem_rx = mlines;
1549         wake_up_interruptible(&dlci->gsm->event);
1550 }
1551
1552 /**
1553  * gsm_process_negotiation      -       process received parameters
1554  * @gsm: GSM channel
1555  * @addr: DLCI address
1556  * @cr: command/response
1557  * @params: encoded parameters from the parameter negotiation message
1558  *
1559  * Used when the response for our parameter negotiation command was
1560  * received.
1561  */
1562 static int gsm_process_negotiation(struct gsm_mux *gsm, unsigned int addr,
1563                                    unsigned int cr,
1564                                    const struct gsm_dlci_param_bits *params)
1565 {
1566         struct gsm_dlci *dlci = gsm->dlci[addr];
1567         unsigned int ftype, i, adaption, prio, n1, k;
1568
1569         i = FIELD_GET(PN_I_CL_FIELD_FTYPE, params->i_cl_bits);
1570         adaption = FIELD_GET(PN_I_CL_FIELD_ADAPTION, params->i_cl_bits) + 1;
1571         prio = FIELD_GET(PN_P_FIELD_PRIO, params->p_bits);
1572         n1 = FIELD_GET(PN_N_FIELD_N1, get_unaligned_le16(&params->n_bits));
1573         k = FIELD_GET(PN_K_FIELD_K, params->k_bits);
1574
1575         if (n1 < MIN_MTU) {
1576                 if (debug & DBG_ERRORS)
1577                         pr_info("%s N1 out of range in PN\n", __func__);
1578                 return -EINVAL;
1579         }
1580
1581         switch (i) {
1582         case 0x00:
1583                 ftype = UIH;
1584                 break;
1585         case 0x01:
1586                 ftype = UI;
1587                 break;
1588         case 0x02: /* I frames are not supported */
1589                 if (debug & DBG_ERRORS)
1590                         pr_info("%s unsupported I frame request in PN\n",
1591                                 __func__);
1592                 return -EINVAL;
1593         default:
1594                 if (debug & DBG_ERRORS)
1595                         pr_info("%s i out of range in PN\n", __func__);
1596                 return -EINVAL;
1597         }
1598
1599         if (!cr && gsm->initiator) {
1600                 if (adaption != dlci->adaption) {
1601                         if (debug & DBG_ERRORS)
1602                                 pr_info("%s invalid adaption %d in PN\n",
1603                                         __func__, adaption);
1604                         return -EINVAL;
1605                 }
1606                 if (prio != dlci->prio) {
1607                         if (debug & DBG_ERRORS)
1608                                 pr_info("%s invalid priority %d in PN",
1609                                         __func__, prio);
1610                         return -EINVAL;
1611                 }
1612                 if (n1 > gsm->mru || n1 > dlci->mtu) {
1613                         /* We requested a frame size but the other party wants
1614                          * to send larger frames. The standard allows only a
1615                          * smaller response value than requested (5.4.6.3.1).
1616                          */
1617                         if (debug & DBG_ERRORS)
1618                                 pr_info("%s invalid N1 %d in PN\n", __func__,
1619                                         n1);
1620                         return -EINVAL;
1621                 }
1622                 dlci->mtu = n1;
1623                 if (ftype != dlci->ftype) {
1624                         if (debug & DBG_ERRORS)
1625                                 pr_info("%s invalid i %d in PN\n", __func__, i);
1626                         return -EINVAL;
1627                 }
1628                 if (ftype != UI && ftype != UIH && k > dlci->k) {
1629                         if (debug & DBG_ERRORS)
1630                                 pr_info("%s invalid k %d in PN\n", __func__, k);
1631                         return -EINVAL;
1632                 }
1633                 dlci->k = k;
1634         } else if (cr && !gsm->initiator) {
1635                 /* Only convergence layer type 1 and 2 are supported. */
1636                 if (adaption != 1 && adaption != 2) {
1637                         if (debug & DBG_ERRORS)
1638                                 pr_info("%s invalid adaption %d in PN\n",
1639                                         __func__, adaption);
1640                         return -EINVAL;
1641                 }
1642                 dlci->adaption = adaption;
1643                 if (n1 > gsm->mru) {
1644                         /* Propose a smaller value */
1645                         dlci->mtu = gsm->mru;
1646                 } else if (n1 > MAX_MTU) {
1647                         /* Propose a smaller value */
1648                         dlci->mtu = MAX_MTU;
1649                 } else {
1650                         dlci->mtu = n1;
1651                 }
1652                 dlci->prio = prio;
1653                 dlci->ftype = ftype;
1654                 dlci->k = k;
1655         } else {
1656                 return -EINVAL;
1657         }
1658
1659         return 0;
1660 }
1661
1662 /**
1663  *      gsm_control_modem       -       modem status received
1664  *      @gsm: GSM channel
1665  *      @data: data following command
1666  *      @clen: command length
1667  *
1668  *      We have received a modem status control message. This is used by
1669  *      the GSM mux protocol to pass virtual modem line status and optionally
1670  *      to indicate break signals. Unpack it, convert to Linux representation
1671  *      and if need be stuff a break message down the tty.
1672  */
1673
1674 static void gsm_control_modem(struct gsm_mux *gsm, const u8 *data, int clen)
1675 {
1676         unsigned int addr = 0;
1677         unsigned int modem = 0;
1678         struct gsm_dlci *dlci;
1679         int len = clen;
1680         int cl = clen;
1681         const u8 *dp = data;
1682         struct tty_struct *tty;
1683
1684         len = gsm_read_ea_val(&addr, data, cl);
1685         if (len < 1)
1686                 return;
1687
1688         addr >>= 1;
1689         /* Closed port, or invalid ? */
1690         if (addr == 0 || addr >= NUM_DLCI || gsm->dlci[addr] == NULL)
1691                 return;
1692         dlci = gsm->dlci[addr];
1693
1694         /* Must be at least one byte following the EA */
1695         if ((cl - len) < 1)
1696                 return;
1697
1698         dp += len;
1699         cl -= len;
1700
1701         /* get the modem status */
1702         len = gsm_read_ea_val(&modem, dp, cl);
1703         if (len < 1)
1704                 return;
1705
1706         tty = tty_port_tty_get(&dlci->port);
1707         gsm_process_modem(tty, dlci, modem, cl);
1708         if (tty) {
1709                 tty_wakeup(tty);
1710                 tty_kref_put(tty);
1711         }
1712         gsm_control_reply(gsm, CMD_MSC, data, clen);
1713 }
1714
1715 /**
1716  * gsm_control_negotiation      -       parameter negotiation received
1717  * @gsm: GSM channel
1718  * @cr: command/response flag
1719  * @data: data following command
1720  * @dlen: data length
1721  *
1722  * We have received a parameter negotiation message. This is used by
1723  * the GSM mux protocol to configure protocol parameters for a new DLCI.
1724  */
1725 static void gsm_control_negotiation(struct gsm_mux *gsm, unsigned int cr,
1726                                     const u8 *data, unsigned int dlen)
1727 {
1728         unsigned int addr;
1729         struct gsm_dlci_param_bits pn_reply;
1730         struct gsm_dlci *dlci;
1731         struct gsm_dlci_param_bits *params;
1732
1733         if (dlen < sizeof(struct gsm_dlci_param_bits))
1734                 return;
1735
1736         /* Invalid DLCI? */
1737         params = (struct gsm_dlci_param_bits *)data;
1738         addr = FIELD_GET(PN_D_FIELD_DLCI, params->d_bits);
1739         if (addr == 0 || addr >= NUM_DLCI || !gsm->dlci[addr])
1740                 return;
1741         dlci = gsm->dlci[addr];
1742
1743         /* Too late for parameter negotiation? */
1744         if ((!cr && dlci->state == DLCI_OPENING) || dlci->state == DLCI_OPEN)
1745                 return;
1746
1747         /* Process the received parameters */
1748         if (gsm_process_negotiation(gsm, addr, cr, params) != 0) {
1749                 /* Negotiation failed. Close the link. */
1750                 if (debug & DBG_ERRORS)
1751                         pr_info("%s PN failed\n", __func__);
1752                 gsm_dlci_close(dlci);
1753                 return;
1754         }
1755
1756         if (cr) {
1757                 /* Reply command with accepted parameters. */
1758                 if (gsm_encode_params(dlci, &pn_reply) == 0)
1759                         gsm_control_reply(gsm, CMD_PN, (const u8 *)&pn_reply,
1760                                           sizeof(pn_reply));
1761                 else if (debug & DBG_ERRORS)
1762                         pr_info("%s PN invalid\n", __func__);
1763         } else if (dlci->state == DLCI_CONFIGURE) {
1764                 /* Proceed with link setup by sending SABM before UA */
1765                 dlci->state = DLCI_OPENING;
1766                 gsm_command(gsm, dlci->addr, SABM|PF);
1767                 mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
1768         } else {
1769                 if (debug & DBG_ERRORS)
1770                         pr_info("%s PN in invalid state\n", __func__);
1771         }
1772 }
1773
1774 /**
1775  *      gsm_control_rls         -       remote line status
1776  *      @gsm: GSM channel
1777  *      @data: data bytes
1778  *      @clen: data length
1779  *
1780  *      The modem sends us a two byte message on the control channel whenever
1781  *      it wishes to send us an error state from the virtual link. Stuff
1782  *      this into the uplink tty if present
1783  */
1784
1785 static void gsm_control_rls(struct gsm_mux *gsm, const u8 *data, int clen)
1786 {
1787         struct tty_port *port;
1788         unsigned int addr = 0;
1789         u8 bits;
1790         int len = clen;
1791         const u8 *dp = data;
1792
1793         while (gsm_read_ea(&addr, *dp++) == 0) {
1794                 len--;
1795                 if (len == 0)
1796                         return;
1797         }
1798         /* Must be at least one byte following ea */
1799         len--;
1800         if (len <= 0)
1801                 return;
1802         addr >>= 1;
1803         /* Closed port, or invalid ? */
1804         if (addr == 0 || addr >= NUM_DLCI || gsm->dlci[addr] == NULL)
1805                 return;
1806         /* No error ? */
1807         bits = *dp;
1808         if ((bits & 1) == 0)
1809                 return;
1810
1811         port = &gsm->dlci[addr]->port;
1812
1813         if (bits & 2)
1814                 tty_insert_flip_char(port, 0, TTY_OVERRUN);
1815         if (bits & 4)
1816                 tty_insert_flip_char(port, 0, TTY_PARITY);
1817         if (bits & 8)
1818                 tty_insert_flip_char(port, 0, TTY_FRAME);
1819
1820         tty_flip_buffer_push(port);
1821
1822         gsm_control_reply(gsm, CMD_RLS, data, clen);
1823 }
1824
1825 static void gsm_dlci_begin_close(struct gsm_dlci *dlci);
1826
1827 /**
1828  *      gsm_control_message     -       DLCI 0 control processing
1829  *      @gsm: our GSM mux
1830  *      @command:  the command EA
1831  *      @data: data beyond the command/length EAs
1832  *      @clen: length
1833  *
1834  *      Input processor for control messages from the other end of the link.
1835  *      Processes the incoming request and queues a response frame or an
1836  *      NSC response if not supported
1837  */
1838
1839 static void gsm_control_message(struct gsm_mux *gsm, unsigned int command,
1840                                                 const u8 *data, int clen)
1841 {
1842         u8 buf[1];
1843
1844         switch (command) {
1845         case CMD_CLD: {
1846                 struct gsm_dlci *dlci = gsm->dlci[0];
1847                 /* Modem wishes to close down */
1848                 if (dlci) {
1849                         dlci->dead = true;
1850                         gsm->dead = true;
1851                         gsm_dlci_begin_close(dlci);
1852                 }
1853                 }
1854                 break;
1855         case CMD_TEST:
1856                 /* Modem wishes to test, reply with the data */
1857                 gsm_control_reply(gsm, CMD_TEST, data, clen);
1858                 break;
1859         case CMD_FCON:
1860                 /* Modem can accept data again */
1861                 gsm->constipated = false;
1862                 gsm_control_reply(gsm, CMD_FCON, NULL, 0);
1863                 /* Kick the link in case it is idling */
1864                 gsmld_write_trigger(gsm);
1865                 break;
1866         case CMD_FCOFF:
1867                 /* Modem wants us to STFU */
1868                 gsm->constipated = true;
1869                 gsm_control_reply(gsm, CMD_FCOFF, NULL, 0);
1870                 break;
1871         case CMD_MSC:
1872                 /* Out of band modem line change indicator for a DLCI */
1873                 gsm_control_modem(gsm, data, clen);
1874                 break;
1875         case CMD_RLS:
1876                 /* Out of band error reception for a DLCI */
1877                 gsm_control_rls(gsm, data, clen);
1878                 break;
1879         case CMD_PSC:
1880                 /* Modem wishes to enter power saving state */
1881                 gsm_control_reply(gsm, CMD_PSC, NULL, 0);
1882                 break;
1883                 /* Optional commands */
1884         case CMD_PN:
1885                 /* Modem sends a parameter negotiation command */
1886                 gsm_control_negotiation(gsm, 1, data, clen);
1887                 break;
1888                 /* Optional unsupported commands */
1889         case CMD_RPN:   /* Remote port negotiation */
1890         case CMD_SNC:   /* Service negotiation command */
1891         default:
1892                 /* Reply to bad commands with an NSC */
1893                 buf[0] = command;
1894                 gsm_control_reply(gsm, CMD_NSC, buf, 1);
1895                 break;
1896         }
1897 }
1898
1899 /**
1900  *      gsm_control_response    -       process a response to our control
1901  *      @gsm: our GSM mux
1902  *      @command: the command (response) EA
1903  *      @data: data beyond the command/length EA
1904  *      @clen: length
1905  *
1906  *      Process a response to an outstanding command. We only allow a single
1907  *      control message in flight so this is fairly easy. All the clean up
1908  *      is done by the caller, we just update the fields, flag it as done
1909  *      and return
1910  */
1911
1912 static void gsm_control_response(struct gsm_mux *gsm, unsigned int command,
1913                                                 const u8 *data, int clen)
1914 {
1915         struct gsm_control *ctrl;
1916         struct gsm_dlci *dlci;
1917         unsigned long flags;
1918
1919         spin_lock_irqsave(&gsm->control_lock, flags);
1920
1921         ctrl = gsm->pending_cmd;
1922         dlci = gsm->dlci[0];
1923         command |= 1;
1924         /* Does the reply match our command */
1925         if (ctrl != NULL && (command == ctrl->cmd || command == CMD_NSC)) {
1926                 /* Our command was replied to, kill the retry timer */
1927                 del_timer(&gsm->t2_timer);
1928                 gsm->pending_cmd = NULL;
1929                 /* Rejected by the other end */
1930                 if (command == CMD_NSC)
1931                         ctrl->error = -EOPNOTSUPP;
1932                 ctrl->done = 1;
1933                 wake_up(&gsm->event);
1934         /* Or did we receive the PN response to our PN command */
1935         } else if (command == CMD_PN) {
1936                 gsm_control_negotiation(gsm, 0, data, clen);
1937         /* Or did we receive the TEST response to our TEST command */
1938         } else if (command == CMD_TEST && clen == 1 && *data == gsm->ka_num) {
1939                 gsm->ka_retries = -1; /* trigger new keep-alive message */
1940                 if (dlci && !dlci->dead)
1941                         mod_timer(&gsm->ka_timer, jiffies + gsm->keep_alive * HZ / 100);
1942         }
1943         spin_unlock_irqrestore(&gsm->control_lock, flags);
1944 }
1945
1946 /**
1947  * gsm_control_keep_alive       -       check timeout or start keep-alive
1948  * @t: timer contained in our gsm object
1949  *
1950  * Called off the keep-alive timer expiry signaling that our link
1951  * partner is not responding anymore. Link will be closed.
1952  * This is also called to startup our timer.
1953  */
1954
1955 static void gsm_control_keep_alive(struct timer_list *t)
1956 {
1957         struct gsm_mux *gsm = from_timer(gsm, t, ka_timer);
1958         unsigned long flags;
1959
1960         spin_lock_irqsave(&gsm->control_lock, flags);
1961         if (gsm->ka_num && gsm->ka_retries == 0) {
1962                 /* Keep-alive expired -> close the link */
1963                 if (debug & DBG_ERRORS)
1964                         pr_debug("%s keep-alive timed out\n", __func__);
1965                 spin_unlock_irqrestore(&gsm->control_lock, flags);
1966                 if (gsm->dlci[0])
1967                         gsm_dlci_begin_close(gsm->dlci[0]);
1968                 return;
1969         } else if (gsm->keep_alive && gsm->dlci[0] && !gsm->dlci[0]->dead) {
1970                 if (gsm->ka_retries > 0) {
1971                         /* T2 expired for keep-alive -> resend */
1972                         gsm->ka_retries--;
1973                 } else {
1974                         /* Start keep-alive timer */
1975                         gsm->ka_num++;
1976                         if (!gsm->ka_num)
1977                                 gsm->ka_num++;
1978                         gsm->ka_retries = (signed int)gsm->n2;
1979                 }
1980                 gsm_control_command(gsm, CMD_TEST, &gsm->ka_num,
1981                                     sizeof(gsm->ka_num));
1982                 mod_timer(&gsm->ka_timer,
1983                           jiffies + gsm->t2 * HZ / 100);
1984         }
1985         spin_unlock_irqrestore(&gsm->control_lock, flags);
1986 }
1987
1988 /**
1989  *      gsm_control_transmit    -       send control packet
1990  *      @gsm: gsm mux
1991  *      @ctrl: frame to send
1992  *
1993  *      Send out a pending control command (called under control lock)
1994  */
1995
1996 static void gsm_control_transmit(struct gsm_mux *gsm, struct gsm_control *ctrl)
1997 {
1998         gsm_control_command(gsm, ctrl->cmd, ctrl->data, ctrl->len);
1999 }
2000
2001 /**
2002  *      gsm_control_retransmit  -       retransmit a control frame
2003  *      @t: timer contained in our gsm object
2004  *
2005  *      Called off the T2 timer expiry in order to retransmit control frames
2006  *      that have been lost in the system somewhere. The control_lock protects
2007  *      us from colliding with another sender or a receive completion event.
2008  *      In that situation the timer may still occur in a small window but
2009  *      gsm->pending_cmd will be NULL and we just let the timer expire.
2010  */
2011
2012 static void gsm_control_retransmit(struct timer_list *t)
2013 {
2014         struct gsm_mux *gsm = from_timer(gsm, t, t2_timer);
2015         struct gsm_control *ctrl;
2016         unsigned long flags;
2017         spin_lock_irqsave(&gsm->control_lock, flags);
2018         ctrl = gsm->pending_cmd;
2019         if (ctrl) {
2020                 if (gsm->cretries == 0 || !gsm->dlci[0] || gsm->dlci[0]->dead) {
2021                         gsm->pending_cmd = NULL;
2022                         ctrl->error = -ETIMEDOUT;
2023                         ctrl->done = 1;
2024                         spin_unlock_irqrestore(&gsm->control_lock, flags);
2025                         wake_up(&gsm->event);
2026                         return;
2027                 }
2028                 gsm->cretries--;
2029                 gsm_control_transmit(gsm, ctrl);
2030                 mod_timer(&gsm->t2_timer, jiffies + gsm->t2 * HZ / 100);
2031         }
2032         spin_unlock_irqrestore(&gsm->control_lock, flags);
2033 }
2034
2035 /**
2036  *      gsm_control_send        -       send a control frame on DLCI 0
2037  *      @gsm: the GSM channel
2038  *      @command: command  to send including CR bit
2039  *      @data: bytes of data (must be kmalloced)
2040  *      @clen: length of the block to send
2041  *
2042  *      Queue and dispatch a control command. Only one command can be
2043  *      active at a time. In theory more can be outstanding but the matching
2044  *      gets really complicated so for now stick to one outstanding.
2045  */
2046
2047 static struct gsm_control *gsm_control_send(struct gsm_mux *gsm,
2048                 unsigned int command, u8 *data, int clen)
2049 {
2050         struct gsm_control *ctrl = kzalloc(sizeof(struct gsm_control),
2051                                                 GFP_ATOMIC);
2052         unsigned long flags;
2053         if (ctrl == NULL)
2054                 return NULL;
2055 retry:
2056         wait_event(gsm->event, gsm->pending_cmd == NULL);
2057         spin_lock_irqsave(&gsm->control_lock, flags);
2058         if (gsm->pending_cmd != NULL) {
2059                 spin_unlock_irqrestore(&gsm->control_lock, flags);
2060                 goto retry;
2061         }
2062         ctrl->cmd = command;
2063         ctrl->data = data;
2064         ctrl->len = clen;
2065         gsm->pending_cmd = ctrl;
2066
2067         /* If DLCI0 is in ADM mode skip retries, it won't respond */
2068         if (gsm->dlci[0]->mode == DLCI_MODE_ADM)
2069                 gsm->cretries = 0;
2070         else
2071                 gsm->cretries = gsm->n2;
2072
2073         mod_timer(&gsm->t2_timer, jiffies + gsm->t2 * HZ / 100);
2074         gsm_control_transmit(gsm, ctrl);
2075         spin_unlock_irqrestore(&gsm->control_lock, flags);
2076         return ctrl;
2077 }
2078
2079 /**
2080  *      gsm_control_wait        -       wait for a control to finish
2081  *      @gsm: GSM mux
2082  *      @control: control we are waiting on
2083  *
2084  *      Waits for the control to complete or time out. Frees any used
2085  *      resources and returns 0 for success, or an error if the remote
2086  *      rejected or ignored the request.
2087  */
2088
2089 static int gsm_control_wait(struct gsm_mux *gsm, struct gsm_control *control)
2090 {
2091         int err;
2092         wait_event(gsm->event, control->done == 1);
2093         err = control->error;
2094         kfree(control);
2095         return err;
2096 }
2097
2098
2099 /*
2100  *      DLCI level handling: Needs krefs
2101  */
2102
2103 /*
2104  *      State transitions and timers
2105  */
2106
2107 /**
2108  *      gsm_dlci_close          -       a DLCI has closed
2109  *      @dlci: DLCI that closed
2110  *
2111  *      Perform processing when moving a DLCI into closed state. If there
2112  *      is an attached tty this is hung up
2113  */
2114
2115 static void gsm_dlci_close(struct gsm_dlci *dlci)
2116 {
2117         del_timer(&dlci->t1);
2118         if (debug & DBG_ERRORS)
2119                 pr_debug("DLCI %d goes closed.\n", dlci->addr);
2120         dlci->state = DLCI_CLOSED;
2121         /* Prevent us from sending data before the link is up again */
2122         dlci->constipated = true;
2123         if (dlci->addr != 0) {
2124                 tty_port_tty_hangup(&dlci->port, false);
2125                 gsm_dlci_clear_queues(dlci->gsm, dlci);
2126                 /* Ensure that gsmtty_open() can return. */
2127                 tty_port_set_initialized(&dlci->port, false);
2128                 wake_up_interruptible(&dlci->port.open_wait);
2129         } else {
2130                 del_timer(&dlci->gsm->ka_timer);
2131                 dlci->gsm->dead = true;
2132         }
2133         /* A DLCI 0 close is a MUX termination so we need to kick that
2134            back to userspace somehow */
2135         gsm_dlci_data_kick(dlci);
2136         wake_up_all(&dlci->gsm->event);
2137 }
2138
2139 /**
2140  *      gsm_dlci_open           -       a DLCI has opened
2141  *      @dlci: DLCI that opened
2142  *
2143  *      Perform processing when moving a DLCI into open state.
2144  */
2145
2146 static void gsm_dlci_open(struct gsm_dlci *dlci)
2147 {
2148         struct gsm_mux *gsm = dlci->gsm;
2149
2150         /* Note that SABM UA .. SABM UA first UA lost can mean that we go
2151            open -> open */
2152         del_timer(&dlci->t1);
2153         /* This will let a tty open continue */
2154         dlci->state = DLCI_OPEN;
2155         dlci->constipated = false;
2156         if (debug & DBG_ERRORS)
2157                 pr_debug("DLCI %d goes open.\n", dlci->addr);
2158         /* Send current modem state */
2159         if (dlci->addr) {
2160                 gsm_modem_update(dlci, 0);
2161         } else {
2162                 /* Start keep-alive control */
2163                 gsm->ka_num = 0;
2164                 gsm->ka_retries = -1;
2165                 mod_timer(&gsm->ka_timer,
2166                           jiffies + gsm->keep_alive * HZ / 100);
2167         }
2168         gsm_dlci_data_kick(dlci);
2169         wake_up(&dlci->gsm->event);
2170 }
2171
2172 /**
2173  * gsm_dlci_negotiate   -       start parameter negotiation
2174  * @dlci: DLCI to open
2175  *
2176  * Starts the parameter negotiation for the new DLCI. This needs to be done
2177  * before the DLCI initialized the channel via SABM.
2178  */
2179 static int gsm_dlci_negotiate(struct gsm_dlci *dlci)
2180 {
2181         struct gsm_mux *gsm = dlci->gsm;
2182         struct gsm_dlci_param_bits params;
2183         int ret;
2184
2185         ret = gsm_encode_params(dlci, &params);
2186         if (ret != 0)
2187                 return ret;
2188
2189         /* We cannot asynchronous wait for the command response with
2190          * gsm_command() and gsm_control_wait() at this point.
2191          */
2192         ret = gsm_control_command(gsm, CMD_PN, (const u8 *)&params,
2193                                   sizeof(params));
2194
2195         return ret;
2196 }
2197
2198 /**
2199  *      gsm_dlci_t1             -       T1 timer expiry
2200  *      @t: timer contained in the DLCI that opened
2201  *
2202  *      The T1 timer handles retransmits of control frames (essentially of
2203  *      SABM and DISC). We resend the command until the retry count runs out
2204  *      in which case an opening port goes back to closed and a closing port
2205  *      is simply put into closed state (any further frames from the other
2206  *      end will get a DM response)
2207  *
2208  *      Some control dlci can stay in ADM mode with other dlci working just
2209  *      fine. In that case we can just keep the control dlci open after the
2210  *      DLCI_OPENING retries time out.
2211  */
2212
2213 static void gsm_dlci_t1(struct timer_list *t)
2214 {
2215         struct gsm_dlci *dlci = from_timer(dlci, t, t1);
2216         struct gsm_mux *gsm = dlci->gsm;
2217
2218         switch (dlci->state) {
2219         case DLCI_CONFIGURE:
2220                 if (dlci->retries && gsm_dlci_negotiate(dlci) == 0) {
2221                         dlci->retries--;
2222                         mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
2223                 } else {
2224                         gsm_dlci_begin_close(dlci); /* prevent half open link */
2225                 }
2226                 break;
2227         case DLCI_OPENING:
2228                 if (dlci->retries) {
2229                         dlci->retries--;
2230                         gsm_command(dlci->gsm, dlci->addr, SABM|PF);
2231                         mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
2232                 } else if (!dlci->addr && gsm->control == (DM | PF)) {
2233                         if (debug & DBG_ERRORS)
2234                                 pr_info("DLCI %d opening in ADM mode.\n",
2235                                         dlci->addr);
2236                         dlci->mode = DLCI_MODE_ADM;
2237                         gsm_dlci_open(dlci);
2238                 } else {
2239                         gsm_dlci_begin_close(dlci); /* prevent half open link */
2240                 }
2241
2242                 break;
2243         case DLCI_CLOSING:
2244                 if (dlci->retries) {
2245                         dlci->retries--;
2246                         gsm_command(dlci->gsm, dlci->addr, DISC|PF);
2247                         mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
2248                 } else
2249                         gsm_dlci_close(dlci);
2250                 break;
2251         default:
2252                 pr_debug("%s: unhandled state: %d\n", __func__, dlci->state);
2253                 break;
2254         }
2255 }
2256
2257 /**
2258  *      gsm_dlci_begin_open     -       start channel open procedure
2259  *      @dlci: DLCI to open
2260  *
2261  *      Commence opening a DLCI from the Linux side. We issue SABM messages
2262  *      to the modem which should then reply with a UA or ADM, at which point
2263  *      we will move into open state. Opening is done asynchronously with retry
2264  *      running off timers and the responses.
2265  *      Parameter negotiation is performed before SABM if required.
2266  */
2267
2268 static void gsm_dlci_begin_open(struct gsm_dlci *dlci)
2269 {
2270         struct gsm_mux *gsm = dlci ? dlci->gsm : NULL;
2271         bool need_pn = false;
2272
2273         if (!gsm)
2274                 return;
2275
2276         if (dlci->addr != 0) {
2277                 if (gsm->adaption != 1 || gsm->adaption != dlci->adaption)
2278                         need_pn = true;
2279                 if (dlci->prio != (roundup(dlci->addr + 1, 8) - 1))
2280                         need_pn = true;
2281                 if (gsm->ftype != dlci->ftype)
2282                         need_pn = true;
2283         }
2284
2285         switch (dlci->state) {
2286         case DLCI_CLOSED:
2287         case DLCI_WAITING_CONFIG:
2288         case DLCI_CLOSING:
2289                 dlci->retries = gsm->n2;
2290                 if (!need_pn) {
2291                         dlci->state = DLCI_OPENING;
2292                         gsm_command(gsm, dlci->addr, SABM|PF);
2293                 } else {
2294                         /* Configure DLCI before setup */
2295                         dlci->state = DLCI_CONFIGURE;
2296                         if (gsm_dlci_negotiate(dlci) != 0) {
2297                                 gsm_dlci_close(dlci);
2298                                 return;
2299                         }
2300                 }
2301                 mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
2302                 break;
2303         default:
2304                 break;
2305         }
2306 }
2307
2308 /**
2309  *      gsm_dlci_set_opening    -       change state to opening
2310  *      @dlci: DLCI to open
2311  *
2312  *      Change internal state to wait for DLCI open from initiator side.
2313  *      We set off timers and responses upon reception of an SABM.
2314  */
2315 static void gsm_dlci_set_opening(struct gsm_dlci *dlci)
2316 {
2317         switch (dlci->state) {
2318         case DLCI_CLOSED:
2319         case DLCI_WAITING_CONFIG:
2320         case DLCI_CLOSING:
2321                 dlci->state = DLCI_OPENING;
2322                 break;
2323         default:
2324                 break;
2325         }
2326 }
2327
2328 /**
2329  * gsm_dlci_set_wait_config     -       wait for channel configuration
2330  * @dlci: DLCI to configure
2331  *
2332  * Wait for a DLCI configuration from the application.
2333  */
2334 static void gsm_dlci_set_wait_config(struct gsm_dlci *dlci)
2335 {
2336         switch (dlci->state) {
2337         case DLCI_CLOSED:
2338         case DLCI_CLOSING:
2339                 dlci->state = DLCI_WAITING_CONFIG;
2340                 break;
2341         default:
2342                 break;
2343         }
2344 }
2345
2346 /**
2347  *      gsm_dlci_begin_close    -       start channel open procedure
2348  *      @dlci: DLCI to open
2349  *
2350  *      Commence closing a DLCI from the Linux side. We issue DISC messages
2351  *      to the modem which should then reply with a UA, at which point we
2352  *      will move into closed state. Closing is done asynchronously with retry
2353  *      off timers. We may also receive a DM reply from the other end which
2354  *      indicates the channel was already closed.
2355  */
2356
2357 static void gsm_dlci_begin_close(struct gsm_dlci *dlci)
2358 {
2359         struct gsm_mux *gsm = dlci->gsm;
2360         if (dlci->state == DLCI_CLOSED || dlci->state == DLCI_CLOSING)
2361                 return;
2362         dlci->retries = gsm->n2;
2363         dlci->state = DLCI_CLOSING;
2364         gsm_command(dlci->gsm, dlci->addr, DISC|PF);
2365         mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
2366         wake_up_interruptible(&gsm->event);
2367 }
2368
2369 /**
2370  *      gsm_dlci_data           -       data arrived
2371  *      @dlci: channel
2372  *      @data: block of bytes received
2373  *      @clen: length of received block
2374  *
2375  *      A UI or UIH frame has arrived which contains data for a channel
2376  *      other than the control channel. If the relevant virtual tty is
2377  *      open we shovel the bits down it, if not we drop them.
2378  */
2379
2380 static void gsm_dlci_data(struct gsm_dlci *dlci, const u8 *data, int clen)
2381 {
2382         /* krefs .. */
2383         struct tty_port *port = &dlci->port;
2384         struct tty_struct *tty;
2385         unsigned int modem = 0;
2386         int len;
2387
2388         if (debug & DBG_TTY)
2389                 pr_debug("%d bytes for tty\n", clen);
2390         switch (dlci->adaption)  {
2391         /* Unsupported types */
2392         case 4:         /* Packetised interruptible data */
2393                 break;
2394         case 3:         /* Packetised uininterruptible voice/data */
2395                 break;
2396         case 2:         /* Asynchronous serial with line state in each frame */
2397                 len = gsm_read_ea_val(&modem, data, clen);
2398                 if (len < 1)
2399                         return;
2400                 tty = tty_port_tty_get(port);
2401                 if (tty) {
2402                         gsm_process_modem(tty, dlci, modem, len);
2403                         tty_wakeup(tty);
2404                         tty_kref_put(tty);
2405                 }
2406                 /* Skip processed modem data */
2407                 data += len;
2408                 clen -= len;
2409                 fallthrough;
2410         case 1:         /* Line state will go via DLCI 0 controls only */
2411         default:
2412                 tty_insert_flip_string(port, data, clen);
2413                 tty_flip_buffer_push(port);
2414         }
2415 }
2416
2417 /**
2418  *      gsm_dlci_command        -       data arrived on control channel
2419  *      @dlci: channel
2420  *      @data: block of bytes received
2421  *      @len: length of received block
2422  *
2423  *      A UI or UIH frame has arrived which contains data for DLCI 0 the
2424  *      control channel. This should contain a command EA followed by
2425  *      control data bytes. The command EA contains a command/response bit
2426  *      and we divide up the work accordingly.
2427  */
2428
2429 static void gsm_dlci_command(struct gsm_dlci *dlci, const u8 *data, int len)
2430 {
2431         /* See what command is involved */
2432         unsigned int command = 0;
2433         unsigned int clen = 0;
2434         unsigned int dlen;
2435
2436         /* read the command */
2437         dlen = gsm_read_ea_val(&command, data, len);
2438         len -= dlen;
2439         data += dlen;
2440
2441         /* read any control data */
2442         dlen = gsm_read_ea_val(&clen, data, len);
2443         len -= dlen;
2444         data += dlen;
2445
2446         /* Malformed command? */
2447         if (clen > len)
2448                 return;
2449
2450         if (command & 1)
2451                 gsm_control_message(dlci->gsm, command, data, clen);
2452         else
2453                 gsm_control_response(dlci->gsm, command, data, clen);
2454 }
2455
2456 /**
2457  *      gsm_kick_timer  -       transmit if possible
2458  *      @t: timer contained in our gsm object
2459  *
2460  *      Transmit data from DLCIs if the queue is empty. We can't rely on
2461  *      a tty wakeup except when we filled the pipe so we need to fire off
2462  *      new data ourselves in other cases.
2463  */
2464 static void gsm_kick_timer(struct timer_list *t)
2465 {
2466         struct gsm_mux *gsm = from_timer(gsm, t, kick_timer);
2467         unsigned long flags;
2468         int sent = 0;
2469
2470         spin_lock_irqsave(&gsm->tx_lock, flags);
2471         /* If we have nothing running then we need to fire up */
2472         if (gsm->tx_bytes < TX_THRESH_LO)
2473                 sent = gsm_dlci_data_sweep(gsm);
2474         spin_unlock_irqrestore(&gsm->tx_lock, flags);
2475
2476         if (sent && debug & DBG_DATA)
2477                 pr_info("%s TX queue stalled\n", __func__);
2478 }
2479
2480 /**
2481  * gsm_dlci_copy_config_values  -       copy DLCI configuration
2482  * @dlci: source DLCI
2483  * @dc: configuration structure to fill
2484  */
2485 static void gsm_dlci_copy_config_values(struct gsm_dlci *dlci, struct gsm_dlci_config *dc)
2486 {
2487         memset(dc, 0, sizeof(*dc));
2488         dc->channel = (u32)dlci->addr;
2489         dc->adaption = (u32)dlci->adaption;
2490         dc->mtu = (u32)dlci->mtu;
2491         dc->priority = (u32)dlci->prio;
2492         if (dlci->ftype == UIH)
2493                 dc->i = 1;
2494         else
2495                 dc->i = 2;
2496         dc->k = (u32)dlci->k;
2497 }
2498
2499 /**
2500  * gsm_dlci_config      -       configure DLCI from configuration
2501  * @dlci: DLCI to configure
2502  * @dc: DLCI configuration
2503  * @open: open DLCI after configuration?
2504  */
2505 static int gsm_dlci_config(struct gsm_dlci *dlci, struct gsm_dlci_config *dc, int open)
2506 {
2507         struct gsm_mux *gsm;
2508         bool need_restart = false;
2509         bool need_open = false;
2510         unsigned int i;
2511
2512         /*
2513          * Check that userspace doesn't put stuff in here to prevent breakages
2514          * in the future.
2515          */
2516         for (i = 0; i < ARRAY_SIZE(dc->reserved); i++)
2517                 if (dc->reserved[i])
2518                         return -EINVAL;
2519
2520         if (!dlci)
2521                 return -EINVAL;
2522         gsm = dlci->gsm;
2523
2524         /* Stuff we don't support yet - I frame transport */
2525         if (dc->adaption != 1 && dc->adaption != 2)
2526                 return -EOPNOTSUPP;
2527         if (dc->mtu > MAX_MTU || dc->mtu < MIN_MTU || dc->mtu > gsm->mru)
2528                 return -EINVAL;
2529         if (dc->priority >= 64)
2530                 return -EINVAL;
2531         if (dc->i == 0 || dc->i > 2)  /* UIH and UI only */
2532                 return -EINVAL;
2533         if (dc->k > 7)
2534                 return -EINVAL;
2535
2536         /*
2537          * See what is needed for reconfiguration
2538          */
2539         /* Framing fields */
2540         if (dc->adaption != dlci->adaption)
2541                 need_restart = true;
2542         if (dc->mtu != dlci->mtu)
2543                 need_restart = true;
2544         if (dc->i != dlci->ftype)
2545                 need_restart = true;
2546         /* Requires care */
2547         if (dc->priority != dlci->prio)
2548                 need_restart = true;
2549
2550         if ((open && gsm->wait_config) || need_restart)
2551                 need_open = true;
2552         if (dlci->state == DLCI_WAITING_CONFIG) {
2553                 need_restart = false;
2554                 need_open = true;
2555         }
2556
2557         /*
2558          * Close down what is needed, restart and initiate the new
2559          * configuration.
2560          */
2561         if (need_restart) {
2562                 gsm_dlci_begin_close(dlci);
2563                 wait_event_interruptible(gsm->event, dlci->state == DLCI_CLOSED);
2564                 if (signal_pending(current))
2565                         return -EINTR;
2566         }
2567         /*
2568          * Setup the new configuration values
2569          */
2570         dlci->adaption = (int)dc->adaption;
2571
2572         if (dc->mtu)
2573                 dlci->mtu = (unsigned int)dc->mtu;
2574         else
2575                 dlci->mtu = gsm->mtu;
2576
2577         if (dc->priority)
2578                 dlci->prio = (u8)dc->priority;
2579         else
2580                 dlci->prio = roundup(dlci->addr + 1, 8) - 1;
2581
2582         if (dc->i == 1)
2583                 dlci->ftype = UIH;
2584         else if (dc->i == 2)
2585                 dlci->ftype = UI;
2586
2587         if (dc->k)
2588                 dlci->k = (u8)dc->k;
2589         else
2590                 dlci->k = gsm->k;
2591
2592         if (need_open) {
2593                 if (gsm->initiator)
2594                         gsm_dlci_begin_open(dlci);
2595                 else
2596                         gsm_dlci_set_opening(dlci);
2597         }
2598
2599         return 0;
2600 }
2601
2602 /*
2603  *      Allocate/Free DLCI channels
2604  */
2605
2606 /**
2607  *      gsm_dlci_alloc          -       allocate a DLCI
2608  *      @gsm: GSM mux
2609  *      @addr: address of the DLCI
2610  *
2611  *      Allocate and install a new DLCI object into the GSM mux.
2612  *
2613  *      FIXME: review locking races
2614  */
2615
2616 static struct gsm_dlci *gsm_dlci_alloc(struct gsm_mux *gsm, int addr)
2617 {
2618         struct gsm_dlci *dlci = kzalloc(sizeof(struct gsm_dlci), GFP_ATOMIC);
2619         if (dlci == NULL)
2620                 return NULL;
2621         spin_lock_init(&dlci->lock);
2622         mutex_init(&dlci->mutex);
2623         if (kfifo_alloc(&dlci->fifo, TX_SIZE, GFP_KERNEL) < 0) {
2624                 kfree(dlci);
2625                 return NULL;
2626         }
2627
2628         skb_queue_head_init(&dlci->skb_list);
2629         timer_setup(&dlci->t1, gsm_dlci_t1, 0);
2630         tty_port_init(&dlci->port);
2631         dlci->port.ops = &gsm_port_ops;
2632         dlci->gsm = gsm;
2633         dlci->addr = addr;
2634         dlci->adaption = gsm->adaption;
2635         dlci->mtu = gsm->mtu;
2636         if (addr == 0)
2637                 dlci->prio = 0;
2638         else
2639                 dlci->prio = roundup(addr + 1, 8) - 1;
2640         dlci->ftype = gsm->ftype;
2641         dlci->k = gsm->k;
2642         dlci->state = DLCI_CLOSED;
2643         if (addr) {
2644                 dlci->data = gsm_dlci_data;
2645                 /* Prevent us from sending data before the link is up */
2646                 dlci->constipated = true;
2647         } else {
2648                 dlci->data = gsm_dlci_command;
2649         }
2650         gsm->dlci[addr] = dlci;
2651         return dlci;
2652 }
2653
2654 /**
2655  *      gsm_dlci_free           -       free DLCI
2656  *      @port: tty port for DLCI to free
2657  *
2658  *      Free up a DLCI.
2659  *
2660  *      Can sleep.
2661  */
2662 static void gsm_dlci_free(struct tty_port *port)
2663 {
2664         struct gsm_dlci *dlci = container_of(port, struct gsm_dlci, port);
2665
2666         timer_shutdown_sync(&dlci->t1);
2667         dlci->gsm->dlci[dlci->addr] = NULL;
2668         kfifo_free(&dlci->fifo);
2669         while ((dlci->skb = skb_dequeue(&dlci->skb_list)))
2670                 dev_kfree_skb(dlci->skb);
2671         kfree(dlci);
2672 }
2673
2674 static inline void dlci_get(struct gsm_dlci *dlci)
2675 {
2676         tty_port_get(&dlci->port);
2677 }
2678
2679 static inline void dlci_put(struct gsm_dlci *dlci)
2680 {
2681         tty_port_put(&dlci->port);
2682 }
2683
2684 static void gsm_destroy_network(struct gsm_dlci *dlci);
2685
2686 /**
2687  *      gsm_dlci_release                -       release DLCI
2688  *      @dlci: DLCI to destroy
2689  *
2690  *      Release a DLCI. Actual free is deferred until either
2691  *      mux is closed or tty is closed - whichever is last.
2692  *
2693  *      Can sleep.
2694  */
2695 static void gsm_dlci_release(struct gsm_dlci *dlci)
2696 {
2697         struct tty_struct *tty = tty_port_tty_get(&dlci->port);
2698         if (tty) {
2699                 mutex_lock(&dlci->mutex);
2700                 gsm_destroy_network(dlci);
2701                 mutex_unlock(&dlci->mutex);
2702
2703                 /* We cannot use tty_hangup() because in tty_kref_put() the tty
2704                  * driver assumes that the hangup queue is free and reuses it to
2705                  * queue release_one_tty() -> NULL pointer panic in
2706                  * process_one_work().
2707                  */
2708                 tty_vhangup(tty);
2709
2710                 tty_port_tty_set(&dlci->port, NULL);
2711                 tty_kref_put(tty);
2712         }
2713         dlci->state = DLCI_CLOSED;
2714         dlci_put(dlci);
2715 }
2716
2717 /*
2718  *      LAPBish link layer logic
2719  */
2720
2721 /**
2722  *      gsm_queue               -       a GSM frame is ready to process
2723  *      @gsm: pointer to our gsm mux
2724  *
2725  *      At this point in time a frame has arrived and been demangled from
2726  *      the line encoding. All the differences between the encodings have
2727  *      been handled below us and the frame is unpacked into the structures.
2728  *      The fcs holds the header FCS but any data FCS must be added here.
2729  */
2730
2731 static void gsm_queue(struct gsm_mux *gsm)
2732 {
2733         struct gsm_dlci *dlci;
2734         u8 cr;
2735         int address;
2736
2737         if (gsm->fcs != GOOD_FCS) {
2738                 gsm->bad_fcs++;
2739                 if (debug & DBG_DATA)
2740                         pr_debug("BAD FCS %02x\n", gsm->fcs);
2741                 return;
2742         }
2743         address = gsm->address >> 1;
2744         if (address >= NUM_DLCI)
2745                 goto invalid;
2746
2747         cr = gsm->address & 1;          /* C/R bit */
2748         cr ^= gsm->initiator ? 0 : 1;   /* Flip so 1 always means command */
2749
2750         gsm_print_packet("<--", address, cr, gsm->control, gsm->buf, gsm->len);
2751
2752         dlci = gsm->dlci[address];
2753
2754         switch (gsm->control) {
2755         case SABM|PF:
2756                 if (cr == 1)
2757                         goto invalid;
2758                 if (dlci == NULL)
2759                         dlci = gsm_dlci_alloc(gsm, address);
2760                 if (dlci == NULL)
2761                         return;
2762                 if (dlci->dead)
2763                         gsm_response(gsm, address, DM|PF);
2764                 else {
2765                         gsm_response(gsm, address, UA|PF);
2766                         gsm_dlci_open(dlci);
2767                 }
2768                 break;
2769         case DISC|PF:
2770                 if (cr == 1)
2771                         goto invalid;
2772                 if (dlci == NULL || dlci->state == DLCI_CLOSED) {
2773                         gsm_response(gsm, address, DM|PF);
2774                         return;
2775                 }
2776                 /* Real close complete */
2777                 gsm_response(gsm, address, UA|PF);
2778                 gsm_dlci_close(dlci);
2779                 break;
2780         case UA|PF:
2781                 if (cr == 0 || dlci == NULL)
2782                         break;
2783                 switch (dlci->state) {
2784                 case DLCI_CLOSING:
2785                         gsm_dlci_close(dlci);
2786                         break;
2787                 case DLCI_OPENING:
2788                         gsm_dlci_open(dlci);
2789                         break;
2790                 default:
2791                         pr_debug("%s: unhandled state: %d\n", __func__,
2792                                         dlci->state);
2793                         break;
2794                 }
2795                 break;
2796         case DM:        /* DM can be valid unsolicited */
2797         case DM|PF:
2798                 if (cr)
2799                         goto invalid;
2800                 if (dlci == NULL)
2801                         return;
2802                 gsm_dlci_close(dlci);
2803                 break;
2804         case UI:
2805         case UI|PF:
2806         case UIH:
2807         case UIH|PF:
2808                 if (dlci == NULL || dlci->state != DLCI_OPEN) {
2809                         gsm_response(gsm, address, DM|PF);
2810                         return;
2811                 }
2812                 dlci->data(dlci, gsm->buf, gsm->len);
2813                 break;
2814         default:
2815                 goto invalid;
2816         }
2817         return;
2818 invalid:
2819         gsm->malformed++;
2820         return;
2821 }
2822
2823
2824 /**
2825  *      gsm0_receive    -       perform processing for non-transparency
2826  *      @gsm: gsm data for this ldisc instance
2827  *      @c: character
2828  *
2829  *      Receive bytes in gsm mode 0
2830  */
2831
2832 static void gsm0_receive(struct gsm_mux *gsm, unsigned char c)
2833 {
2834         unsigned int len;
2835
2836         switch (gsm->state) {
2837         case GSM_SEARCH:        /* SOF marker */
2838                 if (c == GSM0_SOF) {
2839                         gsm->state = GSM_ADDRESS;
2840                         gsm->address = 0;
2841                         gsm->len = 0;
2842                         gsm->fcs = INIT_FCS;
2843                 }
2844                 break;
2845         case GSM_ADDRESS:       /* Address EA */
2846                 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
2847                 if (gsm_read_ea(&gsm->address, c))
2848                         gsm->state = GSM_CONTROL;
2849                 break;
2850         case GSM_CONTROL:       /* Control Byte */
2851                 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
2852                 gsm->control = c;
2853                 gsm->state = GSM_LEN0;
2854                 break;
2855         case GSM_LEN0:          /* Length EA */
2856                 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
2857                 if (gsm_read_ea(&gsm->len, c)) {
2858                         if (gsm->len > gsm->mru) {
2859                                 gsm->bad_size++;
2860                                 gsm->state = GSM_SEARCH;
2861                                 break;
2862                         }
2863                         gsm->count = 0;
2864                         if (!gsm->len)
2865                                 gsm->state = GSM_FCS;
2866                         else
2867                                 gsm->state = GSM_DATA;
2868                         break;
2869                 }
2870                 gsm->state = GSM_LEN1;
2871                 break;
2872         case GSM_LEN1:
2873                 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
2874                 len = c;
2875                 gsm->len |= len << 7;
2876                 if (gsm->len > gsm->mru) {
2877                         gsm->bad_size++;
2878                         gsm->state = GSM_SEARCH;
2879                         break;
2880                 }
2881                 gsm->count = 0;
2882                 if (!gsm->len)
2883                         gsm->state = GSM_FCS;
2884                 else
2885                         gsm->state = GSM_DATA;
2886                 break;
2887         case GSM_DATA:          /* Data */
2888                 gsm->buf[gsm->count++] = c;
2889                 if (gsm->count == gsm->len) {
2890                         /* Calculate final FCS for UI frames over all data */
2891                         if ((gsm->control & ~PF) != UIH) {
2892                                 gsm->fcs = gsm_fcs_add_block(gsm->fcs, gsm->buf,
2893                                                              gsm->count);
2894                         }
2895                         gsm->state = GSM_FCS;
2896                 }
2897                 break;
2898         case GSM_FCS:           /* FCS follows the packet */
2899                 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
2900                 gsm->state = GSM_SSOF;
2901                 break;
2902         case GSM_SSOF:
2903                 gsm->state = GSM_SEARCH;
2904                 if (c == GSM0_SOF)
2905                         gsm_queue(gsm);
2906                 else
2907                         gsm->bad_size++;
2908                 break;
2909         default:
2910                 pr_debug("%s: unhandled state: %d\n", __func__, gsm->state);
2911                 break;
2912         }
2913 }
2914
2915 /**
2916  *      gsm1_receive    -       perform processing for non-transparency
2917  *      @gsm: gsm data for this ldisc instance
2918  *      @c: character
2919  *
2920  *      Receive bytes in mode 1 (Advanced option)
2921  */
2922
2923 static void gsm1_receive(struct gsm_mux *gsm, unsigned char c)
2924 {
2925         /* handle XON/XOFF */
2926         if ((c & ISO_IEC_646_MASK) == XON) {
2927                 gsm->constipated = true;
2928                 return;
2929         } else if ((c & ISO_IEC_646_MASK) == XOFF) {
2930                 gsm->constipated = false;
2931                 /* Kick the link in case it is idling */
2932                 gsmld_write_trigger(gsm);
2933                 return;
2934         }
2935         if (c == GSM1_SOF) {
2936                 /* EOF is only valid in frame if we have got to the data state */
2937                 if (gsm->state == GSM_DATA) {
2938                         if (gsm->count < 1) {
2939                                 /* Missing FSC */
2940                                 gsm->malformed++;
2941                                 gsm->state = GSM_START;
2942                                 return;
2943                         }
2944                         /* Remove the FCS from data */
2945                         gsm->count--;
2946                         if ((gsm->control & ~PF) != UIH) {
2947                                 /* Calculate final FCS for UI frames over all
2948                                  * data but FCS
2949                                  */
2950                                 gsm->fcs = gsm_fcs_add_block(gsm->fcs, gsm->buf,
2951                                                              gsm->count);
2952                         }
2953                         /* Add the FCS itself to test against GOOD_FCS */
2954                         gsm->fcs = gsm_fcs_add(gsm->fcs, gsm->buf[gsm->count]);
2955                         gsm->len = gsm->count;
2956                         gsm_queue(gsm);
2957                         gsm->state  = GSM_START;
2958                         return;
2959                 }
2960                 /* Any partial frame was a runt so go back to start */
2961                 if (gsm->state != GSM_START) {
2962                         if (gsm->state != GSM_SEARCH)
2963                                 gsm->malformed++;
2964                         gsm->state = GSM_START;
2965                 }
2966                 /* A SOF in GSM_START means we are still reading idling or
2967                    framing bytes */
2968                 return;
2969         }
2970
2971         if (c == GSM1_ESCAPE) {
2972                 gsm->escape = true;
2973                 return;
2974         }
2975
2976         /* Only an unescaped SOF gets us out of GSM search */
2977         if (gsm->state == GSM_SEARCH)
2978                 return;
2979
2980         if (gsm->escape) {
2981                 c ^= GSM1_ESCAPE_BITS;
2982                 gsm->escape = false;
2983         }
2984         switch (gsm->state) {
2985         case GSM_START:         /* First byte after SOF */
2986                 gsm->address = 0;
2987                 gsm->state = GSM_ADDRESS;
2988                 gsm->fcs = INIT_FCS;
2989                 fallthrough;
2990         case GSM_ADDRESS:       /* Address continuation */
2991                 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
2992                 if (gsm_read_ea(&gsm->address, c))
2993                         gsm->state = GSM_CONTROL;
2994                 break;
2995         case GSM_CONTROL:       /* Control Byte */
2996                 gsm->fcs = gsm_fcs_add(gsm->fcs, c);
2997                 gsm->control = c;
2998                 gsm->count = 0;
2999                 gsm->state = GSM_DATA;
3000                 break;
3001         case GSM_DATA:          /* Data */
3002                 if (gsm->count > gsm->mru) {    /* Allow one for the FCS */
3003                         gsm->state = GSM_OVERRUN;
3004                         gsm->bad_size++;
3005                 } else
3006                         gsm->buf[gsm->count++] = c;
3007                 break;
3008         case GSM_OVERRUN:       /* Over-long - eg a dropped SOF */
3009                 break;
3010         default:
3011                 pr_debug("%s: unhandled state: %d\n", __func__, gsm->state);
3012                 break;
3013         }
3014 }
3015
3016 /**
3017  *      gsm_error               -       handle tty error
3018  *      @gsm: ldisc data
3019  *
3020  *      Handle an error in the receipt of data for a frame. Currently we just
3021  *      go back to hunting for a SOF.
3022  *
3023  *      FIXME: better diagnostics ?
3024  */
3025
3026 static void gsm_error(struct gsm_mux *gsm)
3027 {
3028         gsm->state = GSM_SEARCH;
3029         gsm->io_error++;
3030 }
3031
3032 /**
3033  *      gsm_cleanup_mux         -       generic GSM protocol cleanup
3034  *      @gsm: our mux
3035  *      @disc: disconnect link?
3036  *
3037  *      Clean up the bits of the mux which are the same for all framing
3038  *      protocols. Remove the mux from the mux table, stop all the timers
3039  *      and then shut down each device hanging up the channels as we go.
3040  */
3041
3042 static void gsm_cleanup_mux(struct gsm_mux *gsm, bool disc)
3043 {
3044         int i;
3045         struct gsm_dlci *dlci = gsm->dlci[0];
3046         struct gsm_msg *txq, *ntxq;
3047
3048         gsm->dead = true;
3049         mutex_lock(&gsm->mutex);
3050
3051         if (dlci) {
3052                 if (disc && dlci->state != DLCI_CLOSED) {
3053                         gsm_dlci_begin_close(dlci);
3054                         wait_event(gsm->event, dlci->state == DLCI_CLOSED);
3055                 }
3056                 dlci->dead = true;
3057         }
3058
3059         /* Finish outstanding timers, making sure they are done */
3060         del_timer_sync(&gsm->kick_timer);
3061         del_timer_sync(&gsm->t2_timer);
3062         del_timer_sync(&gsm->ka_timer);
3063
3064         /* Finish writing to ldisc */
3065         flush_work(&gsm->tx_work);
3066
3067         /* Free up any link layer users and finally the control channel */
3068         if (gsm->has_devices) {
3069                 gsm_unregister_devices(gsm_tty_driver, gsm->num);
3070                 gsm->has_devices = false;
3071         }
3072         for (i = NUM_DLCI - 1; i >= 0; i--)
3073                 if (gsm->dlci[i]) {
3074                         gsm_dlci_release(gsm->dlci[i]);
3075                         gsm->dlci[i] = NULL;
3076                 }
3077         mutex_unlock(&gsm->mutex);
3078         /* Now wipe the queues */
3079         tty_ldisc_flush(gsm->tty);
3080         list_for_each_entry_safe(txq, ntxq, &gsm->tx_ctrl_list, list)
3081                 kfree(txq);
3082         INIT_LIST_HEAD(&gsm->tx_ctrl_list);
3083         list_for_each_entry_safe(txq, ntxq, &gsm->tx_data_list, list)
3084                 kfree(txq);
3085         INIT_LIST_HEAD(&gsm->tx_data_list);
3086 }
3087
3088 /**
3089  *      gsm_activate_mux        -       generic GSM setup
3090  *      @gsm: our mux
3091  *
3092  *      Set up the bits of the mux which are the same for all framing
3093  *      protocols. Add the mux to the mux table so it can be opened and
3094  *      finally kick off connecting to DLCI 0 on the modem.
3095  */
3096
3097 static int gsm_activate_mux(struct gsm_mux *gsm)
3098 {
3099         struct gsm_dlci *dlci;
3100         int ret;
3101
3102         dlci = gsm_dlci_alloc(gsm, 0);
3103         if (dlci == NULL)
3104                 return -ENOMEM;
3105
3106         if (gsm->encoding == GSM_BASIC_OPT)
3107                 gsm->receive = gsm0_receive;
3108         else
3109                 gsm->receive = gsm1_receive;
3110
3111         ret = gsm_register_devices(gsm_tty_driver, gsm->num);
3112         if (ret)
3113                 return ret;
3114
3115         gsm->has_devices = true;
3116         gsm->dead = false;              /* Tty opens are now permissible */
3117         return 0;
3118 }
3119
3120 /**
3121  *      gsm_free_mux            -       free up a mux
3122  *      @gsm: mux to free
3123  *
3124  *      Dispose of allocated resources for a dead mux
3125  */
3126 static void gsm_free_mux(struct gsm_mux *gsm)
3127 {
3128         int i;
3129
3130         for (i = 0; i < MAX_MUX; i++) {
3131                 if (gsm == gsm_mux[i]) {
3132                         gsm_mux[i] = NULL;
3133                         break;
3134                 }
3135         }
3136         mutex_destroy(&gsm->mutex);
3137         kfree(gsm->txframe);
3138         kfree(gsm->buf);
3139         kfree(gsm);
3140 }
3141
3142 /**
3143  *      gsm_free_muxr           -       free up a mux
3144  *      @ref: kreference to the mux to free
3145  *
3146  *      Dispose of allocated resources for a dead mux
3147  */
3148 static void gsm_free_muxr(struct kref *ref)
3149 {
3150         struct gsm_mux *gsm = container_of(ref, struct gsm_mux, ref);
3151         gsm_free_mux(gsm);
3152 }
3153
3154 static inline void mux_get(struct gsm_mux *gsm)
3155 {
3156         unsigned long flags;
3157
3158         spin_lock_irqsave(&gsm_mux_lock, flags);
3159         kref_get(&gsm->ref);
3160         spin_unlock_irqrestore(&gsm_mux_lock, flags);
3161 }
3162
3163 static inline void mux_put(struct gsm_mux *gsm)
3164 {
3165         unsigned long flags;
3166
3167         spin_lock_irqsave(&gsm_mux_lock, flags);
3168         kref_put(&gsm->ref, gsm_free_muxr);
3169         spin_unlock_irqrestore(&gsm_mux_lock, flags);
3170 }
3171
3172 static inline unsigned int mux_num_to_base(struct gsm_mux *gsm)
3173 {
3174         return gsm->num * NUM_DLCI;
3175 }
3176
3177 static inline unsigned int mux_line_to_num(unsigned int line)
3178 {
3179         return line / NUM_DLCI;
3180 }
3181
3182 /**
3183  *      gsm_alloc_mux           -       allocate a mux
3184  *
3185  *      Creates a new mux ready for activation.
3186  */
3187
3188 static struct gsm_mux *gsm_alloc_mux(void)
3189 {
3190         int i;
3191         struct gsm_mux *gsm = kzalloc(sizeof(struct gsm_mux), GFP_KERNEL);
3192         if (gsm == NULL)
3193                 return NULL;
3194         gsm->buf = kmalloc(MAX_MRU + 1, GFP_KERNEL);
3195         if (gsm->buf == NULL) {
3196                 kfree(gsm);
3197                 return NULL;
3198         }
3199         gsm->txframe = kmalloc(2 * (MAX_MTU + PROT_OVERHEAD - 1), GFP_KERNEL);
3200         if (gsm->txframe == NULL) {
3201                 kfree(gsm->buf);
3202                 kfree(gsm);
3203                 return NULL;
3204         }
3205         spin_lock_init(&gsm->lock);
3206         mutex_init(&gsm->mutex);
3207         kref_init(&gsm->ref);
3208         INIT_LIST_HEAD(&gsm->tx_ctrl_list);
3209         INIT_LIST_HEAD(&gsm->tx_data_list);
3210         timer_setup(&gsm->kick_timer, gsm_kick_timer, 0);
3211         timer_setup(&gsm->t2_timer, gsm_control_retransmit, 0);
3212         timer_setup(&gsm->ka_timer, gsm_control_keep_alive, 0);
3213         INIT_WORK(&gsm->tx_work, gsmld_write_task);
3214         init_waitqueue_head(&gsm->event);
3215         spin_lock_init(&gsm->control_lock);
3216         spin_lock_init(&gsm->tx_lock);
3217
3218         gsm->t1 = T1;
3219         gsm->t2 = T2;
3220         gsm->t3 = T3;
3221         gsm->n2 = N2;
3222         gsm->k = K;
3223         gsm->ftype = UIH;
3224         gsm->adaption = 1;
3225         gsm->encoding = GSM_ADV_OPT;
3226         gsm->mru = 64;  /* Default to encoding 1 so these should be 64 */
3227         gsm->mtu = 64;
3228         gsm->dead = true;       /* Avoid early tty opens */
3229         gsm->wait_config = false; /* Disabled */
3230         gsm->keep_alive = 0;    /* Disabled */
3231
3232         /* Store the instance to the mux array or abort if no space is
3233          * available.
3234          */
3235         spin_lock(&gsm_mux_lock);
3236         for (i = 0; i < MAX_MUX; i++) {
3237                 if (!gsm_mux[i]) {
3238                         gsm_mux[i] = gsm;
3239                         gsm->num = i;
3240                         break;
3241                 }
3242         }
3243         spin_unlock(&gsm_mux_lock);
3244         if (i == MAX_MUX) {
3245                 mutex_destroy(&gsm->mutex);
3246                 kfree(gsm->txframe);
3247                 kfree(gsm->buf);
3248                 kfree(gsm);
3249                 return NULL;
3250         }
3251
3252         return gsm;
3253 }
3254
3255 static void gsm_copy_config_values(struct gsm_mux *gsm,
3256                                    struct gsm_config *c)
3257 {
3258         memset(c, 0, sizeof(*c));
3259         c->adaption = gsm->adaption;
3260         c->encapsulation = gsm->encoding;
3261         c->initiator = gsm->initiator;
3262         c->t1 = gsm->t1;
3263         c->t2 = gsm->t2;
3264         c->t3 = gsm->t3;
3265         c->n2 = gsm->n2;
3266         if (gsm->ftype == UIH)
3267                 c->i = 1;
3268         else
3269                 c->i = 2;
3270         pr_debug("Ftype %d i %d\n", gsm->ftype, c->i);
3271         c->mru = gsm->mru;
3272         c->mtu = gsm->mtu;
3273         c->k = gsm->k;
3274 }
3275
3276 static int gsm_config(struct gsm_mux *gsm, struct gsm_config *c)
3277 {
3278         int ret = 0;
3279         int need_close = 0;
3280         int need_restart = 0;
3281
3282         /* Stuff we don't support yet - UI or I frame transport */
3283         if (c->adaption != 1 && c->adaption != 2)
3284                 return -EOPNOTSUPP;
3285         /* Check the MRU/MTU range looks sane */
3286         if (c->mru < MIN_MTU || c->mtu < MIN_MTU)
3287                 return -EINVAL;
3288         if (c->mru > MAX_MRU || c->mtu > MAX_MTU)
3289                 return -EINVAL;
3290         if (c->t3 > MAX_T3)
3291                 return -EINVAL;
3292         if (c->n2 > 255)
3293                 return -EINVAL;
3294         if (c->encapsulation > 1)       /* Basic, advanced, no I */
3295                 return -EINVAL;
3296         if (c->initiator > 1)
3297                 return -EINVAL;
3298         if (c->k > MAX_WINDOW_SIZE)
3299                 return -EINVAL;
3300         if (c->i == 0 || c->i > 2)      /* UIH and UI only */
3301                 return -EINVAL;
3302         /*
3303          * See what is needed for reconfiguration
3304          */
3305
3306         /* Timing fields */
3307         if (c->t1 != 0 && c->t1 != gsm->t1)
3308                 need_restart = 1;
3309         if (c->t2 != 0 && c->t2 != gsm->t2)
3310                 need_restart = 1;
3311         if (c->encapsulation != gsm->encoding)
3312                 need_restart = 1;
3313         if (c->adaption != gsm->adaption)
3314                 need_restart = 1;
3315         /* Requires care */
3316         if (c->initiator != gsm->initiator)
3317                 need_close = 1;
3318         if (c->mru != gsm->mru)
3319                 need_restart = 1;
3320         if (c->mtu != gsm->mtu)
3321                 need_restart = 1;
3322
3323         /*
3324          * Close down what is needed, restart and initiate the new
3325          * configuration. On the first time there is no DLCI[0]
3326          * and closing or cleaning up is not necessary.
3327          */
3328         if (need_close || need_restart)
3329                 gsm_cleanup_mux(gsm, true);
3330
3331         gsm->initiator = c->initiator;
3332         gsm->mru = c->mru;
3333         gsm->mtu = c->mtu;
3334         gsm->encoding = c->encapsulation ? GSM_ADV_OPT : GSM_BASIC_OPT;
3335         gsm->adaption = c->adaption;
3336         gsm->n2 = c->n2;
3337
3338         if (c->i == 1)
3339                 gsm->ftype = UIH;
3340         else if (c->i == 2)
3341                 gsm->ftype = UI;
3342
3343         if (c->t1)
3344                 gsm->t1 = c->t1;
3345         if (c->t2)
3346                 gsm->t2 = c->t2;
3347         if (c->t3)
3348                 gsm->t3 = c->t3;
3349         if (c->k)
3350                 gsm->k = c->k;
3351
3352         /*
3353          * FIXME: We need to separate activation/deactivation from adding
3354          * and removing from the mux array
3355          */
3356         if (gsm->dead) {
3357                 ret = gsm_activate_mux(gsm);
3358                 if (ret)
3359                         return ret;
3360                 if (gsm->initiator)
3361                         gsm_dlci_begin_open(gsm->dlci[0]);
3362         }
3363         return 0;
3364 }
3365
3366 static void gsm_copy_config_ext_values(struct gsm_mux *gsm,
3367                                        struct gsm_config_ext *ce)
3368 {
3369         memset(ce, 0, sizeof(*ce));
3370         ce->wait_config = gsm->wait_config ? 1 : 0;
3371         ce->keep_alive = gsm->keep_alive;
3372 }
3373
3374 static int gsm_config_ext(struct gsm_mux *gsm, struct gsm_config_ext *ce)
3375 {
3376         unsigned int i;
3377
3378         /*
3379          * Check that userspace doesn't put stuff in here to prevent breakages
3380          * in the future.
3381          */
3382         for (i = 0; i < ARRAY_SIZE(ce->reserved); i++)
3383                 if (ce->reserved[i])
3384                         return -EINVAL;
3385
3386         /*
3387          * Setup the new configuration values
3388          */
3389         gsm->wait_config = ce->wait_config ? true : false;
3390         gsm->keep_alive = ce->keep_alive;
3391
3392         return 0;
3393 }
3394
3395 /**
3396  *      gsmld_output            -       write to link
3397  *      @gsm: our mux
3398  *      @data: bytes to output
3399  *      @len: size
3400  *
3401  *      Write a block of data from the GSM mux to the data channel. This
3402  *      will eventually be serialized from above but at the moment isn't.
3403  */
3404
3405 static int gsmld_output(struct gsm_mux *gsm, u8 *data, int len)
3406 {
3407         if (tty_write_room(gsm->tty) < len) {
3408                 set_bit(TTY_DO_WRITE_WAKEUP, &gsm->tty->flags);
3409                 return -ENOSPC;
3410         }
3411         if (debug & DBG_DATA)
3412                 gsm_hex_dump_bytes(__func__, data, len);
3413         return gsm->tty->ops->write(gsm->tty, data, len);
3414 }
3415
3416
3417 /**
3418  *      gsmld_write_trigger     -       schedule ldisc write task
3419  *      @gsm: our mux
3420  */
3421 static void gsmld_write_trigger(struct gsm_mux *gsm)
3422 {
3423         if (!gsm || !gsm->dlci[0] || gsm->dlci[0]->dead)
3424                 return;
3425         schedule_work(&gsm->tx_work);
3426 }
3427
3428
3429 /**
3430  *      gsmld_write_task        -       ldisc write task
3431  *      @work: our tx write work
3432  *
3433  *      Writes out data to the ldisc if possible. We are doing this here to
3434  *      avoid dead-locking. This returns if no space or data is left for output.
3435  */
3436 static void gsmld_write_task(struct work_struct *work)
3437 {
3438         struct gsm_mux *gsm = container_of(work, struct gsm_mux, tx_work);
3439         unsigned long flags;
3440         int i, ret;
3441
3442         /* All outstanding control channel and control messages and one data
3443          * frame is sent.
3444          */
3445         ret = -ENODEV;
3446         spin_lock_irqsave(&gsm->tx_lock, flags);
3447         if (gsm->tty)
3448                 ret = gsm_data_kick(gsm);
3449         spin_unlock_irqrestore(&gsm->tx_lock, flags);
3450
3451         if (ret >= 0)
3452                 for (i = 0; i < NUM_DLCI; i++)
3453                         if (gsm->dlci[i])
3454                                 tty_port_tty_wakeup(&gsm->dlci[i]->port);
3455 }
3456
3457 /**
3458  *      gsmld_attach_gsm        -       mode set up
3459  *      @tty: our tty structure
3460  *      @gsm: our mux
3461  *
3462  *      Set up the MUX for basic mode and commence connecting to the
3463  *      modem. Currently called from the line discipline set up but
3464  *      will need moving to an ioctl path.
3465  */
3466
3467 static void gsmld_attach_gsm(struct tty_struct *tty, struct gsm_mux *gsm)
3468 {
3469         gsm->tty = tty_kref_get(tty);
3470         /* Turn off tty XON/XOFF handling to handle it explicitly. */
3471         gsm->old_c_iflag = tty->termios.c_iflag;
3472         tty->termios.c_iflag &= (IXON | IXOFF);
3473 }
3474
3475 /**
3476  *      gsmld_detach_gsm        -       stop doing 0710 mux
3477  *      @tty: tty attached to the mux
3478  *      @gsm: mux
3479  *
3480  *      Shutdown and then clean up the resources used by the line discipline
3481  */
3482
3483 static void gsmld_detach_gsm(struct tty_struct *tty, struct gsm_mux *gsm)
3484 {
3485         WARN_ON(tty != gsm->tty);
3486         /* Restore tty XON/XOFF handling. */
3487         gsm->tty->termios.c_iflag = gsm->old_c_iflag;
3488         tty_kref_put(gsm->tty);
3489         gsm->tty = NULL;
3490 }
3491
3492 static void gsmld_receive_buf(struct tty_struct *tty, const u8 *cp,
3493                               const u8 *fp, size_t count)
3494 {
3495         struct gsm_mux *gsm = tty->disc_data;
3496         char flags = TTY_NORMAL;
3497
3498         if (debug & DBG_DATA)
3499                 gsm_hex_dump_bytes(__func__, cp, count);
3500
3501         for (; count; count--, cp++) {
3502                 if (fp)
3503                         flags = *fp++;
3504                 switch (flags) {
3505                 case TTY_NORMAL:
3506                         if (gsm->receive)
3507                                 gsm->receive(gsm, *cp);
3508                         break;
3509                 case TTY_OVERRUN:
3510                 case TTY_BREAK:
3511                 case TTY_PARITY:
3512                 case TTY_FRAME:
3513                         gsm_error(gsm);
3514                         break;
3515                 default:
3516                         WARN_ONCE(1, "%s: unknown flag %d\n",
3517                                tty_name(tty), flags);
3518                         break;
3519                 }
3520         }
3521         /* FASYNC if needed ? */
3522         /* If clogged call tty_throttle(tty); */
3523 }
3524
3525 /**
3526  *      gsmld_flush_buffer      -       clean input queue
3527  *      @tty:   terminal device
3528  *
3529  *      Flush the input buffer. Called when the line discipline is
3530  *      being closed, when the tty layer wants the buffer flushed (eg
3531  *      at hangup).
3532  */
3533
3534 static void gsmld_flush_buffer(struct tty_struct *tty)
3535 {
3536 }
3537
3538 /**
3539  *      gsmld_close             -       close the ldisc for this tty
3540  *      @tty: device
3541  *
3542  *      Called from the terminal layer when this line discipline is
3543  *      being shut down, either because of a close or becsuse of a
3544  *      discipline change. The function will not be called while other
3545  *      ldisc methods are in progress.
3546  */
3547
3548 static void gsmld_close(struct tty_struct *tty)
3549 {
3550         struct gsm_mux *gsm = tty->disc_data;
3551
3552         /* The ldisc locks and closes the port before calling our close. This
3553          * means we have no way to do a proper disconnect. We will not bother
3554          * to do one.
3555          */
3556         gsm_cleanup_mux(gsm, false);
3557
3558         gsmld_detach_gsm(tty, gsm);
3559
3560         gsmld_flush_buffer(tty);
3561         /* Do other clean up here */
3562         mux_put(gsm);
3563 }
3564
3565 /**
3566  *      gsmld_open              -       open an ldisc
3567  *      @tty: terminal to open
3568  *
3569  *      Called when this line discipline is being attached to the
3570  *      terminal device. Can sleep. Called serialized so that no
3571  *      other events will occur in parallel. No further open will occur
3572  *      until a close.
3573  */
3574
3575 static int gsmld_open(struct tty_struct *tty)
3576 {
3577         struct gsm_mux *gsm;
3578
3579         if (!capable(CAP_NET_ADMIN))
3580                 return -EPERM;
3581
3582         if (tty->ops->write == NULL)
3583                 return -EINVAL;
3584
3585         /* Attach our ldisc data */
3586         gsm = gsm_alloc_mux();
3587         if (gsm == NULL)
3588                 return -ENOMEM;
3589
3590         tty->disc_data = gsm;
3591         tty->receive_room = 65536;
3592
3593         /* Attach the initial passive connection */
3594         gsmld_attach_gsm(tty, gsm);
3595
3596         /* The mux will not be activated yet, we wait for correct
3597          * configuration first.
3598          */
3599         if (gsm->encoding == GSM_BASIC_OPT)
3600                 gsm->receive = gsm0_receive;
3601         else
3602                 gsm->receive = gsm1_receive;
3603
3604         return 0;
3605 }
3606
3607 /**
3608  *      gsmld_write_wakeup      -       asynchronous I/O notifier
3609  *      @tty: tty device
3610  *
3611  *      Required for the ptys, serial driver etc. since processes
3612  *      that attach themselves to the master and rely on ASYNC
3613  *      IO must be woken up
3614  */
3615
3616 static void gsmld_write_wakeup(struct tty_struct *tty)
3617 {
3618         struct gsm_mux *gsm = tty->disc_data;
3619
3620         /* Queue poll */
3621         gsmld_write_trigger(gsm);
3622 }
3623
3624 /**
3625  *      gsmld_read              -       read function for tty
3626  *      @tty: tty device
3627  *      @file: file object
3628  *      @buf: userspace buffer pointer
3629  *      @nr: size of I/O
3630  *      @cookie: unused
3631  *      @offset: unused
3632  *
3633  *      Perform reads for the line discipline. We are guaranteed that the
3634  *      line discipline will not be closed under us but we may get multiple
3635  *      parallel readers and must handle this ourselves. We may also get
3636  *      a hangup. Always called in user context, may sleep.
3637  *
3638  *      This code must be sure never to sleep through a hangup.
3639  */
3640
3641 static ssize_t gsmld_read(struct tty_struct *tty, struct file *file, u8 *buf,
3642                           size_t nr, void **cookie, unsigned long offset)
3643 {
3644         return -EOPNOTSUPP;
3645 }
3646
3647 /**
3648  *      gsmld_write             -       write function for tty
3649  *      @tty: tty device
3650  *      @file: file object
3651  *      @buf: userspace buffer pointer
3652  *      @nr: size of I/O
3653  *
3654  *      Called when the owner of the device wants to send a frame
3655  *      itself (or some other control data). The data is transferred
3656  *      as-is and must be properly framed and checksummed as appropriate
3657  *      by userspace. Frames are either sent whole or not at all as this
3658  *      avoids pain user side.
3659  */
3660
3661 static ssize_t gsmld_write(struct tty_struct *tty, struct file *file,
3662                            const u8 *buf, size_t nr)
3663 {
3664         struct gsm_mux *gsm = tty->disc_data;
3665         unsigned long flags;
3666         int space;
3667         int ret;
3668
3669         if (!gsm)
3670                 return -ENODEV;
3671
3672         ret = -ENOBUFS;
3673         spin_lock_irqsave(&gsm->tx_lock, flags);
3674         space = tty_write_room(tty);
3675         if (space >= nr)
3676                 ret = tty->ops->write(tty, buf, nr);
3677         else
3678                 set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
3679         spin_unlock_irqrestore(&gsm->tx_lock, flags);
3680
3681         return ret;
3682 }
3683
3684 /**
3685  *      gsmld_poll              -       poll method for N_GSM0710
3686  *      @tty: terminal device
3687  *      @file: file accessing it
3688  *      @wait: poll table
3689  *
3690  *      Called when the line discipline is asked to poll() for data or
3691  *      for special events. This code is not serialized with respect to
3692  *      other events save open/close.
3693  *
3694  *      This code must be sure never to sleep through a hangup.
3695  *      Called without the kernel lock held - fine
3696  */
3697
3698 static __poll_t gsmld_poll(struct tty_struct *tty, struct file *file,
3699                                                         poll_table *wait)
3700 {
3701         __poll_t mask = 0;
3702         struct gsm_mux *gsm = tty->disc_data;
3703
3704         poll_wait(file, &tty->read_wait, wait);
3705         poll_wait(file, &tty->write_wait, wait);
3706
3707         if (gsm->dead)
3708                 mask |= EPOLLHUP;
3709         if (tty_hung_up_p(file))
3710                 mask |= EPOLLHUP;
3711         if (test_bit(TTY_OTHER_CLOSED, &tty->flags))
3712                 mask |= EPOLLHUP;
3713         if (!tty_is_writelocked(tty) && tty_write_room(tty) > 0)
3714                 mask |= EPOLLOUT | EPOLLWRNORM;
3715         return mask;
3716 }
3717
3718 static int gsmld_ioctl(struct tty_struct *tty, unsigned int cmd,
3719                        unsigned long arg)
3720 {
3721         struct gsm_config c;
3722         struct gsm_config_ext ce;
3723         struct gsm_dlci_config dc;
3724         struct gsm_mux *gsm = tty->disc_data;
3725         unsigned int base, addr;
3726         struct gsm_dlci *dlci;
3727
3728         switch (cmd) {
3729         case GSMIOC_GETCONF:
3730                 gsm_copy_config_values(gsm, &c);
3731                 if (copy_to_user((void __user *)arg, &c, sizeof(c)))
3732                         return -EFAULT;
3733                 return 0;
3734         case GSMIOC_SETCONF:
3735                 if (copy_from_user(&c, (void __user *)arg, sizeof(c)))
3736                         return -EFAULT;
3737                 return gsm_config(gsm, &c);
3738         case GSMIOC_GETFIRST:
3739                 base = mux_num_to_base(gsm);
3740                 return put_user(base + 1, (__u32 __user *)arg);
3741         case GSMIOC_GETCONF_EXT:
3742                 gsm_copy_config_ext_values(gsm, &ce);
3743                 if (copy_to_user((void __user *)arg, &ce, sizeof(ce)))
3744                         return -EFAULT;
3745                 return 0;
3746         case GSMIOC_SETCONF_EXT:
3747                 if (copy_from_user(&ce, (void __user *)arg, sizeof(ce)))
3748                         return -EFAULT;
3749                 return gsm_config_ext(gsm, &ce);
3750         case GSMIOC_GETCONF_DLCI:
3751                 if (copy_from_user(&dc, (void __user *)arg, sizeof(dc)))
3752                         return -EFAULT;
3753                 if (dc.channel == 0 || dc.channel >= NUM_DLCI)
3754                         return -EINVAL;
3755                 addr = array_index_nospec(dc.channel, NUM_DLCI);
3756                 dlci = gsm->dlci[addr];
3757                 if (!dlci) {
3758                         dlci = gsm_dlci_alloc(gsm, addr);
3759                         if (!dlci)
3760                                 return -ENOMEM;
3761                 }
3762                 gsm_dlci_copy_config_values(dlci, &dc);
3763                 if (copy_to_user((void __user *)arg, &dc, sizeof(dc)))
3764                         return -EFAULT;
3765                 return 0;
3766         case GSMIOC_SETCONF_DLCI:
3767                 if (copy_from_user(&dc, (void __user *)arg, sizeof(dc)))
3768                         return -EFAULT;
3769                 if (dc.channel == 0 || dc.channel >= NUM_DLCI)
3770                         return -EINVAL;
3771                 addr = array_index_nospec(dc.channel, NUM_DLCI);
3772                 dlci = gsm->dlci[addr];
3773                 if (!dlci) {
3774                         dlci = gsm_dlci_alloc(gsm, addr);
3775                         if (!dlci)
3776                                 return -ENOMEM;
3777                 }
3778                 return gsm_dlci_config(dlci, &dc, 0);
3779         default:
3780                 return n_tty_ioctl_helper(tty, cmd, arg);
3781         }
3782 }
3783
3784 /*
3785  *      Network interface
3786  *
3787  */
3788
3789 static int gsm_mux_net_open(struct net_device *net)
3790 {
3791         pr_debug("%s called\n", __func__);
3792         netif_start_queue(net);
3793         return 0;
3794 }
3795
3796 static int gsm_mux_net_close(struct net_device *net)
3797 {
3798         netif_stop_queue(net);
3799         return 0;
3800 }
3801
3802 static void dlci_net_free(struct gsm_dlci *dlci)
3803 {
3804         if (!dlci->net) {
3805                 WARN_ON(1);
3806                 return;
3807         }
3808         dlci->adaption = dlci->prev_adaption;
3809         dlci->data = dlci->prev_data;
3810         free_netdev(dlci->net);
3811         dlci->net = NULL;
3812 }
3813 static void net_free(struct kref *ref)
3814 {
3815         struct gsm_mux_net *mux_net;
3816         struct gsm_dlci *dlci;
3817
3818         mux_net = container_of(ref, struct gsm_mux_net, ref);
3819         dlci = mux_net->dlci;
3820
3821         if (dlci->net) {
3822                 unregister_netdev(dlci->net);
3823                 dlci_net_free(dlci);
3824         }
3825 }
3826
3827 static inline void muxnet_get(struct gsm_mux_net *mux_net)
3828 {
3829         kref_get(&mux_net->ref);
3830 }
3831
3832 static inline void muxnet_put(struct gsm_mux_net *mux_net)
3833 {
3834         kref_put(&mux_net->ref, net_free);
3835 }
3836
3837 static netdev_tx_t gsm_mux_net_start_xmit(struct sk_buff *skb,
3838                                       struct net_device *net)
3839 {
3840         struct gsm_mux_net *mux_net = netdev_priv(net);
3841         struct gsm_dlci *dlci = mux_net->dlci;
3842         muxnet_get(mux_net);
3843
3844         skb_queue_head(&dlci->skb_list, skb);
3845         net->stats.tx_packets++;
3846         net->stats.tx_bytes += skb->len;
3847         gsm_dlci_data_kick(dlci);
3848         /* And tell the kernel when the last transmit started. */
3849         netif_trans_update(net);
3850         muxnet_put(mux_net);
3851         return NETDEV_TX_OK;
3852 }
3853
3854 /* called when a packet did not ack after watchdogtimeout */
3855 static void gsm_mux_net_tx_timeout(struct net_device *net, unsigned int txqueue)
3856 {
3857         /* Tell syslog we are hosed. */
3858         dev_dbg(&net->dev, "Tx timed out.\n");
3859
3860         /* Update statistics */
3861         net->stats.tx_errors++;
3862 }
3863
3864 static void gsm_mux_rx_netchar(struct gsm_dlci *dlci,
3865                                 const unsigned char *in_buf, int size)
3866 {
3867         struct net_device *net = dlci->net;
3868         struct sk_buff *skb;
3869         struct gsm_mux_net *mux_net = netdev_priv(net);
3870         muxnet_get(mux_net);
3871
3872         /* Allocate an sk_buff */
3873         skb = dev_alloc_skb(size + NET_IP_ALIGN);
3874         if (!skb) {
3875                 /* We got no receive buffer. */
3876                 net->stats.rx_dropped++;
3877                 muxnet_put(mux_net);
3878                 return;
3879         }
3880         skb_reserve(skb, NET_IP_ALIGN);
3881         skb_put_data(skb, in_buf, size);
3882
3883         skb->dev = net;
3884         skb->protocol = htons(ETH_P_IP);
3885
3886         /* Ship it off to the kernel */
3887         netif_rx(skb);
3888
3889         /* update out statistics */
3890         net->stats.rx_packets++;
3891         net->stats.rx_bytes += size;
3892         muxnet_put(mux_net);
3893         return;
3894 }
3895
3896 static void gsm_mux_net_init(struct net_device *net)
3897 {
3898         static const struct net_device_ops gsm_netdev_ops = {
3899                 .ndo_open               = gsm_mux_net_open,
3900                 .ndo_stop               = gsm_mux_net_close,
3901                 .ndo_start_xmit         = gsm_mux_net_start_xmit,
3902                 .ndo_tx_timeout         = gsm_mux_net_tx_timeout,
3903         };
3904
3905         net->netdev_ops = &gsm_netdev_ops;
3906
3907         /* fill in the other fields */
3908         net->watchdog_timeo = GSM_NET_TX_TIMEOUT;
3909         net->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
3910         net->type = ARPHRD_NONE;
3911         net->tx_queue_len = 10;
3912 }
3913
3914
3915 /* caller holds the dlci mutex */
3916 static void gsm_destroy_network(struct gsm_dlci *dlci)
3917 {
3918         struct gsm_mux_net *mux_net;
3919
3920         pr_debug("destroy network interface\n");
3921         if (!dlci->net)
3922                 return;
3923         mux_net = netdev_priv(dlci->net);
3924         muxnet_put(mux_net);
3925 }
3926
3927
3928 /* caller holds the dlci mutex */
3929 static int gsm_create_network(struct gsm_dlci *dlci, struct gsm_netconfig *nc)
3930 {
3931         char *netname;
3932         int retval = 0;
3933         struct net_device *net;
3934         struct gsm_mux_net *mux_net;
3935
3936         if (!capable(CAP_NET_ADMIN))
3937                 return -EPERM;
3938
3939         /* Already in a non tty mode */
3940         if (dlci->adaption > 2)
3941                 return -EBUSY;
3942
3943         if (nc->protocol != htons(ETH_P_IP))
3944                 return -EPROTONOSUPPORT;
3945
3946         if (nc->adaption != 3 && nc->adaption != 4)
3947                 return -EPROTONOSUPPORT;
3948
3949         pr_debug("create network interface\n");
3950
3951         netname = "gsm%d";
3952         if (nc->if_name[0] != '\0')
3953                 netname = nc->if_name;
3954         net = alloc_netdev(sizeof(struct gsm_mux_net), netname,
3955                            NET_NAME_UNKNOWN, gsm_mux_net_init);
3956         if (!net) {
3957                 pr_err("alloc_netdev failed\n");
3958                 return -ENOMEM;
3959         }
3960         net->mtu = dlci->mtu;
3961         net->min_mtu = MIN_MTU;
3962         net->max_mtu = dlci->mtu;
3963         mux_net = netdev_priv(net);
3964         mux_net->dlci = dlci;
3965         kref_init(&mux_net->ref);
3966         strncpy(nc->if_name, net->name, IFNAMSIZ); /* return net name */
3967
3968         /* reconfigure dlci for network */
3969         dlci->prev_adaption = dlci->adaption;
3970         dlci->prev_data = dlci->data;
3971         dlci->adaption = nc->adaption;
3972         dlci->data = gsm_mux_rx_netchar;
3973         dlci->net = net;
3974
3975         pr_debug("register netdev\n");
3976         retval = register_netdev(net);
3977         if (retval) {
3978                 pr_err("network register fail %d\n", retval);
3979                 dlci_net_free(dlci);
3980                 return retval;
3981         }
3982         return net->ifindex;    /* return network index */
3983 }
3984
3985 /* Line discipline for real tty */
3986 static struct tty_ldisc_ops tty_ldisc_packet = {
3987         .owner           = THIS_MODULE,
3988         .num             = N_GSM0710,
3989         .name            = "n_gsm",
3990         .open            = gsmld_open,
3991         .close           = gsmld_close,
3992         .flush_buffer    = gsmld_flush_buffer,
3993         .read            = gsmld_read,
3994         .write           = gsmld_write,
3995         .ioctl           = gsmld_ioctl,
3996         .poll            = gsmld_poll,
3997         .receive_buf     = gsmld_receive_buf,
3998         .write_wakeup    = gsmld_write_wakeup
3999 };
4000
4001 /*
4002  *      Virtual tty side
4003  */
4004
4005 /**
4006  *      gsm_modem_upd_via_data  -       send modem bits via convergence layer
4007  *      @dlci: channel
4008  *      @brk: break signal
4009  *
4010  *      Send an empty frame to signal mobile state changes and to transmit the
4011  *      break signal for adaption 2.
4012  */
4013
4014 static void gsm_modem_upd_via_data(struct gsm_dlci *dlci, u8 brk)
4015 {
4016         struct gsm_mux *gsm = dlci->gsm;
4017         unsigned long flags;
4018
4019         if (dlci->state != DLCI_OPEN || dlci->adaption != 2)
4020                 return;
4021
4022         spin_lock_irqsave(&gsm->tx_lock, flags);
4023         gsm_dlci_modem_output(gsm, dlci, brk);
4024         spin_unlock_irqrestore(&gsm->tx_lock, flags);
4025 }
4026
4027 /**
4028  *      gsm_modem_upd_via_msc   -       send modem bits via control frame
4029  *      @dlci: channel
4030  *      @brk: break signal
4031  */
4032
4033 static int gsm_modem_upd_via_msc(struct gsm_dlci *dlci, u8 brk)
4034 {
4035         u8 modembits[3];
4036         struct gsm_control *ctrl;
4037         int len = 2;
4038
4039         if (dlci->gsm->encoding != GSM_BASIC_OPT)
4040                 return 0;
4041
4042         modembits[0] = (dlci->addr << 2) | 2 | EA;  /* DLCI, Valid, EA */
4043         if (!brk) {
4044                 modembits[1] = (gsm_encode_modem(dlci) << 1) | EA;
4045         } else {
4046                 modembits[1] = gsm_encode_modem(dlci) << 1;
4047                 modembits[2] = (brk << 4) | 2 | EA; /* Length, Break, EA */
4048                 len++;
4049         }
4050         ctrl = gsm_control_send(dlci->gsm, CMD_MSC, modembits, len);
4051         if (ctrl == NULL)
4052                 return -ENOMEM;
4053         return gsm_control_wait(dlci->gsm, ctrl);
4054 }
4055
4056 /**
4057  *      gsm_modem_update        -       send modem status line state
4058  *      @dlci: channel
4059  *      @brk: break signal
4060  */
4061
4062 static int gsm_modem_update(struct gsm_dlci *dlci, u8 brk)
4063 {
4064         if (dlci->adaption == 2) {
4065                 /* Send convergence layer type 2 empty data frame. */
4066                 gsm_modem_upd_via_data(dlci, brk);
4067                 return 0;
4068         } else if (dlci->gsm->encoding == GSM_BASIC_OPT) {
4069                 /* Send as MSC control message. */
4070                 return gsm_modem_upd_via_msc(dlci, brk);
4071         }
4072
4073         /* Modem status lines are not supported. */
4074         return -EPROTONOSUPPORT;
4075 }
4076
4077 /**
4078  * gsm_wait_modem_change - wait for modem status line change
4079  * @dlci: channel
4080  * @mask: modem status line bits
4081  *
4082  * The function returns if:
4083  * - any given modem status line bit changed
4084  * - the wait event function got interrupted (e.g. by a signal)
4085  * - the underlying DLCI was closed
4086  * - the underlying ldisc device was removed
4087  */
4088 static int gsm_wait_modem_change(struct gsm_dlci *dlci, u32 mask)
4089 {
4090         struct gsm_mux *gsm = dlci->gsm;
4091         u32 old = dlci->modem_rx;
4092         int ret;
4093
4094         ret = wait_event_interruptible(gsm->event, gsm->dead ||
4095                                        dlci->state != DLCI_OPEN ||
4096                                        (old ^ dlci->modem_rx) & mask);
4097         if (gsm->dead)
4098                 return -ENODEV;
4099         if (dlci->state != DLCI_OPEN)
4100                 return -EL2NSYNC;
4101         return ret;
4102 }
4103
4104 static bool gsm_carrier_raised(struct tty_port *port)
4105 {
4106         struct gsm_dlci *dlci = container_of(port, struct gsm_dlci, port);
4107         struct gsm_mux *gsm = dlci->gsm;
4108
4109         /* Not yet open so no carrier info */
4110         if (dlci->state != DLCI_OPEN)
4111                 return false;
4112         if (debug & DBG_CD_ON)
4113                 return true;
4114
4115         /*
4116          * Basic mode with control channel in ADM mode may not respond
4117          * to CMD_MSC at all and modem_rx is empty.
4118          */
4119         if (gsm->encoding == GSM_BASIC_OPT &&
4120             gsm->dlci[0]->mode == DLCI_MODE_ADM && !dlci->modem_rx)
4121                 return true;
4122
4123         return dlci->modem_rx & TIOCM_CD;
4124 }
4125
4126 static void gsm_dtr_rts(struct tty_port *port, bool active)
4127 {
4128         struct gsm_dlci *dlci = container_of(port, struct gsm_dlci, port);
4129         unsigned int modem_tx = dlci->modem_tx;
4130         if (active)
4131                 modem_tx |= TIOCM_DTR | TIOCM_RTS;
4132         else
4133                 modem_tx &= ~(TIOCM_DTR | TIOCM_RTS);
4134         if (modem_tx != dlci->modem_tx) {
4135                 dlci->modem_tx = modem_tx;
4136                 gsm_modem_update(dlci, 0);
4137         }
4138 }
4139
4140 static const struct tty_port_operations gsm_port_ops = {
4141         .carrier_raised = gsm_carrier_raised,
4142         .dtr_rts = gsm_dtr_rts,
4143         .destruct = gsm_dlci_free,
4144 };
4145
4146 static int gsmtty_install(struct tty_driver *driver, struct tty_struct *tty)
4147 {
4148         struct gsm_mux *gsm;
4149         struct gsm_dlci *dlci;
4150         unsigned int line = tty->index;
4151         unsigned int mux = mux_line_to_num(line);
4152         bool alloc = false;
4153         int ret;
4154
4155         line = line & 0x3F;
4156
4157         if (mux >= MAX_MUX)
4158                 return -ENXIO;
4159         /* FIXME: we need to lock gsm_mux for lifetimes of ttys eventually */
4160         if (gsm_mux[mux] == NULL)
4161                 return -EUNATCH;
4162         if (line == 0 || line > 61)     /* 62/63 reserved */
4163                 return -ECHRNG;
4164         gsm = gsm_mux[mux];
4165         if (gsm->dead)
4166                 return -EL2HLT;
4167         /* If DLCI 0 is not yet fully open return an error.
4168         This is ok from a locking
4169         perspective as we don't have to worry about this
4170         if DLCI0 is lost */
4171         mutex_lock(&gsm->mutex);
4172         if (gsm->dlci[0] && gsm->dlci[0]->state != DLCI_OPEN) {
4173                 mutex_unlock(&gsm->mutex);
4174                 return -EL2NSYNC;
4175         }
4176         dlci = gsm->dlci[line];
4177         if (dlci == NULL) {
4178                 alloc = true;
4179                 dlci = gsm_dlci_alloc(gsm, line);
4180         }
4181         if (dlci == NULL) {
4182                 mutex_unlock(&gsm->mutex);
4183                 return -ENOMEM;
4184         }
4185         ret = tty_port_install(&dlci->port, driver, tty);
4186         if (ret) {
4187                 if (alloc)
4188                         dlci_put(dlci);
4189                 mutex_unlock(&gsm->mutex);
4190                 return ret;
4191         }
4192
4193         dlci_get(dlci);
4194         dlci_get(gsm->dlci[0]);
4195         mux_get(gsm);
4196         tty->driver_data = dlci;
4197         mutex_unlock(&gsm->mutex);
4198
4199         return 0;
4200 }
4201
4202 static int gsmtty_open(struct tty_struct *tty, struct file *filp)
4203 {
4204         struct gsm_dlci *dlci = tty->driver_data;
4205         struct tty_port *port = &dlci->port;
4206
4207         port->count++;
4208         tty_port_tty_set(port, tty);
4209
4210         dlci->modem_rx = 0;
4211         /* We could in theory open and close before we wait - eg if we get
4212            a DM straight back. This is ok as that will have caused a hangup */
4213         tty_port_set_initialized(port, true);
4214         /* Start sending off SABM messages */
4215         if (!dlci->gsm->wait_config) {
4216                 /* Start sending off SABM messages */
4217                 if (dlci->gsm->initiator)
4218                         gsm_dlci_begin_open(dlci);
4219                 else
4220                         gsm_dlci_set_opening(dlci);
4221         } else {
4222                 gsm_dlci_set_wait_config(dlci);
4223         }
4224         /* And wait for virtual carrier */
4225         return tty_port_block_til_ready(port, tty, filp);
4226 }
4227
4228 static void gsmtty_close(struct tty_struct *tty, struct file *filp)
4229 {
4230         struct gsm_dlci *dlci = tty->driver_data;
4231
4232         if (dlci == NULL)
4233                 return;
4234         if (dlci->state == DLCI_CLOSED)
4235                 return;
4236         mutex_lock(&dlci->mutex);
4237         gsm_destroy_network(dlci);
4238         mutex_unlock(&dlci->mutex);
4239         if (tty_port_close_start(&dlci->port, tty, filp) == 0)
4240                 return;
4241         gsm_dlci_begin_close(dlci);
4242         if (tty_port_initialized(&dlci->port) && C_HUPCL(tty))
4243                 tty_port_lower_dtr_rts(&dlci->port);
4244         tty_port_close_end(&dlci->port, tty);
4245         tty_port_tty_set(&dlci->port, NULL);
4246         return;
4247 }
4248
4249 static void gsmtty_hangup(struct tty_struct *tty)
4250 {
4251         struct gsm_dlci *dlci = tty->driver_data;
4252         if (dlci->state == DLCI_CLOSED)
4253                 return;
4254         tty_port_hangup(&dlci->port);
4255         gsm_dlci_begin_close(dlci);
4256 }
4257
4258 static ssize_t gsmtty_write(struct tty_struct *tty, const u8 *buf, size_t len)
4259 {
4260         int sent;
4261         struct gsm_dlci *dlci = tty->driver_data;
4262         if (dlci->state == DLCI_CLOSED)
4263                 return -EINVAL;
4264         /* Stuff the bytes into the fifo queue */
4265         sent = kfifo_in_locked(&dlci->fifo, buf, len, &dlci->lock);
4266         /* Need to kick the channel */
4267         gsm_dlci_data_kick(dlci);
4268         return sent;
4269 }
4270
4271 static unsigned int gsmtty_write_room(struct tty_struct *tty)
4272 {
4273         struct gsm_dlci *dlci = tty->driver_data;
4274         if (dlci->state == DLCI_CLOSED)
4275                 return 0;
4276         return kfifo_avail(&dlci->fifo);
4277 }
4278
4279 static unsigned int gsmtty_chars_in_buffer(struct tty_struct *tty)
4280 {
4281         struct gsm_dlci *dlci = tty->driver_data;
4282         if (dlci->state == DLCI_CLOSED)
4283                 return 0;
4284         return kfifo_len(&dlci->fifo);
4285 }
4286
4287 static void gsmtty_flush_buffer(struct tty_struct *tty)
4288 {
4289         struct gsm_dlci *dlci = tty->driver_data;
4290         unsigned long flags;
4291
4292         if (dlci->state == DLCI_CLOSED)
4293                 return;
4294         /* Caution needed: If we implement reliable transport classes
4295            then the data being transmitted can't simply be junked once
4296            it has first hit the stack. Until then we can just blow it
4297            away */
4298         spin_lock_irqsave(&dlci->lock, flags);
4299         kfifo_reset(&dlci->fifo);
4300         spin_unlock_irqrestore(&dlci->lock, flags);
4301         /* Need to unhook this DLCI from the transmit queue logic */
4302 }
4303
4304 static void gsmtty_wait_until_sent(struct tty_struct *tty, int timeout)
4305 {
4306         /* The FIFO handles the queue so the kernel will do the right
4307            thing waiting on chars_in_buffer before calling us. No work
4308            to do here */
4309 }
4310
4311 static int gsmtty_tiocmget(struct tty_struct *tty)
4312 {
4313         struct gsm_dlci *dlci = tty->driver_data;
4314         if (dlci->state == DLCI_CLOSED)
4315                 return -EINVAL;
4316         return dlci->modem_rx;
4317 }
4318
4319 static int gsmtty_tiocmset(struct tty_struct *tty,
4320         unsigned int set, unsigned int clear)
4321 {
4322         struct gsm_dlci *dlci = tty->driver_data;
4323         unsigned int modem_tx = dlci->modem_tx;
4324
4325         if (dlci->state == DLCI_CLOSED)
4326                 return -EINVAL;
4327         modem_tx &= ~clear;
4328         modem_tx |= set;
4329
4330         if (modem_tx != dlci->modem_tx) {
4331                 dlci->modem_tx = modem_tx;
4332                 return gsm_modem_update(dlci, 0);
4333         }
4334         return 0;
4335 }
4336
4337
4338 static int gsmtty_ioctl(struct tty_struct *tty,
4339                         unsigned int cmd, unsigned long arg)
4340 {
4341         struct gsm_dlci *dlci = tty->driver_data;
4342         struct gsm_netconfig nc;
4343         struct gsm_dlci_config dc;
4344         int index;
4345
4346         if (dlci->state == DLCI_CLOSED)
4347                 return -EINVAL;
4348         switch (cmd) {
4349         case GSMIOC_ENABLE_NET:
4350                 if (copy_from_user(&nc, (void __user *)arg, sizeof(nc)))
4351                         return -EFAULT;
4352                 nc.if_name[IFNAMSIZ-1] = '\0';
4353                 /* return net interface index or error code */
4354                 mutex_lock(&dlci->mutex);
4355                 index = gsm_create_network(dlci, &nc);
4356                 mutex_unlock(&dlci->mutex);
4357                 if (copy_to_user((void __user *)arg, &nc, sizeof(nc)))
4358                         return -EFAULT;
4359                 return index;
4360         case GSMIOC_DISABLE_NET:
4361                 if (!capable(CAP_NET_ADMIN))
4362                         return -EPERM;
4363                 mutex_lock(&dlci->mutex);
4364                 gsm_destroy_network(dlci);
4365                 mutex_unlock(&dlci->mutex);
4366                 return 0;
4367         case GSMIOC_GETCONF_DLCI:
4368                 if (copy_from_user(&dc, (void __user *)arg, sizeof(dc)))
4369                         return -EFAULT;
4370                 if (dc.channel != dlci->addr)
4371                         return -EPERM;
4372                 gsm_dlci_copy_config_values(dlci, &dc);
4373                 if (copy_to_user((void __user *)arg, &dc, sizeof(dc)))
4374                         return -EFAULT;
4375                 return 0;
4376         case GSMIOC_SETCONF_DLCI:
4377                 if (copy_from_user(&dc, (void __user *)arg, sizeof(dc)))
4378                         return -EFAULT;
4379                 if (dc.channel >= NUM_DLCI)
4380                         return -EINVAL;
4381                 if (dc.channel != 0 && dc.channel != dlci->addr)
4382                         return -EPERM;
4383                 return gsm_dlci_config(dlci, &dc, 1);
4384         case TIOCMIWAIT:
4385                 return gsm_wait_modem_change(dlci, (u32)arg);
4386         default:
4387                 return -ENOIOCTLCMD;
4388         }
4389 }
4390
4391 static void gsmtty_set_termios(struct tty_struct *tty,
4392                                const struct ktermios *old)
4393 {
4394         struct gsm_dlci *dlci = tty->driver_data;
4395         if (dlci->state == DLCI_CLOSED)
4396                 return;
4397         /* For the moment its fixed. In actual fact the speed information
4398            for the virtual channel can be propogated in both directions by
4399            the RPN control message. This however rapidly gets nasty as we
4400            then have to remap modem signals each way according to whether
4401            our virtual cable is null modem etc .. */
4402         tty_termios_copy_hw(&tty->termios, old);
4403 }
4404
4405 static void gsmtty_throttle(struct tty_struct *tty)
4406 {
4407         struct gsm_dlci *dlci = tty->driver_data;
4408         if (dlci->state == DLCI_CLOSED)
4409                 return;
4410         if (C_CRTSCTS(tty))
4411                 dlci->modem_tx &= ~TIOCM_RTS;
4412         dlci->throttled = true;
4413         /* Send an MSC with RTS cleared */
4414         gsm_modem_update(dlci, 0);
4415 }
4416
4417 static void gsmtty_unthrottle(struct tty_struct *tty)
4418 {
4419         struct gsm_dlci *dlci = tty->driver_data;
4420         if (dlci->state == DLCI_CLOSED)
4421                 return;
4422         if (C_CRTSCTS(tty))
4423                 dlci->modem_tx |= TIOCM_RTS;
4424         dlci->throttled = false;
4425         /* Send an MSC with RTS set */
4426         gsm_modem_update(dlci, 0);
4427 }
4428
4429 static int gsmtty_break_ctl(struct tty_struct *tty, int state)
4430 {
4431         struct gsm_dlci *dlci = tty->driver_data;
4432         int encode = 0; /* Off */
4433         if (dlci->state == DLCI_CLOSED)
4434                 return -EINVAL;
4435
4436         if (state == -1)        /* "On indefinitely" - we can't encode this
4437                                     properly */
4438                 encode = 0x0F;
4439         else if (state > 0) {
4440                 encode = state / 200;   /* mS to encoding */
4441                 if (encode > 0x0F)
4442                         encode = 0x0F;  /* Best effort */
4443         }
4444         return gsm_modem_update(dlci, encode);
4445 }
4446
4447 static void gsmtty_cleanup(struct tty_struct *tty)
4448 {
4449         struct gsm_dlci *dlci = tty->driver_data;
4450         struct gsm_mux *gsm = dlci->gsm;
4451
4452         dlci_put(dlci);
4453         dlci_put(gsm->dlci[0]);
4454         mux_put(gsm);
4455 }
4456
4457 /* Virtual ttys for the demux */
4458 static const struct tty_operations gsmtty_ops = {
4459         .install                = gsmtty_install,
4460         .open                   = gsmtty_open,
4461         .close                  = gsmtty_close,
4462         .write                  = gsmtty_write,
4463         .write_room             = gsmtty_write_room,
4464         .chars_in_buffer        = gsmtty_chars_in_buffer,
4465         .flush_buffer           = gsmtty_flush_buffer,
4466         .ioctl                  = gsmtty_ioctl,
4467         .throttle               = gsmtty_throttle,
4468         .unthrottle             = gsmtty_unthrottle,
4469         .set_termios            = gsmtty_set_termios,
4470         .hangup                 = gsmtty_hangup,
4471         .wait_until_sent        = gsmtty_wait_until_sent,
4472         .tiocmget               = gsmtty_tiocmget,
4473         .tiocmset               = gsmtty_tiocmset,
4474         .break_ctl              = gsmtty_break_ctl,
4475         .cleanup                = gsmtty_cleanup,
4476 };
4477
4478
4479
4480 static int __init gsm_init(void)
4481 {
4482         /* Fill in our line protocol discipline, and register it */
4483         int status = tty_register_ldisc(&tty_ldisc_packet);
4484         if (status != 0) {
4485                 pr_err("n_gsm: can't register line discipline (err = %d)\n",
4486                                                                 status);
4487                 return status;
4488         }
4489
4490         gsm_tty_driver = tty_alloc_driver(GSM_TTY_MINORS, TTY_DRIVER_REAL_RAW |
4491                         TTY_DRIVER_DYNAMIC_DEV | TTY_DRIVER_HARDWARE_BREAK);
4492         if (IS_ERR(gsm_tty_driver)) {
4493                 pr_err("gsm_init: tty allocation failed.\n");
4494                 status = PTR_ERR(gsm_tty_driver);
4495                 goto err_unreg_ldisc;
4496         }
4497         gsm_tty_driver->driver_name     = "gsmtty";
4498         gsm_tty_driver->name            = "gsmtty";
4499         gsm_tty_driver->major           = 0;    /* Dynamic */
4500         gsm_tty_driver->minor_start     = 0;
4501         gsm_tty_driver->type            = TTY_DRIVER_TYPE_SERIAL;
4502         gsm_tty_driver->subtype = SERIAL_TYPE_NORMAL;
4503         gsm_tty_driver->init_termios    = tty_std_termios;
4504         /* Fixme */
4505         gsm_tty_driver->init_termios.c_lflag &= ~ECHO;
4506         tty_set_operations(gsm_tty_driver, &gsmtty_ops);
4507
4508         if (tty_register_driver(gsm_tty_driver)) {
4509                 pr_err("gsm_init: tty registration failed.\n");
4510                 status = -EBUSY;
4511                 goto err_put_driver;
4512         }
4513         pr_debug("gsm_init: loaded as %d,%d.\n",
4514                         gsm_tty_driver->major, gsm_tty_driver->minor_start);
4515         return 0;
4516 err_put_driver:
4517         tty_driver_kref_put(gsm_tty_driver);
4518 err_unreg_ldisc:
4519         tty_unregister_ldisc(&tty_ldisc_packet);
4520         return status;
4521 }
4522
4523 static void __exit gsm_exit(void)
4524 {
4525         tty_unregister_ldisc(&tty_ldisc_packet);
4526         tty_unregister_driver(gsm_tty_driver);
4527         tty_driver_kref_put(gsm_tty_driver);
4528 }
4529
4530 module_init(gsm_init);
4531 module_exit(gsm_exit);
4532
4533
4534 MODULE_LICENSE("GPL");
4535 MODULE_ALIAS_LDISC(N_GSM0710);