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