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                         gsm->dlci[i] = NULL;
3103                 }
3104         mutex_unlock(&gsm->mutex);
3105         /* Now wipe the queues */
3106         tty_ldisc_flush(gsm->tty);
3107         list_for_each_entry_safe(txq, ntxq, &gsm->tx_ctrl_list, list)
3108                 kfree(txq);
3109         INIT_LIST_HEAD(&gsm->tx_ctrl_list);
3110         list_for_each_entry_safe(txq, ntxq, &gsm->tx_data_list, list)
3111                 kfree(txq);
3112         INIT_LIST_HEAD(&gsm->tx_data_list);
3113 }
3114
3115 /**
3116  *      gsm_activate_mux        -       generic GSM setup
3117  *      @gsm: our mux
3118  *
3119  *      Set up the bits of the mux which are the same for all framing
3120  *      protocols. Add the mux to the mux table so it can be opened and
3121  *      finally kick off connecting to DLCI 0 on the modem.
3122  */
3123
3124 static int gsm_activate_mux(struct gsm_mux *gsm)
3125 {
3126         struct gsm_dlci *dlci;
3127         int ret;
3128
3129         dlci = gsm_dlci_alloc(gsm, 0);
3130         if (dlci == NULL)
3131                 return -ENOMEM;
3132
3133         if (gsm->encoding == GSM_BASIC_OPT)
3134                 gsm->receive = gsm0_receive;
3135         else
3136                 gsm->receive = gsm1_receive;
3137
3138         ret = gsm_register_devices(gsm_tty_driver, gsm->num);
3139         if (ret)
3140                 return ret;
3141
3142         gsm->has_devices = true;
3143         gsm->dead = false;              /* Tty opens are now permissible */
3144         return 0;
3145 }
3146
3147 /**
3148  *      gsm_free_mux            -       free up a mux
3149  *      @gsm: mux to free
3150  *
3151  *      Dispose of allocated resources for a dead mux
3152  */
3153 static void gsm_free_mux(struct gsm_mux *gsm)
3154 {
3155         int i;
3156
3157         for (i = 0; i < MAX_MUX; i++) {
3158                 if (gsm == gsm_mux[i]) {
3159                         gsm_mux[i] = NULL;
3160                         break;
3161                 }
3162         }
3163         mutex_destroy(&gsm->mutex);
3164         kfree(gsm->txframe);
3165         kfree(gsm->buf);
3166         kfree(gsm);
3167 }
3168
3169 /**
3170  *      gsm_free_muxr           -       free up a mux
3171  *      @ref: kreference to the mux to free
3172  *
3173  *      Dispose of allocated resources for a dead mux
3174  */
3175 static void gsm_free_muxr(struct kref *ref)
3176 {
3177         struct gsm_mux *gsm = container_of(ref, struct gsm_mux, ref);
3178         gsm_free_mux(gsm);
3179 }
3180
3181 static inline void mux_get(struct gsm_mux *gsm)
3182 {
3183         unsigned long flags;
3184
3185         spin_lock_irqsave(&gsm_mux_lock, flags);
3186         kref_get(&gsm->ref);
3187         spin_unlock_irqrestore(&gsm_mux_lock, flags);
3188 }
3189
3190 static inline void mux_put(struct gsm_mux *gsm)
3191 {
3192         unsigned long flags;
3193
3194         spin_lock_irqsave(&gsm_mux_lock, flags);
3195         kref_put(&gsm->ref, gsm_free_muxr);
3196         spin_unlock_irqrestore(&gsm_mux_lock, flags);
3197 }
3198
3199 static inline unsigned int mux_num_to_base(struct gsm_mux *gsm)
3200 {
3201         return gsm->num * NUM_DLCI;
3202 }
3203
3204 static inline unsigned int mux_line_to_num(unsigned int line)
3205 {
3206         return line / NUM_DLCI;
3207 }
3208
3209 /**
3210  *      gsm_alloc_mux           -       allocate a mux
3211  *
3212  *      Creates a new mux ready for activation.
3213  */
3214
3215 static struct gsm_mux *gsm_alloc_mux(void)
3216 {
3217         int i;
3218         struct gsm_mux *gsm = kzalloc(sizeof(struct gsm_mux), GFP_KERNEL);
3219         if (gsm == NULL)
3220                 return NULL;
3221         gsm->buf = kmalloc(MAX_MRU + 1, GFP_KERNEL);
3222         if (gsm->buf == NULL) {
3223                 kfree(gsm);
3224                 return NULL;
3225         }
3226         gsm->txframe = kmalloc(2 * (MAX_MTU + PROT_OVERHEAD - 1), GFP_KERNEL);
3227         if (gsm->txframe == NULL) {
3228                 kfree(gsm->buf);
3229                 kfree(gsm);
3230                 return NULL;
3231         }
3232         spin_lock_init(&gsm->lock);
3233         mutex_init(&gsm->mutex);
3234         kref_init(&gsm->ref);
3235         INIT_LIST_HEAD(&gsm->tx_ctrl_list);
3236         INIT_LIST_HEAD(&gsm->tx_data_list);
3237         timer_setup(&gsm->kick_timer, gsm_kick_timer, 0);
3238         timer_setup(&gsm->t2_timer, gsm_control_retransmit, 0);
3239         timer_setup(&gsm->ka_timer, gsm_control_keep_alive, 0);
3240         INIT_WORK(&gsm->tx_work, gsmld_write_task);
3241         init_waitqueue_head(&gsm->event);
3242         spin_lock_init(&gsm->control_lock);
3243         spin_lock_init(&gsm->tx_lock);
3244
3245         gsm->t1 = T1;
3246         gsm->t2 = T2;
3247         gsm->t3 = T3;
3248         gsm->n2 = N2;
3249         gsm->k = K;
3250         gsm->ftype = UIH;
3251         gsm->adaption = 1;
3252         gsm->encoding = GSM_ADV_OPT;
3253         gsm->mru = 64;  /* Default to encoding 1 so these should be 64 */
3254         gsm->mtu = 64;
3255         gsm->dead = true;       /* Avoid early tty opens */
3256         gsm->wait_config = false; /* Disabled */
3257         gsm->keep_alive = 0;    /* Disabled */
3258
3259         /* Store the instance to the mux array or abort if no space is
3260          * available.
3261          */
3262         spin_lock(&gsm_mux_lock);
3263         for (i = 0; i < MAX_MUX; i++) {
3264                 if (!gsm_mux[i]) {
3265                         gsm_mux[i] = gsm;
3266                         gsm->num = i;
3267                         break;
3268                 }
3269         }
3270         spin_unlock(&gsm_mux_lock);
3271         if (i == MAX_MUX) {
3272                 mutex_destroy(&gsm->mutex);
3273                 kfree(gsm->txframe);
3274                 kfree(gsm->buf);
3275                 kfree(gsm);
3276                 return NULL;
3277         }
3278
3279         return gsm;
3280 }
3281
3282 static void gsm_copy_config_values(struct gsm_mux *gsm,
3283                                    struct gsm_config *c)
3284 {
3285         memset(c, 0, sizeof(*c));
3286         c->adaption = gsm->adaption;
3287         c->encapsulation = gsm->encoding;
3288         c->initiator = gsm->initiator;
3289         c->t1 = gsm->t1;
3290         c->t2 = gsm->t2;
3291         c->t3 = gsm->t3;
3292         c->n2 = gsm->n2;
3293         if (gsm->ftype == UIH)
3294                 c->i = 1;
3295         else
3296                 c->i = 2;
3297         pr_debug("Ftype %d i %d\n", gsm->ftype, c->i);
3298         c->mru = gsm->mru;
3299         c->mtu = gsm->mtu;
3300         c->k = gsm->k;
3301 }
3302
3303 static int gsm_config(struct gsm_mux *gsm, struct gsm_config *c)
3304 {
3305         int need_close = 0;
3306         int need_restart = 0;
3307
3308         /* Stuff we don't support yet - UI or I frame transport */
3309         if (c->adaption != 1 && c->adaption != 2)
3310                 return -EOPNOTSUPP;
3311         /* Check the MRU/MTU range looks sane */
3312         if (c->mru < MIN_MTU || c->mtu < MIN_MTU)
3313                 return -EINVAL;
3314         if (c->mru > MAX_MRU || c->mtu > MAX_MTU)
3315                 return -EINVAL;
3316         if (c->t3 > MAX_T3)
3317                 return -EINVAL;
3318         if (c->n2 > 255)
3319                 return -EINVAL;
3320         if (c->encapsulation > 1)       /* Basic, advanced, no I */
3321                 return -EINVAL;
3322         if (c->initiator > 1)
3323                 return -EINVAL;
3324         if (c->k > MAX_WINDOW_SIZE)
3325                 return -EINVAL;
3326         if (c->i == 0 || c->i > 2)      /* UIH and UI only */
3327                 return -EINVAL;
3328         /*
3329          * See what is needed for reconfiguration
3330          */
3331
3332         /* Timing fields */
3333         if (c->t1 != 0 && c->t1 != gsm->t1)
3334                 need_restart = 1;
3335         if (c->t2 != 0 && c->t2 != gsm->t2)
3336                 need_restart = 1;
3337         if (c->encapsulation != gsm->encoding)
3338                 need_restart = 1;
3339         if (c->adaption != gsm->adaption)
3340                 need_restart = 1;
3341         /* Requires care */
3342         if (c->initiator != gsm->initiator)
3343                 need_close = 1;
3344         if (c->mru != gsm->mru)
3345                 need_restart = 1;
3346         if (c->mtu != gsm->mtu)
3347                 need_restart = 1;
3348
3349         /*
3350          * Close down what is needed, restart and initiate the new
3351          * configuration. On the first time there is no DLCI[0]
3352          * and closing or cleaning up is not necessary.
3353          */
3354         if (need_close || need_restart)
3355                 gsm_cleanup_mux(gsm, true);
3356
3357         gsm->initiator = c->initiator;
3358         gsm->mru = c->mru;
3359         gsm->mtu = c->mtu;
3360         gsm->encoding = c->encapsulation ? GSM_ADV_OPT : GSM_BASIC_OPT;
3361         gsm->adaption = c->adaption;
3362         gsm->n2 = c->n2;
3363
3364         if (c->i == 1)
3365                 gsm->ftype = UIH;
3366         else if (c->i == 2)
3367                 gsm->ftype = UI;
3368
3369         if (c->t1)
3370                 gsm->t1 = c->t1;
3371         if (c->t2)
3372                 gsm->t2 = c->t2;
3373         if (c->t3)
3374                 gsm->t3 = c->t3;
3375         if (c->k)
3376                 gsm->k = c->k;
3377
3378         /*
3379          * FIXME: We need to separate activation/deactivation from adding
3380          * and removing from the mux array
3381          */
3382         if (gsm->dead) {
3383                 int ret = gsm_activate_mux(gsm);
3384                 if (ret)
3385                         return ret;
3386                 if (gsm->initiator)
3387                         gsm_dlci_begin_open(gsm->dlci[0]);
3388         }
3389         return 0;
3390 }
3391
3392 static void gsm_copy_config_ext_values(struct gsm_mux *gsm,
3393                                        struct gsm_config_ext *ce)
3394 {
3395         memset(ce, 0, sizeof(*ce));
3396         ce->wait_config = gsm->wait_config ? 1 : 0;
3397         ce->keep_alive = gsm->keep_alive;
3398 }
3399
3400 static int gsm_config_ext(struct gsm_mux *gsm, struct gsm_config_ext *ce)
3401 {
3402         bool need_restart = false;
3403         unsigned int i;
3404
3405         /*
3406          * Check that userspace doesn't put stuff in here to prevent breakages
3407          * in the future.
3408          */
3409         for (i = 0; i < ARRAY_SIZE(ce->reserved); i++)
3410                 if (ce->reserved[i])
3411                         return -EINVAL;
3412         if (ce->flags & ~GSM_FL_RESTART)
3413                 return -EINVAL;
3414
3415         /* Requires care */
3416         if (ce->flags & GSM_FL_RESTART)
3417                 need_restart = true;
3418
3419         /*
3420          * Close down what is needed, restart and initiate the new
3421          * configuration. On the first time there is no DLCI[0]
3422          * and closing or cleaning up is not necessary.
3423          */
3424         if (need_restart)
3425                 gsm_cleanup_mux(gsm, true);
3426
3427         /*
3428          * Setup the new configuration values
3429          */
3430         gsm->wait_config = ce->wait_config ? true : false;
3431         gsm->keep_alive = ce->keep_alive;
3432
3433         if (gsm->dead) {
3434                 int ret = gsm_activate_mux(gsm);
3435                 if (ret)
3436                         return ret;
3437                 if (gsm->initiator)
3438                         gsm_dlci_begin_open(gsm->dlci[0]);
3439         }
3440
3441         return 0;
3442 }
3443
3444 /**
3445  *      gsmld_output            -       write to link
3446  *      @gsm: our mux
3447  *      @data: bytes to output
3448  *      @len: size
3449  *
3450  *      Write a block of data from the GSM mux to the data channel. This
3451  *      will eventually be serialized from above but at the moment isn't.
3452  */
3453
3454 static int gsmld_output(struct gsm_mux *gsm, u8 *data, int len)
3455 {
3456         if (tty_write_room(gsm->tty) < len) {
3457                 set_bit(TTY_DO_WRITE_WAKEUP, &gsm->tty->flags);
3458                 return -ENOSPC;
3459         }
3460         if (debug & DBG_DATA)
3461                 gsm_hex_dump_bytes(__func__, data, len);
3462         return gsm->tty->ops->write(gsm->tty, data, len);
3463 }
3464
3465
3466 /**
3467  *      gsmld_write_trigger     -       schedule ldisc write task
3468  *      @gsm: our mux
3469  */
3470 static void gsmld_write_trigger(struct gsm_mux *gsm)
3471 {
3472         if (!gsm || !gsm->dlci[0] || gsm->dlci[0]->dead)
3473                 return;
3474         schedule_work(&gsm->tx_work);
3475 }
3476
3477
3478 /**
3479  *      gsmld_write_task        -       ldisc write task
3480  *      @work: our tx write work
3481  *
3482  *      Writes out data to the ldisc if possible. We are doing this here to
3483  *      avoid dead-locking. This returns if no space or data is left for output.
3484  */
3485 static void gsmld_write_task(struct work_struct *work)
3486 {
3487         struct gsm_mux *gsm = container_of(work, struct gsm_mux, tx_work);
3488         unsigned long flags;
3489         int i, ret;
3490
3491         /* All outstanding control channel and control messages and one data
3492          * frame is sent.
3493          */
3494         ret = -ENODEV;
3495         spin_lock_irqsave(&gsm->tx_lock, flags);
3496         if (gsm->tty)
3497                 ret = gsm_data_kick(gsm);
3498         spin_unlock_irqrestore(&gsm->tx_lock, flags);
3499
3500         if (ret >= 0)
3501                 for (i = 0; i < NUM_DLCI; i++)
3502                         if (gsm->dlci[i])
3503                                 tty_port_tty_wakeup(&gsm->dlci[i]->port);
3504 }
3505
3506 /**
3507  *      gsmld_attach_gsm        -       mode set up
3508  *      @tty: our tty structure
3509  *      @gsm: our mux
3510  *
3511  *      Set up the MUX for basic mode and commence connecting to the
3512  *      modem. Currently called from the line discipline set up but
3513  *      will need moving to an ioctl path.
3514  */
3515
3516 static void gsmld_attach_gsm(struct tty_struct *tty, struct gsm_mux *gsm)
3517 {
3518         gsm->tty = tty_kref_get(tty);
3519         /* Turn off tty XON/XOFF handling to handle it explicitly. */
3520         gsm->old_c_iflag = tty->termios.c_iflag;
3521         tty->termios.c_iflag &= (IXON | IXOFF);
3522 }
3523
3524 /**
3525  *      gsmld_detach_gsm        -       stop doing 0710 mux
3526  *      @tty: tty attached to the mux
3527  *      @gsm: mux
3528  *
3529  *      Shutdown and then clean up the resources used by the line discipline
3530  */
3531
3532 static void gsmld_detach_gsm(struct tty_struct *tty, struct gsm_mux *gsm)
3533 {
3534         WARN_ON(tty != gsm->tty);
3535         /* Restore tty XON/XOFF handling. */
3536         gsm->tty->termios.c_iflag = gsm->old_c_iflag;
3537         tty_kref_put(gsm->tty);
3538         gsm->tty = NULL;
3539 }
3540
3541 static void gsmld_receive_buf(struct tty_struct *tty, const u8 *cp,
3542                               const u8 *fp, size_t count)
3543 {
3544         struct gsm_mux *gsm = tty->disc_data;
3545         char flags = TTY_NORMAL;
3546
3547         if (debug & DBG_DATA)
3548                 gsm_hex_dump_bytes(__func__, cp, count);
3549
3550         for (; count; count--, cp++) {
3551                 if (fp)
3552                         flags = *fp++;
3553                 switch (flags) {
3554                 case TTY_NORMAL:
3555                         if (gsm->receive)
3556                                 gsm->receive(gsm, *cp);
3557                         break;
3558                 case TTY_OVERRUN:
3559                 case TTY_BREAK:
3560                 case TTY_PARITY:
3561                 case TTY_FRAME:
3562                         gsm_error(gsm);
3563                         break;
3564                 default:
3565                         WARN_ONCE(1, "%s: unknown flag %d\n",
3566                                tty_name(tty), flags);
3567                         break;
3568                 }
3569         }
3570         /* FASYNC if needed ? */
3571         /* If clogged call tty_throttle(tty); */
3572 }
3573
3574 /**
3575  *      gsmld_flush_buffer      -       clean input queue
3576  *      @tty:   terminal device
3577  *
3578  *      Flush the input buffer. Called when the line discipline is
3579  *      being closed, when the tty layer wants the buffer flushed (eg
3580  *      at hangup).
3581  */
3582
3583 static void gsmld_flush_buffer(struct tty_struct *tty)
3584 {
3585 }
3586
3587 /**
3588  *      gsmld_close             -       close the ldisc for this tty
3589  *      @tty: device
3590  *
3591  *      Called from the terminal layer when this line discipline is
3592  *      being shut down, either because of a close or becsuse of a
3593  *      discipline change. The function will not be called while other
3594  *      ldisc methods are in progress.
3595  */
3596
3597 static void gsmld_close(struct tty_struct *tty)
3598 {
3599         struct gsm_mux *gsm = tty->disc_data;
3600
3601         /* The ldisc locks and closes the port before calling our close. This
3602          * means we have no way to do a proper disconnect. We will not bother
3603          * to do one.
3604          */
3605         gsm_cleanup_mux(gsm, false);
3606
3607         gsmld_detach_gsm(tty, gsm);
3608
3609         gsmld_flush_buffer(tty);
3610         /* Do other clean up here */
3611         mux_put(gsm);
3612 }
3613
3614 /**
3615  *      gsmld_open              -       open an ldisc
3616  *      @tty: terminal to open
3617  *
3618  *      Called when this line discipline is being attached to the
3619  *      terminal device. Can sleep. Called serialized so that no
3620  *      other events will occur in parallel. No further open will occur
3621  *      until a close.
3622  */
3623
3624 static int gsmld_open(struct tty_struct *tty)
3625 {
3626         struct gsm_mux *gsm;
3627
3628         if (!capable(CAP_NET_ADMIN))
3629                 return -EPERM;
3630
3631         if (tty->ops->write == NULL)
3632                 return -EINVAL;
3633
3634         /* Attach our ldisc data */
3635         gsm = gsm_alloc_mux();
3636         if (gsm == NULL)
3637                 return -ENOMEM;
3638
3639         tty->disc_data = gsm;
3640         tty->receive_room = 65536;
3641
3642         /* Attach the initial passive connection */
3643         gsmld_attach_gsm(tty, gsm);
3644
3645         /* The mux will not be activated yet, we wait for correct
3646          * configuration first.
3647          */
3648         if (gsm->encoding == GSM_BASIC_OPT)
3649                 gsm->receive = gsm0_receive;
3650         else
3651                 gsm->receive = gsm1_receive;
3652
3653         return 0;
3654 }
3655
3656 /**
3657  *      gsmld_write_wakeup      -       asynchronous I/O notifier
3658  *      @tty: tty device
3659  *
3660  *      Required for the ptys, serial driver etc. since processes
3661  *      that attach themselves to the master and rely on ASYNC
3662  *      IO must be woken up
3663  */
3664
3665 static void gsmld_write_wakeup(struct tty_struct *tty)
3666 {
3667         struct gsm_mux *gsm = tty->disc_data;
3668
3669         /* Queue poll */
3670         gsmld_write_trigger(gsm);
3671 }
3672
3673 /**
3674  *      gsmld_read              -       read function for tty
3675  *      @tty: tty device
3676  *      @file: file object
3677  *      @buf: userspace buffer pointer
3678  *      @nr: size of I/O
3679  *      @cookie: unused
3680  *      @offset: unused
3681  *
3682  *      Perform reads for the line discipline. We are guaranteed that the
3683  *      line discipline will not be closed under us but we may get multiple
3684  *      parallel readers and must handle this ourselves. We may also get
3685  *      a hangup. Always called in user context, may sleep.
3686  *
3687  *      This code must be sure never to sleep through a hangup.
3688  */
3689
3690 static ssize_t gsmld_read(struct tty_struct *tty, struct file *file, u8 *buf,
3691                           size_t nr, void **cookie, unsigned long offset)
3692 {
3693         return -EOPNOTSUPP;
3694 }
3695
3696 /**
3697  *      gsmld_write             -       write function for tty
3698  *      @tty: tty device
3699  *      @file: file object
3700  *      @buf: userspace buffer pointer
3701  *      @nr: size of I/O
3702  *
3703  *      Called when the owner of the device wants to send a frame
3704  *      itself (or some other control data). The data is transferred
3705  *      as-is and must be properly framed and checksummed as appropriate
3706  *      by userspace. Frames are either sent whole or not at all as this
3707  *      avoids pain user side.
3708  */
3709
3710 static ssize_t gsmld_write(struct tty_struct *tty, struct file *file,
3711                            const u8 *buf, size_t nr)
3712 {
3713         struct gsm_mux *gsm = tty->disc_data;
3714         unsigned long flags;
3715         int space;
3716         int ret;
3717
3718         if (!gsm)
3719                 return -ENODEV;
3720
3721         ret = -ENOBUFS;
3722         spin_lock_irqsave(&gsm->tx_lock, flags);
3723         space = tty_write_room(tty);
3724         if (space >= nr)
3725                 ret = tty->ops->write(tty, buf, nr);
3726         else
3727                 set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
3728         spin_unlock_irqrestore(&gsm->tx_lock, flags);
3729
3730         return ret;
3731 }
3732
3733 /**
3734  *      gsmld_poll              -       poll method for N_GSM0710
3735  *      @tty: terminal device
3736  *      @file: file accessing it
3737  *      @wait: poll table
3738  *
3739  *      Called when the line discipline is asked to poll() for data or
3740  *      for special events. This code is not serialized with respect to
3741  *      other events save open/close.
3742  *
3743  *      This code must be sure never to sleep through a hangup.
3744  *      Called without the kernel lock held - fine
3745  */
3746
3747 static __poll_t gsmld_poll(struct tty_struct *tty, struct file *file,
3748                                                         poll_table *wait)
3749 {
3750         __poll_t mask = 0;
3751         struct gsm_mux *gsm = tty->disc_data;
3752
3753         poll_wait(file, &tty->read_wait, wait);
3754         poll_wait(file, &tty->write_wait, wait);
3755
3756         if (gsm->dead)
3757                 mask |= EPOLLHUP;
3758         if (tty_hung_up_p(file))
3759                 mask |= EPOLLHUP;
3760         if (test_bit(TTY_OTHER_CLOSED, &tty->flags))
3761                 mask |= EPOLLHUP;
3762         if (!tty_is_writelocked(tty) && tty_write_room(tty) > 0)
3763                 mask |= EPOLLOUT | EPOLLWRNORM;
3764         return mask;
3765 }
3766
3767 static int gsmld_ioctl(struct tty_struct *tty, unsigned int cmd,
3768                        unsigned long arg)
3769 {
3770         struct gsm_config c;
3771         struct gsm_config_ext ce;
3772         struct gsm_dlci_config dc;
3773         struct gsm_mux *gsm = tty->disc_data;
3774         unsigned int base, addr;
3775         struct gsm_dlci *dlci;
3776
3777         switch (cmd) {
3778         case GSMIOC_GETCONF:
3779                 gsm_copy_config_values(gsm, &c);
3780                 if (copy_to_user((void __user *)arg, &c, sizeof(c)))
3781                         return -EFAULT;
3782                 return 0;
3783         case GSMIOC_SETCONF:
3784                 if (copy_from_user(&c, (void __user *)arg, sizeof(c)))
3785                         return -EFAULT;
3786                 return gsm_config(gsm, &c);
3787         case GSMIOC_GETFIRST:
3788                 base = mux_num_to_base(gsm);
3789                 return put_user(base + 1, (__u32 __user *)arg);
3790         case GSMIOC_GETCONF_EXT:
3791                 gsm_copy_config_ext_values(gsm, &ce);
3792                 if (copy_to_user((void __user *)arg, &ce, sizeof(ce)))
3793                         return -EFAULT;
3794                 return 0;
3795         case GSMIOC_SETCONF_EXT:
3796                 if (copy_from_user(&ce, (void __user *)arg, sizeof(ce)))
3797                         return -EFAULT;
3798                 return gsm_config_ext(gsm, &ce);
3799         case GSMIOC_GETCONF_DLCI:
3800                 if (copy_from_user(&dc, (void __user *)arg, sizeof(dc)))
3801                         return -EFAULT;
3802                 if (dc.channel == 0 || dc.channel >= NUM_DLCI)
3803                         return -EINVAL;
3804                 addr = array_index_nospec(dc.channel, NUM_DLCI);
3805                 dlci = gsm->dlci[addr];
3806                 if (!dlci) {
3807                         dlci = gsm_dlci_alloc(gsm, addr);
3808                         if (!dlci)
3809                                 return -ENOMEM;
3810                 }
3811                 gsm_dlci_copy_config_values(dlci, &dc);
3812                 if (copy_to_user((void __user *)arg, &dc, sizeof(dc)))
3813                         return -EFAULT;
3814                 return 0;
3815         case GSMIOC_SETCONF_DLCI:
3816                 if (copy_from_user(&dc, (void __user *)arg, sizeof(dc)))
3817                         return -EFAULT;
3818                 if (dc.channel == 0 || dc.channel >= NUM_DLCI)
3819                         return -EINVAL;
3820                 addr = array_index_nospec(dc.channel, NUM_DLCI);
3821                 dlci = gsm->dlci[addr];
3822                 if (!dlci) {
3823                         dlci = gsm_dlci_alloc(gsm, addr);
3824                         if (!dlci)
3825                                 return -ENOMEM;
3826                 }
3827                 return gsm_dlci_config(dlci, &dc, 0);
3828         default:
3829                 return n_tty_ioctl_helper(tty, cmd, arg);
3830         }
3831 }
3832
3833 /*
3834  *      Network interface
3835  *
3836  */
3837
3838 static int gsm_mux_net_open(struct net_device *net)
3839 {
3840         pr_debug("%s called\n", __func__);
3841         netif_start_queue(net);
3842         return 0;
3843 }
3844
3845 static int gsm_mux_net_close(struct net_device *net)
3846 {
3847         netif_stop_queue(net);
3848         return 0;
3849 }
3850
3851 static void dlci_net_free(struct gsm_dlci *dlci)
3852 {
3853         if (!dlci->net) {
3854                 WARN_ON(1);
3855                 return;
3856         }
3857         dlci->adaption = dlci->prev_adaption;
3858         dlci->data = dlci->prev_data;
3859         free_netdev(dlci->net);
3860         dlci->net = NULL;
3861 }
3862 static void net_free(struct kref *ref)
3863 {
3864         struct gsm_mux_net *mux_net;
3865         struct gsm_dlci *dlci;
3866
3867         mux_net = container_of(ref, struct gsm_mux_net, ref);
3868         dlci = mux_net->dlci;
3869
3870         if (dlci->net) {
3871                 unregister_netdev(dlci->net);
3872                 dlci_net_free(dlci);
3873         }
3874 }
3875
3876 static inline void muxnet_get(struct gsm_mux_net *mux_net)
3877 {
3878         kref_get(&mux_net->ref);
3879 }
3880
3881 static inline void muxnet_put(struct gsm_mux_net *mux_net)
3882 {
3883         kref_put(&mux_net->ref, net_free);
3884 }
3885
3886 static netdev_tx_t gsm_mux_net_start_xmit(struct sk_buff *skb,
3887                                       struct net_device *net)
3888 {
3889         struct gsm_mux_net *mux_net = netdev_priv(net);
3890         struct gsm_dlci *dlci = mux_net->dlci;
3891         muxnet_get(mux_net);
3892
3893         skb_queue_head(&dlci->skb_list, skb);
3894         net->stats.tx_packets++;
3895         net->stats.tx_bytes += skb->len;
3896         gsm_dlci_data_kick(dlci);
3897         /* And tell the kernel when the last transmit started. */
3898         netif_trans_update(net);
3899         muxnet_put(mux_net);
3900         return NETDEV_TX_OK;
3901 }
3902
3903 /* called when a packet did not ack after watchdogtimeout */
3904 static void gsm_mux_net_tx_timeout(struct net_device *net, unsigned int txqueue)
3905 {
3906         /* Tell syslog we are hosed. */
3907         dev_dbg(&net->dev, "Tx timed out.\n");
3908
3909         /* Update statistics */
3910         net->stats.tx_errors++;
3911 }
3912
3913 static void gsm_mux_rx_netchar(struct gsm_dlci *dlci,
3914                                 const unsigned char *in_buf, int size)
3915 {
3916         struct net_device *net = dlci->net;
3917         struct sk_buff *skb;
3918         struct gsm_mux_net *mux_net = netdev_priv(net);
3919         muxnet_get(mux_net);
3920
3921         /* Allocate an sk_buff */
3922         skb = dev_alloc_skb(size + NET_IP_ALIGN);
3923         if (!skb) {
3924                 /* We got no receive buffer. */
3925                 net->stats.rx_dropped++;
3926                 muxnet_put(mux_net);
3927                 return;
3928         }
3929         skb_reserve(skb, NET_IP_ALIGN);
3930         skb_put_data(skb, in_buf, size);
3931
3932         skb->dev = net;
3933         skb->protocol = htons(ETH_P_IP);
3934
3935         /* Ship it off to the kernel */
3936         netif_rx(skb);
3937
3938         /* update out statistics */
3939         net->stats.rx_packets++;
3940         net->stats.rx_bytes += size;
3941         muxnet_put(mux_net);
3942         return;
3943 }
3944
3945 static void gsm_mux_net_init(struct net_device *net)
3946 {
3947         static const struct net_device_ops gsm_netdev_ops = {
3948                 .ndo_open               = gsm_mux_net_open,
3949                 .ndo_stop               = gsm_mux_net_close,
3950                 .ndo_start_xmit         = gsm_mux_net_start_xmit,
3951                 .ndo_tx_timeout         = gsm_mux_net_tx_timeout,
3952         };
3953
3954         net->netdev_ops = &gsm_netdev_ops;
3955
3956         /* fill in the other fields */
3957         net->watchdog_timeo = GSM_NET_TX_TIMEOUT;
3958         net->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
3959         net->type = ARPHRD_NONE;
3960         net->tx_queue_len = 10;
3961 }
3962
3963
3964 /* caller holds the dlci mutex */
3965 static void gsm_destroy_network(struct gsm_dlci *dlci)
3966 {
3967         struct gsm_mux_net *mux_net;
3968
3969         pr_debug("destroy network interface\n");
3970         if (!dlci->net)
3971                 return;
3972         mux_net = netdev_priv(dlci->net);
3973         muxnet_put(mux_net);
3974 }
3975
3976
3977 /* caller holds the dlci mutex */
3978 static int gsm_create_network(struct gsm_dlci *dlci, struct gsm_netconfig *nc)
3979 {
3980         char *netname;
3981         int retval = 0;
3982         struct net_device *net;
3983         struct gsm_mux_net *mux_net;
3984
3985         if (!capable(CAP_NET_ADMIN))
3986                 return -EPERM;
3987
3988         /* Already in a non tty mode */
3989         if (dlci->adaption > 2)
3990                 return -EBUSY;
3991
3992         if (nc->protocol != htons(ETH_P_IP))
3993                 return -EPROTONOSUPPORT;
3994
3995         if (nc->adaption != 3 && nc->adaption != 4)
3996                 return -EPROTONOSUPPORT;
3997
3998         pr_debug("create network interface\n");
3999
4000         netname = "gsm%d";
4001         if (nc->if_name[0] != '\0')
4002                 netname = nc->if_name;
4003         net = alloc_netdev(sizeof(struct gsm_mux_net), netname,
4004                            NET_NAME_UNKNOWN, gsm_mux_net_init);
4005         if (!net) {
4006                 pr_err("alloc_netdev failed\n");
4007                 return -ENOMEM;
4008         }
4009         net->mtu = dlci->mtu;
4010         net->min_mtu = MIN_MTU;
4011         net->max_mtu = dlci->mtu;
4012         mux_net = netdev_priv(net);
4013         mux_net->dlci = dlci;
4014         kref_init(&mux_net->ref);
4015         strncpy(nc->if_name, net->name, IFNAMSIZ); /* return net name */
4016
4017         /* reconfigure dlci for network */
4018         dlci->prev_adaption = dlci->adaption;
4019         dlci->prev_data = dlci->data;
4020         dlci->adaption = nc->adaption;
4021         dlci->data = gsm_mux_rx_netchar;
4022         dlci->net = net;
4023
4024         pr_debug("register netdev\n");
4025         retval = register_netdev(net);
4026         if (retval) {
4027                 pr_err("network register fail %d\n", retval);
4028                 dlci_net_free(dlci);
4029                 return retval;
4030         }
4031         return net->ifindex;    /* return network index */
4032 }
4033
4034 /* Line discipline for real tty */
4035 static struct tty_ldisc_ops tty_ldisc_packet = {
4036         .owner           = THIS_MODULE,
4037         .num             = N_GSM0710,
4038         .name            = "n_gsm",
4039         .open            = gsmld_open,
4040         .close           = gsmld_close,
4041         .flush_buffer    = gsmld_flush_buffer,
4042         .read            = gsmld_read,
4043         .write           = gsmld_write,
4044         .ioctl           = gsmld_ioctl,
4045         .poll            = gsmld_poll,
4046         .receive_buf     = gsmld_receive_buf,
4047         .write_wakeup    = gsmld_write_wakeup
4048 };
4049
4050 /*
4051  *      Virtual tty side
4052  */
4053
4054 /**
4055  *      gsm_modem_upd_via_data  -       send modem bits via convergence layer
4056  *      @dlci: channel
4057  *      @brk: break signal
4058  *
4059  *      Send an empty frame to signal mobile state changes and to transmit the
4060  *      break signal for adaption 2.
4061  */
4062
4063 static void gsm_modem_upd_via_data(struct gsm_dlci *dlci, u8 brk)
4064 {
4065         struct gsm_mux *gsm = dlci->gsm;
4066         unsigned long flags;
4067
4068         if (dlci->state != DLCI_OPEN || dlci->adaption != 2)
4069                 return;
4070
4071         spin_lock_irqsave(&gsm->tx_lock, flags);
4072         gsm_dlci_modem_output(gsm, dlci, brk);
4073         spin_unlock_irqrestore(&gsm->tx_lock, flags);
4074 }
4075
4076 /**
4077  *      gsm_modem_upd_via_msc   -       send modem bits via control frame
4078  *      @dlci: channel
4079  *      @brk: break signal
4080  */
4081
4082 static int gsm_modem_upd_via_msc(struct gsm_dlci *dlci, u8 brk)
4083 {
4084         u8 modembits[3];
4085         struct gsm_control *ctrl;
4086         int len = 2;
4087
4088         if (dlci->gsm->encoding != GSM_BASIC_OPT)
4089                 return 0;
4090
4091         modembits[0] = (dlci->addr << 2) | 2 | EA;  /* DLCI, Valid, EA */
4092         if (!brk) {
4093                 modembits[1] = (gsm_encode_modem(dlci) << 1) | EA;
4094         } else {
4095                 modembits[1] = gsm_encode_modem(dlci) << 1;
4096                 modembits[2] = (brk << 4) | 2 | EA; /* Length, Break, EA */
4097                 len++;
4098         }
4099         ctrl = gsm_control_send(dlci->gsm, CMD_MSC, modembits, len);
4100         if (ctrl == NULL)
4101                 return -ENOMEM;
4102         return gsm_control_wait(dlci->gsm, ctrl);
4103 }
4104
4105 /**
4106  *      gsm_modem_update        -       send modem status line state
4107  *      @dlci: channel
4108  *      @brk: break signal
4109  */
4110
4111 static int gsm_modem_update(struct gsm_dlci *dlci, u8 brk)
4112 {
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);