drivers/tty/n_gsm.c

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