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