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         list_for_each_entry_safe(txq, ntxq, &gsm->tx_ctrl_list, list)
3161                 kfree(txq);
3162         INIT_LIST_HEAD(&gsm->tx_ctrl_list);
3163         list_for_each_entry_safe(txq, ntxq, &gsm->tx_data_list, list)
3164                 kfree(txq);
3165         INIT_LIST_HEAD(&gsm->tx_data_list);
3166 }
3167
3168 /**
3169  *      gsm_activate_mux        -       generic GSM setup
3170  *      @gsm: our mux
3171  *
3172  *      Set up the bits of the mux which are the same for all framing
3173  *      protocols. Add the mux to the mux table so it can be opened and
3174  *      finally kick off connecting to DLCI 0 on the modem.
3175  */
3176
3177 static int gsm_activate_mux(struct gsm_mux *gsm)
3178 {
3179         struct gsm_dlci *dlci;
3180         int ret;
3181
3182         dlci = gsm_dlci_alloc(gsm, 0);
3183         if (dlci == NULL)
3184                 return -ENOMEM;
3185
3186         if (gsm->encoding == GSM_BASIC_OPT)
3187                 gsm->receive = gsm0_receive;
3188         else
3189                 gsm->receive = gsm1_receive;
3190
3191         ret = gsm_register_devices(gsm_tty_driver, gsm->num);
3192         if (ret)
3193                 return ret;
3194
3195         gsm->has_devices = true;
3196         gsm->dead = false;              /* Tty opens are now permissible */
3197         return 0;
3198 }
3199
3200 /**
3201  *      gsm_free_mux            -       free up a mux
3202  *      @gsm: mux to free
3203  *
3204  *      Dispose of allocated resources for a dead mux
3205  */
3206 static void gsm_free_mux(struct gsm_mux *gsm)
3207 {
3208         int i;
3209
3210         for (i = 0; i < MAX_MUX; i++) {
3211                 if (gsm == gsm_mux[i]) {
3212                         gsm_mux[i] = NULL;
3213                         break;
3214                 }
3215         }
3216         mutex_destroy(&gsm->mutex);
3217         kfree(gsm->txframe);
3218         kfree(gsm->buf);
3219         kfree(gsm);
3220 }
3221
3222 /**
3223  *      gsm_free_muxr           -       free up a mux
3224  *      @ref: kreference to the mux to free
3225  *
3226  *      Dispose of allocated resources for a dead mux
3227  */
3228 static void gsm_free_muxr(struct kref *ref)
3229 {
3230         struct gsm_mux *gsm = container_of(ref, struct gsm_mux, ref);
3231         gsm_free_mux(gsm);
3232 }
3233
3234 static inline void mux_get(struct gsm_mux *gsm)
3235 {
3236         unsigned long flags;
3237
3238         spin_lock_irqsave(&gsm_mux_lock, flags);
3239         kref_get(&gsm->ref);
3240         spin_unlock_irqrestore(&gsm_mux_lock, flags);
3241 }
3242
3243 static inline void mux_put(struct gsm_mux *gsm)
3244 {
3245         unsigned long flags;
3246
3247         spin_lock_irqsave(&gsm_mux_lock, flags);
3248         kref_put(&gsm->ref, gsm_free_muxr);
3249         spin_unlock_irqrestore(&gsm_mux_lock, flags);
3250 }
3251
3252 static inline unsigned int mux_num_to_base(struct gsm_mux *gsm)
3253 {
3254         return gsm->num * NUM_DLCI;
3255 }
3256
3257 static inline unsigned int mux_line_to_num(unsigned int line)
3258 {
3259         return line / NUM_DLCI;
3260 }
3261
3262 /**
3263  *      gsm_alloc_mux           -       allocate a mux
3264  *
3265  *      Creates a new mux ready for activation.
3266  */
3267
3268 static struct gsm_mux *gsm_alloc_mux(void)
3269 {
3270         int i;
3271         struct gsm_mux *gsm = kzalloc(sizeof(struct gsm_mux), GFP_KERNEL);
3272         if (gsm == NULL)
3273                 return NULL;
3274         gsm->buf = kmalloc(MAX_MRU + 1, GFP_KERNEL);
3275         if (gsm->buf == NULL) {
3276                 kfree(gsm);
3277                 return NULL;
3278         }
3279         gsm->txframe = kmalloc(2 * (MAX_MTU + PROT_OVERHEAD - 1), GFP_KERNEL);
3280         if (gsm->txframe == NULL) {
3281                 kfree(gsm->buf);
3282                 kfree(gsm);
3283                 return NULL;
3284         }
3285         spin_lock_init(&gsm->lock);
3286         mutex_init(&gsm->mutex);
3287         kref_init(&gsm->ref);
3288         INIT_LIST_HEAD(&gsm->tx_ctrl_list);
3289         INIT_LIST_HEAD(&gsm->tx_data_list);
3290         timer_setup(&gsm->kick_timer, gsm_kick_timer, 0);
3291         timer_setup(&gsm->t2_timer, gsm_control_retransmit, 0);
3292         timer_setup(&gsm->ka_timer, gsm_control_keep_alive, 0);
3293         INIT_WORK(&gsm->tx_work, gsmld_write_task);
3294         init_waitqueue_head(&gsm->event);
3295         spin_lock_init(&gsm->control_lock);
3296         spin_lock_init(&gsm->tx_lock);
3297
3298         gsm->t1 = T1;
3299         gsm->t2 = T2;
3300         gsm->t3 = T3;
3301         gsm->n2 = N2;
3302         gsm->k = K;
3303         gsm->ftype = UIH;
3304         gsm->adaption = 1;
3305         gsm->encoding = GSM_ADV_OPT;
3306         gsm->mru = 64;  /* Default to encoding 1 so these should be 64 */
3307         gsm->mtu = 64;
3308         gsm->dead = true;       /* Avoid early tty opens */
3309         gsm->wait_config = false; /* Disabled */
3310         gsm->keep_alive = 0;    /* Disabled */
3311
3312         /* Store the instance to the mux array or abort if no space is
3313          * available.
3314          */
3315         spin_lock(&gsm_mux_lock);
3316         for (i = 0; i < MAX_MUX; i++) {
3317                 if (!gsm_mux[i]) {
3318                         gsm_mux[i] = gsm;
3319                         gsm->num = i;
3320                         break;
3321                 }
3322         }
3323         spin_unlock(&gsm_mux_lock);
3324         if (i == MAX_MUX) {
3325                 mutex_destroy(&gsm->mutex);
3326                 kfree(gsm->txframe);
3327                 kfree(gsm->buf);
3328                 kfree(gsm);
3329                 return NULL;
3330         }
3331
3332         return gsm;
3333 }
3334
3335 static void gsm_copy_config_values(struct gsm_mux *gsm,
3336                                    struct gsm_config *c)
3337 {
3338         memset(c, 0, sizeof(*c));
3339         c->adaption = gsm->adaption;
3340         c->encapsulation = gsm->encoding;
3341         c->initiator = gsm->initiator;
3342         c->t1 = gsm->t1;
3343         c->t2 = gsm->t2;
3344         c->t3 = gsm->t3;
3345         c->n2 = gsm->n2;
3346         if (gsm->ftype == UIH)
3347                 c->i = 1;
3348         else
3349                 c->i = 2;
3350         pr_debug("Ftype %d i %d\n", gsm->ftype, c->i);
3351         c->mru = gsm->mru;
3352         c->mtu = gsm->mtu;
3353         c->k = gsm->k;
3354 }
3355
3356 static int gsm_config(struct gsm_mux *gsm, struct gsm_config *c)
3357 {
3358         int need_close = 0;
3359         int need_restart = 0;
3360
3361         /* Stuff we don't support yet - UI or I frame transport */
3362         if (c->adaption != 1 && c->adaption != 2)
3363                 return -EOPNOTSUPP;
3364         /* Check the MRU/MTU range looks sane */
3365         if (c->mru < MIN_MTU || c->mtu < MIN_MTU)
3366                 return -EINVAL;
3367         if (c->mru > MAX_MRU || c->mtu > MAX_MTU)
3368                 return -EINVAL;
3369         if (c->t3 > MAX_T3)
3370                 return -EINVAL;
3371         if (c->n2 > 255)
3372                 return -EINVAL;
3373         if (c->encapsulation > 1)       /* Basic, advanced, no I */
3374                 return -EINVAL;
3375         if (c->initiator > 1)
3376                 return -EINVAL;
3377         if (c->k > MAX_WINDOW_SIZE)
3378                 return -EINVAL;
3379         if (c->i == 0 || c->i > 2)      /* UIH and UI only */
3380                 return -EINVAL;
3381         /*
3382          * See what is needed for reconfiguration
3383          */
3384
3385         /* Timing fields */
3386         if (c->t1 != 0 && c->t1 != gsm->t1)
3387                 need_restart = 1;
3388         if (c->t2 != 0 && c->t2 != gsm->t2)
3389                 need_restart = 1;
3390         if (c->encapsulation != gsm->encoding)
3391                 need_restart = 1;
3392         if (c->adaption != gsm->adaption)
3393                 need_restart = 1;
3394         /* Requires care */
3395         if (c->initiator != gsm->initiator)
3396                 need_close = 1;
3397         if (c->mru != gsm->mru)
3398                 need_restart = 1;
3399         if (c->mtu != gsm->mtu)
3400                 need_restart = 1;
3401
3402         /*
3403          * Close down what is needed, restart and initiate the new
3404          * configuration. On the first time there is no DLCI[0]
3405          * and closing or cleaning up is not necessary.
3406          */
3407         if (need_close || need_restart)
3408                 gsm_cleanup_mux(gsm, true);
3409
3410         gsm->initiator = c->initiator;
3411         gsm->mru = c->mru;
3412         gsm->mtu = c->mtu;
3413         gsm->encoding = c->encapsulation ? GSM_ADV_OPT : GSM_BASIC_OPT;
3414         gsm->adaption = c->adaption;
3415         gsm->n2 = c->n2;
3416
3417         if (c->i == 1)
3418                 gsm->ftype = UIH;
3419         else if (c->i == 2)
3420                 gsm->ftype = UI;
3421
3422         if (c->t1)
3423                 gsm->t1 = c->t1;
3424         if (c->t2)
3425                 gsm->t2 = c->t2;
3426         if (c->t3)
3427                 gsm->t3 = c->t3;
3428         if (c->k)
3429                 gsm->k = c->k;
3430
3431         /*
3432          * FIXME: We need to separate activation/deactivation from adding
3433          * and removing from the mux array
3434          */
3435         if (gsm->dead) {
3436                 int ret = gsm_activate_mux(gsm);
3437                 if (ret)
3438                         return ret;
3439                 if (gsm->initiator)
3440                         gsm_dlci_begin_open(gsm->dlci[0]);
3441         }
3442         return 0;
3443 }
3444
3445 static void gsm_copy_config_ext_values(struct gsm_mux *gsm,
3446                                        struct gsm_config_ext *ce)
3447 {
3448         memset(ce, 0, sizeof(*ce));
3449         ce->wait_config = gsm->wait_config ? 1 : 0;
3450         ce->keep_alive = gsm->keep_alive;
3451 }
3452
3453 static int gsm_config_ext(struct gsm_mux *gsm, struct gsm_config_ext *ce)
3454 {
3455         bool need_restart = false;
3456         unsigned int i;
3457
3458         /*
3459          * Check that userspace doesn't put stuff in here to prevent breakages
3460          * in the future.
3461          */
3462         for (i = 0; i < ARRAY_SIZE(ce->reserved); i++)
3463                 if (ce->reserved[i])
3464                         return -EINVAL;
3465         if (ce->flags & ~GSM_FL_RESTART)
3466                 return -EINVAL;
3467
3468         /* Requires care */
3469         if (ce->flags & GSM_FL_RESTART)
3470                 need_restart = true;
3471
3472         /*
3473          * Close down what is needed, restart and initiate the new
3474          * configuration. On the first time there is no DLCI[0]
3475          * and closing or cleaning up is not necessary.
3476          */
3477         if (need_restart)
3478                 gsm_cleanup_mux(gsm, true);
3479
3480         /*
3481          * Setup the new configuration values
3482          */
3483         gsm->wait_config = ce->wait_config ? true : false;
3484         gsm->keep_alive = ce->keep_alive;
3485
3486         if (gsm->dead) {
3487                 int ret = gsm_activate_mux(gsm);
3488                 if (ret)
3489                         return ret;
3490                 if (gsm->initiator)
3491                         gsm_dlci_begin_open(gsm->dlci[0]);
3492         }
3493
3494         return 0;
3495 }
3496
3497 /**
3498  *      gsmld_output            -       write to link
3499  *      @gsm: our mux
3500  *      @data: bytes to output
3501  *      @len: size
3502  *
3503  *      Write a block of data from the GSM mux to the data channel. This
3504  *      will eventually be serialized from above but at the moment isn't.
3505  */
3506
3507 static int gsmld_output(struct gsm_mux *gsm, u8 *data, int len)
3508 {
3509         if (tty_write_room(gsm->tty) < len) {
3510                 set_bit(TTY_DO_WRITE_WAKEUP, &gsm->tty->flags);
3511                 return -ENOSPC;
3512         }
3513         if (debug & DBG_DATA)
3514                 gsm_hex_dump_bytes(__func__, data, len);
3515         return gsm->tty->ops->write(gsm->tty, data, len);
3516 }
3517
3518
3519 /**
3520  *      gsmld_write_trigger     -       schedule ldisc write task
3521  *      @gsm: our mux
3522  */
3523 static void gsmld_write_trigger(struct gsm_mux *gsm)
3524 {
3525         if (!gsm || !gsm->dlci[0] || gsm->dlci[0]->dead)
3526                 return;
3527         schedule_work(&gsm->tx_work);
3528 }
3529
3530
3531 /**
3532  *      gsmld_write_task        -       ldisc write task
3533  *      @work: our tx write work
3534  *
3535  *      Writes out data to the ldisc if possible. We are doing this here to
3536  *      avoid dead-locking. This returns if no space or data is left for output.
3537  */
3538 static void gsmld_write_task(struct work_struct *work)
3539 {
3540         struct gsm_mux *gsm = container_of(work, struct gsm_mux, tx_work);
3541         unsigned long flags;
3542         int i, ret;
3543
3544         /* All outstanding control channel and control messages and one data
3545          * frame is sent.
3546          */
3547         ret = -ENODEV;
3548         spin_lock_irqsave(&gsm->tx_lock, flags);
3549         if (gsm->tty)
3550                 ret = gsm_data_kick(gsm);
3551         spin_unlock_irqrestore(&gsm->tx_lock, flags);
3552
3553         if (ret >= 0)
3554                 for (i = 0; i < NUM_DLCI; i++)
3555                         if (gsm->dlci[i])
3556                                 tty_port_tty_wakeup(&gsm->dlci[i]->port);
3557 }
3558
3559 /**
3560  *      gsmld_attach_gsm        -       mode set up
3561  *      @tty: our tty structure
3562  *      @gsm: our mux
3563  *
3564  *      Set up the MUX for basic mode and commence connecting to the
3565  *      modem. Currently called from the line discipline set up but
3566  *      will need moving to an ioctl path.
3567  */
3568
3569 static void gsmld_attach_gsm(struct tty_struct *tty, struct gsm_mux *gsm)
3570 {
3571         gsm->tty = tty_kref_get(tty);
3572         /* Turn off tty XON/XOFF handling to handle it explicitly. */
3573         gsm->old_c_iflag = tty->termios.c_iflag;
3574         tty->termios.c_iflag &= (IXON | IXOFF);
3575 }
3576
3577 /**
3578  *      gsmld_detach_gsm        -       stop doing 0710 mux
3579  *      @tty: tty attached to the mux
3580  *      @gsm: mux
3581  *
3582  *      Shutdown and then clean up the resources used by the line discipline
3583  */
3584
3585 static void gsmld_detach_gsm(struct tty_struct *tty, struct gsm_mux *gsm)
3586 {
3587         WARN_ON(tty != gsm->tty);
3588         /* Restore tty XON/XOFF handling. */
3589         gsm->tty->termios.c_iflag = gsm->old_c_iflag;
3590         tty_kref_put(gsm->tty);
3591         gsm->tty = NULL;
3592 }
3593
3594 static void gsmld_receive_buf(struct tty_struct *tty, const u8 *cp,
3595                               const u8 *fp, size_t count)
3596 {
3597         struct gsm_mux *gsm = tty->disc_data;
3598         u8 flags = TTY_NORMAL;
3599
3600         if (debug & DBG_DATA)
3601                 gsm_hex_dump_bytes(__func__, cp, count);
3602
3603         for (; count; count--, cp++) {
3604                 if (fp)
3605                         flags = *fp++;
3606                 switch (flags) {
3607                 case TTY_NORMAL:
3608                         if (gsm->receive)
3609                                 gsm->receive(gsm, *cp);
3610                         break;
3611                 case TTY_OVERRUN:
3612                 case TTY_BREAK:
3613                 case TTY_PARITY:
3614                 case TTY_FRAME:
3615                         gsm_error(gsm);
3616                         break;
3617                 default:
3618                         WARN_ONCE(1, "%s: unknown flag %d\n",
3619                                tty_name(tty), flags);
3620                         break;
3621                 }
3622         }
3623         /* FASYNC if needed ? */
3624         /* If clogged call tty_throttle(tty); */
3625 }
3626
3627 /**
3628  *      gsmld_flush_buffer      -       clean input queue
3629  *      @tty:   terminal device
3630  *
3631  *      Flush the input buffer. Called when the line discipline is
3632  *      being closed, when the tty layer wants the buffer flushed (eg
3633  *      at hangup).
3634  */
3635
3636 static void gsmld_flush_buffer(struct tty_struct *tty)
3637 {
3638 }
3639
3640 /**
3641  *      gsmld_close             -       close the ldisc for this tty
3642  *      @tty: device
3643  *
3644  *      Called from the terminal layer when this line discipline is
3645  *      being shut down, either because of a close or becsuse of a
3646  *      discipline change. The function will not be called while other
3647  *      ldisc methods are in progress.
3648  */
3649
3650 static void gsmld_close(struct tty_struct *tty)
3651 {
3652         struct gsm_mux *gsm = tty->disc_data;
3653
3654         /* The ldisc locks and closes the port before calling our close. This
3655          * means we have no way to do a proper disconnect. We will not bother
3656          * to do one.
3657          */
3658         gsm_cleanup_mux(gsm, false);
3659
3660         gsmld_detach_gsm(tty, gsm);
3661
3662         gsmld_flush_buffer(tty);
3663         /* Do other clean up here */
3664         mux_put(gsm);
3665 }
3666
3667 /**
3668  *      gsmld_open              -       open an ldisc
3669  *      @tty: terminal to open
3670  *
3671  *      Called when this line discipline is being attached to the
3672  *      terminal device. Can sleep. Called serialized so that no
3673  *      other events will occur in parallel. No further open will occur
3674  *      until a close.
3675  */
3676
3677 static int gsmld_open(struct tty_struct *tty)
3678 {
3679         struct gsm_mux *gsm;
3680
3681         if (!capable(CAP_NET_ADMIN))
3682                 return -EPERM;
3683
3684         if (tty->ops->write == NULL)
3685                 return -EINVAL;
3686
3687         /* Attach our ldisc data */
3688         gsm = gsm_alloc_mux();
3689         if (gsm == NULL)
3690                 return -ENOMEM;
3691
3692         tty->disc_data = gsm;
3693         tty->receive_room = 65536;
3694
3695         /* Attach the initial passive connection */
3696         gsmld_attach_gsm(tty, gsm);
3697
3698         /* The mux will not be activated yet, we wait for correct
3699          * configuration first.
3700          */
3701         if (gsm->encoding == GSM_BASIC_OPT)
3702                 gsm->receive = gsm0_receive;
3703         else
3704                 gsm->receive = gsm1_receive;
3705
3706         return 0;
3707 }
3708
3709 /**
3710  *      gsmld_write_wakeup      -       asynchronous I/O notifier
3711  *      @tty: tty device
3712  *
3713  *      Required for the ptys, serial driver etc. since processes
3714  *      that attach themselves to the master and rely on ASYNC
3715  *      IO must be woken up
3716  */
3717
3718 static void gsmld_write_wakeup(struct tty_struct *tty)
3719 {
3720         struct gsm_mux *gsm = tty->disc_data;
3721
3722         /* Queue poll */
3723         gsmld_write_trigger(gsm);
3724 }
3725
3726 /**
3727  *      gsmld_read              -       read function for tty
3728  *      @tty: tty device
3729  *      @file: file object
3730  *      @buf: userspace buffer pointer
3731  *      @nr: size of I/O
3732  *      @cookie: unused
3733  *      @offset: unused
3734  *
3735  *      Perform reads for the line discipline. We are guaranteed that the
3736  *      line discipline will not be closed under us but we may get multiple
3737  *      parallel readers and must handle this ourselves. We may also get
3738  *      a hangup. Always called in user context, may sleep.
3739  *
3740  *      This code must be sure never to sleep through a hangup.
3741  */
3742
3743 static ssize_t gsmld_read(struct tty_struct *tty, struct file *file, u8 *buf,
3744                           size_t nr, void **cookie, unsigned long offset)
3745 {
3746         return -EOPNOTSUPP;
3747 }
3748
3749 /**
3750  *      gsmld_write             -       write function for tty
3751  *      @tty: tty device
3752  *      @file: file object
3753  *      @buf: userspace buffer pointer
3754  *      @nr: size of I/O
3755  *
3756  *      Called when the owner of the device wants to send a frame
3757  *      itself (or some other control data). The data is transferred
3758  *      as-is and must be properly framed and checksummed as appropriate
3759  *      by userspace. Frames are either sent whole or not at all as this
3760  *      avoids pain user side.
3761  */
3762
3763 static ssize_t gsmld_write(struct tty_struct *tty, struct file *file,
3764                            const u8 *buf, size_t nr)
3765 {
3766         struct gsm_mux *gsm = tty->disc_data;
3767         unsigned long flags;
3768         size_t space;
3769         int ret;
3770
3771         if (!gsm)
3772                 return -ENODEV;
3773
3774         ret = -ENOBUFS;
3775         spin_lock_irqsave(&gsm->tx_lock, flags);
3776         space = tty_write_room(tty);
3777         if (space >= nr)
3778                 ret = tty->ops->write(tty, buf, nr);
3779         else
3780                 set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
3781         spin_unlock_irqrestore(&gsm->tx_lock, flags);
3782
3783         return ret;
3784 }
3785
3786 /**
3787  *      gsmld_poll              -       poll method for N_GSM0710
3788  *      @tty: terminal device
3789  *      @file: file accessing it
3790  *      @wait: poll table
3791  *
3792  *      Called when the line discipline is asked to poll() for data or
3793  *      for special events. This code is not serialized with respect to
3794  *      other events save open/close.
3795  *
3796  *      This code must be sure never to sleep through a hangup.
3797  *      Called without the kernel lock held - fine
3798  */
3799
3800 static __poll_t gsmld_poll(struct tty_struct *tty, struct file *file,
3801                                                         poll_table *wait)
3802 {
3803         __poll_t mask = 0;
3804         struct gsm_mux *gsm = tty->disc_data;
3805
3806         poll_wait(file, &tty->read_wait, wait);
3807         poll_wait(file, &tty->write_wait, wait);
3808
3809         if (gsm->dead)
3810                 mask |= EPOLLHUP;
3811         if (tty_hung_up_p(file))
3812                 mask |= EPOLLHUP;
3813         if (test_bit(TTY_OTHER_CLOSED, &tty->flags))
3814                 mask |= EPOLLHUP;
3815         if (!tty_is_writelocked(tty) && tty_write_room(tty) > 0)
3816                 mask |= EPOLLOUT | EPOLLWRNORM;
3817         return mask;
3818 }
3819
3820 static int gsmld_ioctl(struct tty_struct *tty, unsigned int cmd,
3821                        unsigned long arg)
3822 {
3823         struct gsm_config c;
3824         struct gsm_config_ext ce;
3825         struct gsm_dlci_config dc;
3826         struct gsm_mux *gsm = tty->disc_data;
3827         unsigned int base, addr;
3828         struct gsm_dlci *dlci;
3829
3830         switch (cmd) {
3831         case GSMIOC_GETCONF:
3832                 gsm_copy_config_values(gsm, &c);
3833                 if (copy_to_user((void __user *)arg, &c, sizeof(c)))
3834                         return -EFAULT;
3835                 return 0;
3836         case GSMIOC_SETCONF:
3837                 if (copy_from_user(&c, (void __user *)arg, sizeof(c)))
3838                         return -EFAULT;
3839                 return gsm_config(gsm, &c);
3840         case GSMIOC_GETFIRST:
3841                 base = mux_num_to_base(gsm);
3842                 return put_user(base + 1, (__u32 __user *)arg);
3843         case GSMIOC_GETCONF_EXT:
3844                 gsm_copy_config_ext_values(gsm, &ce);
3845                 if (copy_to_user((void __user *)arg, &ce, sizeof(ce)))
3846                         return -EFAULT;
3847                 return 0;
3848         case GSMIOC_SETCONF_EXT:
3849                 if (copy_from_user(&ce, (void __user *)arg, sizeof(ce)))
3850                         return -EFAULT;
3851                 return gsm_config_ext(gsm, &ce);
3852         case GSMIOC_GETCONF_DLCI:
3853                 if (copy_from_user(&dc, (void __user *)arg, sizeof(dc)))
3854                         return -EFAULT;
3855                 if (dc.channel == 0 || dc.channel >= NUM_DLCI)
3856                         return -EINVAL;
3857                 addr = array_index_nospec(dc.channel, NUM_DLCI);
3858                 dlci = gsm->dlci[addr];
3859                 if (!dlci) {
3860                         dlci = gsm_dlci_alloc(gsm, addr);
3861                         if (!dlci)
3862                                 return -ENOMEM;
3863                 }
3864                 gsm_dlci_copy_config_values(dlci, &dc);
3865                 if (copy_to_user((void __user *)arg, &dc, sizeof(dc)))
3866                         return -EFAULT;
3867                 return 0;
3868         case GSMIOC_SETCONF_DLCI:
3869                 if (copy_from_user(&dc, (void __user *)arg, sizeof(dc)))
3870                         return -EFAULT;
3871                 if (dc.channel == 0 || dc.channel >= NUM_DLCI)
3872                         return -EINVAL;
3873                 addr = array_index_nospec(dc.channel, NUM_DLCI);
3874                 dlci = gsm->dlci[addr];
3875                 if (!dlci) {
3876                         dlci = gsm_dlci_alloc(gsm, addr);
3877                         if (!dlci)
3878                                 return -ENOMEM;
3879                 }
3880                 return gsm_dlci_config(dlci, &dc, 0);
3881         default:
3882                 return n_tty_ioctl_helper(tty, cmd, arg);
3883         }
3884 }
3885
3886 /*
3887  *      Network interface
3888  *
3889  */
3890
3891 static int gsm_mux_net_open(struct net_device *net)
3892 {
3893         pr_debug("%s called\n", __func__);
3894         netif_start_queue(net);
3895         return 0;
3896 }
3897
3898 static int gsm_mux_net_close(struct net_device *net)
3899 {
3900         netif_stop_queue(net);
3901         return 0;
3902 }
3903
3904 static void dlci_net_free(struct gsm_dlci *dlci)
3905 {
3906         if (!dlci->net) {
3907                 WARN_ON(1);
3908                 return;
3909         }
3910         dlci->adaption = dlci->prev_adaption;
3911         dlci->data = dlci->prev_data;
3912         free_netdev(dlci->net);
3913         dlci->net = NULL;
3914 }
3915 static void net_free(struct kref *ref)
3916 {
3917         struct gsm_mux_net *mux_net;
3918         struct gsm_dlci *dlci;
3919
3920         mux_net = container_of(ref, struct gsm_mux_net, ref);
3921         dlci = mux_net->dlci;
3922
3923         if (dlci->net) {
3924                 unregister_netdev(dlci->net);
3925                 dlci_net_free(dlci);
3926         }
3927 }
3928
3929 static inline void muxnet_get(struct gsm_mux_net *mux_net)
3930 {
3931         kref_get(&mux_net->ref);
3932 }
3933
3934 static inline void muxnet_put(struct gsm_mux_net *mux_net)
3935 {
3936         kref_put(&mux_net->ref, net_free);
3937 }
3938
3939 static netdev_tx_t gsm_mux_net_start_xmit(struct sk_buff *skb,
3940                                       struct net_device *net)
3941 {
3942         struct gsm_mux_net *mux_net = netdev_priv(net);
3943         struct gsm_dlci *dlci = mux_net->dlci;
3944         muxnet_get(mux_net);
3945
3946         skb_queue_head(&dlci->skb_list, skb);
3947         net->stats.tx_packets++;
3948         net->stats.tx_bytes += skb->len;
3949         gsm_dlci_data_kick(dlci);
3950         /* And tell the kernel when the last transmit started. */
3951         netif_trans_update(net);
3952         muxnet_put(mux_net);
3953         return NETDEV_TX_OK;
3954 }
3955
3956 /* called when a packet did not ack after watchdogtimeout */
3957 static void gsm_mux_net_tx_timeout(struct net_device *net, unsigned int txqueue)
3958 {
3959         /* Tell syslog we are hosed. */
3960         dev_dbg(&net->dev, "Tx timed out.\n");
3961
3962         /* Update statistics */
3963         net->stats.tx_errors++;
3964 }
3965
3966 static void gsm_mux_rx_netchar(struct gsm_dlci *dlci, const u8 *in_buf, int size)
3967 {
3968         struct net_device *net = dlci->net;
3969         struct sk_buff *skb;
3970         struct gsm_mux_net *mux_net = netdev_priv(net);
3971         muxnet_get(mux_net);
3972
3973         /* Allocate an sk_buff */
3974         skb = dev_alloc_skb(size + NET_IP_ALIGN);
3975         if (!skb) {
3976                 /* We got no receive buffer. */
3977                 net->stats.rx_dropped++;
3978                 muxnet_put(mux_net);
3979                 return;
3980         }
3981         skb_reserve(skb, NET_IP_ALIGN);
3982         skb_put_data(skb, in_buf, size);
3983
3984         skb->dev = net;
3985         skb->protocol = htons(ETH_P_IP);
3986
3987         /* Ship it off to the kernel */
3988         netif_rx(skb);
3989
3990         /* update out statistics */
3991         net->stats.rx_packets++;
3992         net->stats.rx_bytes += size;
3993         muxnet_put(mux_net);
3994         return;
3995 }
3996
3997 static void gsm_mux_net_init(struct net_device *net)
3998 {
3999         static const struct net_device_ops gsm_netdev_ops = {
4000                 .ndo_open               = gsm_mux_net_open,
4001                 .ndo_stop               = gsm_mux_net_close,
4002                 .ndo_start_xmit         = gsm_mux_net_start_xmit,
4003                 .ndo_tx_timeout         = gsm_mux_net_tx_timeout,
4004         };
4005
4006         net->netdev_ops = &gsm_netdev_ops;
4007
4008         /* fill in the other fields */
4009         net->watchdog_timeo = GSM_NET_TX_TIMEOUT;
4010         net->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
4011         net->type = ARPHRD_NONE;
4012         net->tx_queue_len = 10;
4013 }
4014
4015
4016 /* caller holds the dlci mutex */
4017 static void gsm_destroy_network(struct gsm_dlci *dlci)
4018 {
4019         struct gsm_mux_net *mux_net;
4020
4021         pr_debug("destroy network interface\n");
4022         if (!dlci->net)
4023                 return;
4024         mux_net = netdev_priv(dlci->net);
4025         muxnet_put(mux_net);
4026 }
4027
4028
4029 /* caller holds the dlci mutex */
4030 static int gsm_create_network(struct gsm_dlci *dlci, struct gsm_netconfig *nc)
4031 {
4032         char *netname;
4033         int retval = 0;
4034         struct net_device *net;
4035         struct gsm_mux_net *mux_net;
4036
4037         if (!capable(CAP_NET_ADMIN))
4038                 return -EPERM;
4039
4040         /* Already in a non tty mode */
4041         if (dlci->adaption > 2)
4042                 return -EBUSY;
4043
4044         if (nc->protocol != htons(ETH_P_IP))
4045                 return -EPROTONOSUPPORT;
4046
4047         if (nc->adaption != 3 && nc->adaption != 4)
4048                 return -EPROTONOSUPPORT;
4049
4050         pr_debug("create network interface\n");
4051
4052         netname = "gsm%d";
4053         if (nc->if_name[0] != '\0')
4054                 netname = nc->if_name;
4055         net = alloc_netdev(sizeof(struct gsm_mux_net), netname,
4056                            NET_NAME_UNKNOWN, gsm_mux_net_init);
4057         if (!net) {
4058                 pr_err("alloc_netdev failed\n");
4059                 return -ENOMEM;
4060         }
4061         net->mtu = dlci->mtu;
4062         net->min_mtu = MIN_MTU;
4063         net->max_mtu = dlci->mtu;
4064         mux_net = netdev_priv(net);
4065         mux_net->dlci = dlci;
4066         kref_init(&mux_net->ref);
4067         strscpy(nc->if_name, net->name); /* return net name */
4068
4069         /* reconfigure dlci for network */
4070         dlci->prev_adaption = dlci->adaption;
4071         dlci->prev_data = dlci->data;
4072         dlci->adaption = nc->adaption;
4073         dlci->data = gsm_mux_rx_netchar;
4074         dlci->net = net;
4075
4076         pr_debug("register netdev\n");
4077         retval = register_netdev(net);
4078         if (retval) {
4079                 pr_err("network register fail %d\n", retval);
4080                 dlci_net_free(dlci);
4081                 return retval;
4082         }
4083         return net->ifindex;    /* return network index */
4084 }
4085
4086 /* Line discipline for real tty */
4087 static struct tty_ldisc_ops tty_ldisc_packet = {
4088         .owner           = THIS_MODULE,
4089         .num             = N_GSM0710,
4090         .name            = "n_gsm",
4091         .open            = gsmld_open,
4092         .close           = gsmld_close,
4093         .flush_buffer    = gsmld_flush_buffer,
4094         .read            = gsmld_read,
4095         .write           = gsmld_write,
4096         .ioctl           = gsmld_ioctl,
4097         .poll            = gsmld_poll,
4098         .receive_buf     = gsmld_receive_buf,
4099         .write_wakeup    = gsmld_write_wakeup
4100 };
4101
4102 /*
4103  *      Virtual tty side
4104  */
4105
4106 /**
4107  *      gsm_modem_upd_via_data  -       send modem bits via convergence layer
4108  *      @dlci: channel
4109  *      @brk: break signal
4110  *
4111  *      Send an empty frame to signal mobile state changes and to transmit the
4112  *      break signal for adaption 2.
4113  */
4114
4115 static void gsm_modem_upd_via_data(struct gsm_dlci *dlci, u8 brk)
4116 {
4117         struct gsm_mux *gsm = dlci->gsm;
4118         unsigned long flags;
4119
4120         if (dlci->state != DLCI_OPEN || dlci->adaption != 2)
4121                 return;
4122
4123         spin_lock_irqsave(&gsm->tx_lock, flags);
4124         gsm_dlci_modem_output(gsm, dlci, brk);
4125         spin_unlock_irqrestore(&gsm->tx_lock, flags);
4126 }
4127
4128 /**
4129  *      gsm_modem_upd_via_msc   -       send modem bits via control frame
4130  *      @dlci: channel
4131  *      @brk: break signal
4132  */
4133
4134 static int gsm_modem_upd_via_msc(struct gsm_dlci *dlci, u8 brk)
4135 {
4136         u8 modembits[3];
4137         struct gsm_control *ctrl;
4138         int len = 2;
4139
4140         if (dlci->gsm->encoding != GSM_BASIC_OPT)
4141                 return 0;
4142
4143         modembits[0] = (dlci->addr << 2) | 2 | EA;  /* DLCI, Valid, EA */
4144         if (!brk) {
4145                 modembits[1] = (gsm_encode_modem(dlci) << 1) | EA;
4146         } else {
4147                 modembits[1] = gsm_encode_modem(dlci) << 1;
4148                 modembits[2] = (brk << 4) | 2 | EA; /* Length, Break, EA */
4149                 len++;
4150         }
4151         ctrl = gsm_control_send(dlci->gsm, CMD_MSC, modembits, len);
4152         if (ctrl == NULL)
4153                 return -ENOMEM;
4154         return gsm_control_wait(dlci->gsm, ctrl);
4155 }
4156
4157 /**
4158  *      gsm_modem_update        -       send modem status line state
4159  *      @dlci: channel
4160  *      @brk: break signal
4161  */
4162
4163 static int gsm_modem_update(struct gsm_dlci *dlci, u8 brk)
4164 {
4165         if (dlci->gsm->dead)
4166                 return -EL2HLT;
4167         if (dlci->adaption == 2) {
4168                 /* Send convergence layer type 2 empty data frame. */
4169                 gsm_modem_upd_via_data(dlci, brk);
4170                 return 0;
4171         } else if (dlci->gsm->encoding == GSM_BASIC_OPT) {
4172                 /* Send as MSC control message. */
4173                 return gsm_modem_upd_via_msc(dlci, brk);
4174         }
4175
4176         /* Modem status lines are not supported. */
4177         return -EPROTONOSUPPORT;
4178 }
4179
4180 /**
4181  * gsm_wait_modem_change - wait for modem status line change
4182  * @dlci: channel
4183  * @mask: modem status line bits
4184  *
4185  * The function returns if:
4186  * - any given modem status line bit changed
4187  * - the wait event function got interrupted (e.g. by a signal)
4188  * - the underlying DLCI was closed
4189  * - the underlying ldisc device was removed
4190  */
4191 static int gsm_wait_modem_change(struct gsm_dlci *dlci, u32 mask)
4192 {
4193         struct gsm_mux *gsm = dlci->gsm;
4194         u32 old = dlci->modem_rx;
4195         int ret;
4196
4197         ret = wait_event_interruptible(gsm->event, gsm->dead ||
4198                                        dlci->state != DLCI_OPEN ||
4199                                        (old ^ dlci->modem_rx) & mask);
4200         if (gsm->dead)
4201                 return -ENODEV;
4202         if (dlci->state != DLCI_OPEN)
4203                 return -EL2NSYNC;
4204         return ret;
4205 }
4206
4207 static bool gsm_carrier_raised(struct tty_port *port)
4208 {
4209         struct gsm_dlci *dlci = container_of(port, struct gsm_dlci, port);
4210         struct gsm_mux *gsm = dlci->gsm;
4211
4212         /* Not yet open so no carrier info */
4213         if (dlci->state != DLCI_OPEN)
4214                 return false;
4215         if (debug & DBG_CD_ON)
4216                 return true;
4217
4218         /*
4219          * Basic mode with control channel in ADM mode may not respond
4220          * to CMD_MSC at all and modem_rx is empty.
4221          */
4222         if (gsm->encoding == GSM_BASIC_OPT &&
4223             gsm->dlci[0]->mode == DLCI_MODE_ADM && !dlci->modem_rx)
4224                 return true;
4225
4226         return dlci->modem_rx & TIOCM_CD;
4227 }
4228
4229 static void gsm_dtr_rts(struct tty_port *port, bool active)
4230 {
4231         struct gsm_dlci *dlci = container_of(port, struct gsm_dlci, port);
4232         unsigned int modem_tx = dlci->modem_tx;
4233         if (active)
4234                 modem_tx |= TIOCM_DTR | TIOCM_RTS;
4235         else
4236                 modem_tx &= ~(TIOCM_DTR | TIOCM_RTS);
4237         if (modem_tx != dlci->modem_tx) {
4238                 dlci->modem_tx = modem_tx;
4239                 gsm_modem_update(dlci, 0);
4240         }
4241 }
4242
4243 static const struct tty_port_operations gsm_port_ops = {
4244         .carrier_raised = gsm_carrier_raised,
4245         .dtr_rts = gsm_dtr_rts,
4246         .destruct = gsm_dlci_free,
4247 };
4248
4249 static int gsmtty_install(struct tty_driver *driver, struct tty_struct *tty)
4250 {
4251         struct gsm_mux *gsm;
4252         struct gsm_dlci *dlci;
4253         unsigned int line = tty->index;
4254         unsigned int mux = mux_line_to_num(line);
4255         bool alloc = false;
4256         int ret;
4257
4258         line = line & 0x3F;
4259
4260         if (mux >= MAX_MUX)
4261                 return -ENXIO;
4262         /* FIXME: we need to lock gsm_mux for lifetimes of ttys eventually */
4263         if (gsm_mux[mux] == NULL)
4264                 return -EUNATCH;
4265         if (line == 0 || line > 61)     /* 62/63 reserved */
4266                 return -ECHRNG;
4267         gsm = gsm_mux[mux];
4268         if (gsm->dead)
4269                 return -EL2HLT;
4270         /* If DLCI 0 is not yet fully open return an error.
4271         This is ok from a locking
4272         perspective as we don't have to worry about this
4273         if DLCI0 is lost */
4274         mutex_lock(&gsm->mutex);
4275         if (gsm->dlci[0] && gsm->dlci[0]->state != DLCI_OPEN) {
4276                 mutex_unlock(&gsm->mutex);
4277                 return -EL2NSYNC;
4278         }
4279         dlci = gsm->dlci[line];
4280         if (dlci == NULL) {
4281                 alloc = true;
4282                 dlci = gsm_dlci_alloc(gsm, line);
4283         }
4284         if (dlci == NULL) {
4285                 mutex_unlock(&gsm->mutex);
4286                 return -ENOMEM;
4287         }
4288         ret = tty_port_install(&dlci->port, driver, tty);
4289         if (ret) {
4290                 if (alloc)
4291                         dlci_put(dlci);
4292                 mutex_unlock(&gsm->mutex);
4293                 return ret;
4294         }
4295
4296         dlci_get(dlci);
4297         dlci_get(gsm->dlci[0]);
4298         mux_get(gsm);
4299         tty->driver_data = dlci;
4300         mutex_unlock(&gsm->mutex);
4301
4302         return 0;
4303 }
4304
4305 static int gsmtty_open(struct tty_struct *tty, struct file *filp)
4306 {
4307         struct gsm_dlci *dlci = tty->driver_data;
4308         struct tty_port *port = &dlci->port;
4309
4310         port->count++;
4311         tty_port_tty_set(port, tty);
4312
4313         dlci->modem_rx = 0;
4314         /* We could in theory open and close before we wait - eg if we get
4315            a DM straight back. This is ok as that will have caused a hangup */
4316         tty_port_set_initialized(port, true);
4317         /* Start sending off SABM messages */
4318         if (!dlci->gsm->wait_config) {
4319                 /* Start sending off SABM messages */
4320                 if (dlci->gsm->initiator)
4321                         gsm_dlci_begin_open(dlci);
4322                 else
4323                         gsm_dlci_set_opening(dlci);
4324         } else {
4325                 gsm_dlci_set_wait_config(dlci);
4326         }
4327         /* And wait for virtual carrier */
4328         return tty_port_block_til_ready(port, tty, filp);
4329 }
4330
4331 static void gsmtty_close(struct tty_struct *tty, struct file *filp)
4332 {
4333         struct gsm_dlci *dlci = tty->driver_data;
4334
4335         if (dlci == NULL)
4336                 return;
4337         if (dlci->state == DLCI_CLOSED)
4338                 return;
4339         mutex_lock(&dlci->mutex);
4340         gsm_destroy_network(dlci);
4341         mutex_unlock(&dlci->mutex);
4342         if (tty_port_close_start(&dlci->port, tty, filp) == 0)
4343                 return;
4344         gsm_dlci_begin_close(dlci);
4345         if (tty_port_initialized(&dlci->port) && C_HUPCL(tty))
4346                 tty_port_lower_dtr_rts(&dlci->port);
4347         tty_port_close_end(&dlci->port, tty);
4348         tty_port_tty_set(&dlci->port, NULL);
4349         return;
4350 }
4351
4352 static void gsmtty_hangup(struct tty_struct *tty)
4353 {
4354         struct gsm_dlci *dlci = tty->driver_data;
4355         if (dlci->state == DLCI_CLOSED)
4356                 return;
4357         tty_port_hangup(&dlci->port);
4358         gsm_dlci_begin_close(dlci);
4359 }
4360
4361 static ssize_t gsmtty_write(struct tty_struct *tty, const u8 *buf, size_t len)
4362 {
4363         int sent;
4364         struct gsm_dlci *dlci = tty->driver_data;
4365         if (dlci->state == DLCI_CLOSED)
4366                 return -EINVAL;
4367         /* Stuff the bytes into the fifo queue */
4368         sent = kfifo_in_locked(&dlci->fifo, buf, len, &dlci->lock);
4369         /* Need to kick the channel */
4370         gsm_dlci_data_kick(dlci);
4371         return sent;
4372 }
4373
4374 static unsigned int gsmtty_write_room(struct tty_struct *tty)
4375 {
4376         struct gsm_dlci *dlci = tty->driver_data;
4377         if (dlci->state == DLCI_CLOSED)
4378                 return 0;
4379         return kfifo_avail(&dlci->fifo);
4380 }
4381
4382 static unsigned int gsmtty_chars_in_buffer(struct tty_struct *tty)
4383 {
4384         struct gsm_dlci *dlci = tty->driver_data;
4385         if (dlci->state == DLCI_CLOSED)
4386                 return 0;
4387         return kfifo_len(&dlci->fifo);
4388 }
4389
4390 static void gsmtty_flush_buffer(struct tty_struct *tty)
4391 {
4392         struct gsm_dlci *dlci = tty->driver_data;
4393         unsigned long flags;
4394
4395         if (dlci->state == DLCI_CLOSED)
4396                 return;
4397         /* Caution needed: If we implement reliable transport classes
4398            then the data being transmitted can't simply be junked once
4399            it has first hit the stack. Until then we can just blow it
4400            away */
4401         spin_lock_irqsave(&dlci->lock, flags);
4402         kfifo_reset(&dlci->fifo);
4403         spin_unlock_irqrestore(&dlci->lock, flags);
4404         /* Need to unhook this DLCI from the transmit queue logic */
4405 }
4406
4407 static void gsmtty_wait_until_sent(struct tty_struct *tty, int timeout)
4408 {
4409         /* The FIFO handles the queue so the kernel will do the right
4410            thing waiting on chars_in_buffer before calling us. No work
4411            to do here */
4412 }
4413
4414 static int gsmtty_tiocmget(struct tty_struct *tty)
4415 {
4416         struct gsm_dlci *dlci = tty->driver_data;
4417         if (dlci->state == DLCI_CLOSED)
4418                 return -EINVAL;
4419         return dlci->modem_rx;
4420 }
4421
4422 static int gsmtty_tiocmset(struct tty_struct *tty,
4423         unsigned int set, unsigned int clear)
4424 {
4425         struct gsm_dlci *dlci = tty->driver_data;
4426         unsigned int modem_tx = dlci->modem_tx;
4427
4428         if (dlci->state == DLCI_CLOSED)
4429                 return -EINVAL;
4430         modem_tx &= ~clear;
4431         modem_tx |= set;
4432
4433         if (modem_tx != dlci->modem_tx) {
4434                 dlci->modem_tx = modem_tx;
4435                 return gsm_modem_update(dlci, 0);
4436         }
4437         return 0;
4438 }
4439
4440
4441 static int gsmtty_ioctl(struct tty_struct *tty,
4442                         unsigned int cmd, unsigned long arg)
4443 {
4444         struct gsm_dlci *dlci = tty->driver_data;
4445         struct gsm_netconfig nc;
4446         struct gsm_dlci_config dc;
4447         int index;
4448
4449         if (dlci->state == DLCI_CLOSED)
4450                 return -EINVAL;
4451         switch (cmd) {
4452         case GSMIOC_ENABLE_NET:
4453                 if (copy_from_user(&nc, (void __user *)arg, sizeof(nc)))
4454                         return -EFAULT;
4455                 nc.if_name[IFNAMSIZ-1] = '\0';
4456                 /* return net interface index or error code */
4457                 mutex_lock(&dlci->mutex);
4458                 index = gsm_create_network(dlci, &nc);
4459                 mutex_unlock(&dlci->mutex);
4460                 if (copy_to_user((void __user *)arg, &nc, sizeof(nc)))
4461                         return -EFAULT;
4462                 return index;
4463         case GSMIOC_DISABLE_NET:
4464                 if (!capable(CAP_NET_ADMIN))
4465                         return -EPERM;
4466                 mutex_lock(&dlci->mutex);
4467                 gsm_destroy_network(dlci);
4468                 mutex_unlock(&dlci->mutex);
4469                 return 0;
4470         case GSMIOC_GETCONF_DLCI:
4471                 if (copy_from_user(&dc, (void __user *)arg, sizeof(dc)))
4472                         return -EFAULT;
4473                 if (dc.channel != dlci->addr)
4474                         return -EPERM;
4475                 gsm_dlci_copy_config_values(dlci, &dc);
4476                 if (copy_to_user((void __user *)arg, &dc, sizeof(dc)))
4477                         return -EFAULT;
4478                 return 0;
4479         case GSMIOC_SETCONF_DLCI:
4480                 if (copy_from_user(&dc, (void __user *)arg, sizeof(dc)))
4481                         return -EFAULT;
4482                 if (dc.channel >= NUM_DLCI)
4483                         return -EINVAL;
4484                 if (dc.channel != 0 && dc.channel != dlci->addr)
4485                         return -EPERM;
4486                 return gsm_dlci_config(dlci, &dc, 1);
4487         case TIOCMIWAIT:
4488                 return gsm_wait_modem_change(dlci, (u32)arg);
4489         default:
4490                 return -ENOIOCTLCMD;
4491         }
4492 }
4493
4494 static void gsmtty_set_termios(struct tty_struct *tty,
4495                                const struct ktermios *old)
4496 {
4497         struct gsm_dlci *dlci = tty->driver_data;
4498         if (dlci->state == DLCI_CLOSED)
4499                 return;
4500         /* For the moment its fixed. In actual fact the speed information
4501            for the virtual channel can be propogated in both directions by
4502            the RPN control message. This however rapidly gets nasty as we
4503            then have to remap modem signals each way according to whether
4504            our virtual cable is null modem etc .. */
4505         tty_termios_copy_hw(&tty->termios, old);
4506 }
4507
4508 static void gsmtty_throttle(struct tty_struct *tty)
4509 {
4510         struct gsm_dlci *dlci = tty->driver_data;
4511         if (dlci->state == DLCI_CLOSED)
4512                 return;
4513         if (C_CRTSCTS(tty))
4514                 dlci->modem_tx &= ~TIOCM_RTS;
4515         dlci->throttled = true;
4516         /* Send an MSC with RTS cleared */
4517         gsm_modem_update(dlci, 0);
4518 }
4519
4520 static void gsmtty_unthrottle(struct tty_struct *tty)
4521 {
4522         struct gsm_dlci *dlci = tty->driver_data;
4523         if (dlci->state == DLCI_CLOSED)
4524                 return;
4525         if (C_CRTSCTS(tty))
4526                 dlci->modem_tx |= TIOCM_RTS;
4527         dlci->throttled = false;
4528         /* Send an MSC with RTS set */
4529         gsm_modem_update(dlci, 0);
4530 }
4531
4532 static int gsmtty_break_ctl(struct tty_struct *tty, int state)
4533 {
4534         struct gsm_dlci *dlci = tty->driver_data;
4535         int encode = 0; /* Off */
4536         if (dlci->state == DLCI_CLOSED)
4537                 return -EINVAL;
4538
4539         if (state == -1)        /* "On indefinitely" - we can't encode this
4540                                     properly */
4541                 encode = 0x0F;
4542         else if (state > 0) {
4543                 encode = state / 200;   /* mS to encoding */
4544                 if (encode > 0x0F)
4545                         encode = 0x0F;  /* Best effort */
4546         }
4547         return gsm_modem_update(dlci, encode);
4548 }
4549
4550 static void gsmtty_cleanup(struct tty_struct *tty)
4551 {
4552         struct gsm_dlci *dlci = tty->driver_data;
4553         struct gsm_mux *gsm = dlci->gsm;
4554
4555         dlci_put(dlci);
4556         dlci_put(gsm->dlci[0]);
4557         mux_put(gsm);
4558 }
4559
4560 /* Virtual ttys for the demux */
4561 static const struct tty_operations gsmtty_ops = {
4562         .install                = gsmtty_install,
4563         .open                   = gsmtty_open,
4564         .close                  = gsmtty_close,
4565         .write                  = gsmtty_write,
4566         .write_room             = gsmtty_write_room,
4567         .chars_in_buffer        = gsmtty_chars_in_buffer,
4568         .flush_buffer           = gsmtty_flush_buffer,
4569         .ioctl                  = gsmtty_ioctl,
4570         .throttle               = gsmtty_throttle,
4571         .unthrottle             = gsmtty_unthrottle,
4572         .set_termios            = gsmtty_set_termios,
4573         .hangup                 = gsmtty_hangup,
4574         .wait_until_sent        = gsmtty_wait_until_sent,
4575         .tiocmget               = gsmtty_tiocmget,
4576         .tiocmset               = gsmtty_tiocmset,
4577         .break_ctl              = gsmtty_break_ctl,
4578         .cleanup                = gsmtty_cleanup,
4579 };
4580
4581
4582
4583 static int __init gsm_init(void)
4584 {
4585         /* Fill in our line protocol discipline, and register it */
4586         int status = tty_register_ldisc(&tty_ldisc_packet);
4587         if (status != 0) {
4588                 pr_err("n_gsm: can't register line discipline (err = %d)\n",
4589                                                                 status);
4590                 return status;
4591         }
4592
4593         gsm_tty_driver = tty_alloc_driver(GSM_TTY_MINORS, TTY_DRIVER_REAL_RAW |
4594                         TTY_DRIVER_DYNAMIC_DEV | TTY_DRIVER_HARDWARE_BREAK);
4595         if (IS_ERR(gsm_tty_driver)) {
4596                 pr_err("gsm_init: tty allocation failed.\n");
4597                 status = PTR_ERR(gsm_tty_driver);
4598                 goto err_unreg_ldisc;
4599         }
4600         gsm_tty_driver->driver_name     = "gsmtty";
4601         gsm_tty_driver->name            = "gsmtty";
4602         gsm_tty_driver->major           = 0;    /* Dynamic */
4603         gsm_tty_driver->minor_start     = 0;
4604         gsm_tty_driver->type            = TTY_DRIVER_TYPE_SERIAL;
4605         gsm_tty_driver->subtype = SERIAL_TYPE_NORMAL;
4606         gsm_tty_driver->init_termios    = tty_std_termios;
4607         /* Fixme */
4608         gsm_tty_driver->init_termios.c_lflag &= ~ECHO;
4609         tty_set_operations(gsm_tty_driver, &gsmtty_ops);
4610
4611         if (tty_register_driver(gsm_tty_driver)) {
4612                 pr_err("gsm_init: tty registration failed.\n");
4613                 status = -EBUSY;
4614                 goto err_put_driver;
4615         }
4616         pr_debug("gsm_init: loaded as %d,%d.\n",
4617                         gsm_tty_driver->major, gsm_tty_driver->minor_start);
4618         return 0;
4619 err_put_driver:
4620         tty_driver_kref_put(gsm_tty_driver);
4621 err_unreg_ldisc:
4622         tty_unregister_ldisc(&tty_ldisc_packet);
4623         return status;
4624 }
4625
4626 static void __exit gsm_exit(void)
4627 {
4628         tty_unregister_ldisc(&tty_ldisc_packet);
4629         tty_unregister_driver(gsm_tty_driver);
4630         tty_driver_kref_put(gsm_tty_driver);
4631 }
4632
4633 module_init(gsm_init);
4634 module_exit(gsm_exit);
4635
4636
4637 MODULE_DESCRIPTION("GSM 0710 tty multiplexor");
4638 MODULE_LICENSE("GPL");
4639 MODULE_ALIAS_LDISC(N_GSM0710);