linux/drivers/tty/n_gsm.c
<<
>>
Prefs
   1/*
   2 * n_gsm.c GSM 0710 tty multiplexor
   3 * Copyright (c) 2009/10 Intel Corporation
   4 *
   5 * This program is free software; you can redistribute it and/or modify
   6 * it under the terms of the GNU General Public License version 2 as
   7 * published by the Free Software Foundation.
   8 *
   9 * This program is distributed in the hope that it will be useful,
  10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 * GNU General Public License for more details.
  13 *
  14 * You should have received a copy of the GNU General Public License
  15 * along with this program; if not, write to the Free Software
  16 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  17 *
  18 *      * THIS IS A DEVELOPMENT SNAPSHOT IT IS NOT A FINAL RELEASE *
  19 *
  20 * TO DO:
  21 *      Mostly done:    ioctls for setting modes/timing
  22 *      Partly done:    hooks so you can pull off frames to non tty devs
  23 *      Restart DLCI 0 when it closes ?
  24 *      Improve the tx engine
  25 *      Resolve tx side locking by adding a queue_head and routing
  26 *              all control traffic via it
  27 *      General tidy/document
  28 *      Review the locking/move to refcounts more (mux now moved to an
  29 *              alloc/free model ready)
  30 *      Use newest tty open/close port helpers and install hooks
  31 *      What to do about power functions ?
  32 *      Termios setting and negotiation
  33 *      Do we need a 'which mux are you' ioctl to correlate mux and tty sets
  34 *
  35 */
  36
  37#include <linux/types.h>
  38#include <linux/major.h>
  39#include <linux/errno.h>
  40#include <linux/signal.h>
  41#include <linux/fcntl.h>
  42#include <linux/sched.h>
  43#include <linux/interrupt.h>
  44#include <linux/tty.h>
  45#include <linux/ctype.h>
  46#include <linux/mm.h>
  47#include <linux/string.h>
  48#include <linux/slab.h>
  49#include <linux/poll.h>
  50#include <linux/bitops.h>
  51#include <linux/file.h>
  52#include <linux/uaccess.h>
  53#include <linux/module.h>
  54#include <linux/timer.h>
  55#include <linux/tty_flip.h>
  56#include <linux/tty_driver.h>
  57#include <linux/serial.h>
  58#include <linux/kfifo.h>
  59#include <linux/skbuff.h>
  60#include <net/arp.h>
  61#include <linux/ip.h>
  62#include <linux/netdevice.h>
  63#include <linux/etherdevice.h>
  64#include <linux/gsmmux.h>
  65
  66static int debug;
  67module_param(debug, int, 0600);
  68
  69/* Defaults: these are from the specification */
  70
  71#define T1      10              /* 100mS */
  72#define T2      34              /* 333mS */
  73#define N2      3               /* Retry 3 times */
  74
  75/* Use long timers for testing at low speed with debug on */
  76#ifdef DEBUG_TIMING
  77#define T1      100
  78#define T2      200
  79#endif
  80
  81/*
  82 * Semi-arbitrary buffer size limits. 0710 is normally run with 32-64 byte
  83 * limits so this is plenty
  84 */
  85#define MAX_MRU 1500
  86#define MAX_MTU 1500
  87#define GSM_NET_TX_TIMEOUT (HZ*10)
  88
  89/**
  90 *      struct gsm_mux_net      -       network interface
  91 *      @struct gsm_dlci* dlci
  92 *      @struct net_device_stats stats;
  93 *
  94 *      Created when net interface is initialized.
  95 **/
  96struct gsm_mux_net {
  97        struct kref ref;
  98        struct gsm_dlci *dlci;
  99        struct net_device_stats stats;
 100};
 101
 102#define STATS(net) (((struct gsm_mux_net *)netdev_priv(net))->stats)
 103
 104/*
 105 *      Each block of data we have queued to go out is in the form of
 106 *      a gsm_msg which holds everything we need in a link layer independent
 107 *      format
 108 */
 109
 110struct gsm_msg {
 111        struct list_head list;
 112        u8 addr;                /* DLCI address + flags */
 113        u8 ctrl;                /* Control byte + flags */
 114        unsigned int len;       /* Length of data block (can be zero) */
 115        unsigned char *data;    /* Points into buffer but not at the start */
 116        unsigned char buffer[0];
 117};
 118
 119/*
 120 *      Each active data link has a gsm_dlci structure associated which ties
 121 *      the link layer to an optional tty (if the tty side is open). To avoid
 122 *      complexity right now these are only ever freed up when the mux is
 123 *      shut down.
 124 *
 125 *      At the moment we don't free DLCI objects until the mux is torn down
 126 *      this avoid object life time issues but might be worth review later.
 127 */
 128
 129struct gsm_dlci {
 130        struct gsm_mux *gsm;
 131        int addr;
 132        int state;
 133#define DLCI_CLOSED             0
 134#define DLCI_OPENING            1       /* Sending SABM not seen UA */
 135#define DLCI_OPEN               2       /* SABM/UA complete */
 136#define DLCI_CLOSING            3       /* Sending DISC not seen UA/DM */
 137        struct mutex mutex;
 138
 139        /* Link layer */
 140        spinlock_t lock;        /* Protects the internal state */
 141        struct timer_list t1;   /* Retransmit timer for SABM and UA */
 142        int retries;
 143        /* Uplink tty if active */
 144        struct tty_port port;   /* The tty bound to this DLCI if there is one */
 145        struct kfifo *fifo;     /* Queue fifo for the DLCI */
 146        struct kfifo _fifo;     /* For new fifo API porting only */
 147        int adaption;           /* Adaption layer in use */
 148        int prev_adaption;
 149        u32 modem_rx;           /* Our incoming virtual modem lines */
 150        u32 modem_tx;           /* Our outgoing modem lines */
 151        int dead;               /* Refuse re-open */
 152        /* Flow control */
 153        int throttled;          /* Private copy of throttle state */
 154        int constipated;        /* Throttle status for outgoing */
 155        /* Packetised I/O */
 156        struct sk_buff *skb;    /* Frame being sent */
 157        struct sk_buff_head skb_list;   /* Queued frames */
 158        /* Data handling callback */
 159        void (*data)(struct gsm_dlci *dlci, u8 *data, int len);
 160        void (*prev_data)(struct gsm_dlci *dlci, u8 *data, int len);
 161        struct net_device *net; /* network interface, if created */
 162};
 163
 164/* DLCI 0, 62/63 are special or reseved see gsmtty_open */
 165
 166#define NUM_DLCI                64
 167
 168/*
 169 *      DLCI 0 is used to pass control blocks out of band of the data
 170 *      flow (and with a higher link priority). One command can be outstanding
 171 *      at a time and we use this structure to manage them. They are created
 172 *      and destroyed by the user context, and updated by the receive paths
 173 *      and timers
 174 */
 175
 176struct gsm_control {
 177        u8 cmd;         /* Command we are issuing */
 178        u8 *data;       /* Data for the command in case we retransmit */
 179        int len;        /* Length of block for retransmission */
 180        int done;       /* Done flag */
 181        int error;      /* Error if any */
 182};
 183
 184/*
 185 *      Each GSM mux we have is represented by this structure. If we are
 186 *      operating as an ldisc then we use this structure as our ldisc
 187 *      state. We need to sort out lifetimes and locking with respect
 188 *      to the gsm mux array. For now we don't free DLCI objects that
 189 *      have been instantiated until the mux itself is terminated.
 190 *
 191 *      To consider further: tty open versus mux shutdown.
 192 */
 193
 194struct gsm_mux {
 195        struct tty_struct *tty;         /* The tty our ldisc is bound to */
 196        spinlock_t lock;
 197        unsigned int num;
 198        struct kref ref;
 199
 200        /* Events on the GSM channel */
 201        wait_queue_head_t event;
 202
 203        /* Bits for GSM mode decoding */
 204
 205        /* Framing Layer */
 206        unsigned char *buf;
 207        int state;
 208#define GSM_SEARCH              0
 209#define GSM_START               1
 210#define GSM_ADDRESS             2
 211#define GSM_CONTROL             3
 212#define GSM_LEN                 4
 213#define GSM_DATA                5
 214#define GSM_FCS                 6
 215#define GSM_OVERRUN             7
 216#define GSM_LEN0                8
 217#define GSM_LEN1                9
 218#define GSM_SSOF                10
 219        unsigned int len;
 220        unsigned int address;
 221        unsigned int count;
 222        int escape;
 223        int encoding;
 224        u8 control;
 225        u8 fcs;
 226        u8 received_fcs;
 227        u8 *txframe;                    /* TX framing buffer */
 228
 229        /* Methods for the receiver side */
 230        void (*receive)(struct gsm_mux *gsm, u8 ch);
 231        void (*error)(struct gsm_mux *gsm, u8 ch, u8 flag);
 232        /* And transmit side */
 233        int (*output)(struct gsm_mux *mux, u8 *data, int len);
 234
 235        /* Link Layer */
 236        unsigned int mru;
 237        unsigned int mtu;
 238        int initiator;                  /* Did we initiate connection */
 239        int dead;                       /* Has the mux been shut down */
 240        struct gsm_dlci *dlci[NUM_DLCI];
 241        int constipated;                /* Asked by remote to shut up */
 242
 243        spinlock_t tx_lock;
 244        unsigned int tx_bytes;          /* TX data outstanding */
 245#define TX_THRESH_HI            8192
 246#define TX_THRESH_LO            2048
 247        struct list_head tx_list;       /* Pending data packets */
 248
 249        /* Control messages */
 250        struct timer_list t2_timer;     /* Retransmit timer for commands */
 251        int cretries;                   /* Command retry counter */
 252        struct gsm_control *pending_cmd;/* Our current pending command */
 253        spinlock_t control_lock;        /* Protects the pending command */
 254
 255        /* Configuration */
 256        int adaption;           /* 1 or 2 supported */
 257        u8 ftype;               /* UI or UIH */
 258        int t1, t2;             /* Timers in 1/100th of a sec */
 259        int n2;                 /* Retry count */
 260
 261        /* Statistics (not currently exposed) */
 262        unsigned long bad_fcs;
 263        unsigned long malformed;
 264        unsigned long io_error;
 265        unsigned long bad_size;
 266        unsigned long unsupported;
 267};
 268
 269
 270/*
 271 *      Mux objects - needed so that we can translate a tty index into the
 272 *      relevant mux and DLCI.
 273 */
 274
 275#define MAX_MUX         4                       /* 256 minors */
 276static struct gsm_mux *gsm_mux[MAX_MUX];        /* GSM muxes */
 277static spinlock_t gsm_mux_lock;
 278
 279static struct tty_driver *gsm_tty_driver;
 280
 281/*
 282 *      This section of the driver logic implements the GSM encodings
 283 *      both the basic and the 'advanced'. Reliable transport is not
 284 *      supported.
 285 */
 286
 287#define CR                      0x02
 288#define EA                      0x01
 289#define PF                      0x10
 290
 291/* I is special: the rest are ..*/
 292#define RR                      0x01
 293#define UI                      0x03
 294#define RNR                     0x05
 295#define REJ                     0x09
 296#define DM                      0x0F
 297#define SABM                    0x2F
 298#define DISC                    0x43
 299#define UA                      0x63
 300#define UIH                     0xEF
 301
 302/* Channel commands */
 303#define CMD_NSC                 0x09
 304#define CMD_TEST                0x11
 305#define CMD_PSC                 0x21
 306#define CMD_RLS                 0x29
 307#define CMD_FCOFF               0x31
 308#define CMD_PN                  0x41
 309#define CMD_RPN                 0x49
 310#define CMD_FCON                0x51
 311#define CMD_CLD                 0x61
 312#define CMD_SNC                 0x69
 313#define CMD_MSC                 0x71
 314
 315/* Virtual modem bits */
 316#define MDM_FC                  0x01
 317#define MDM_RTC                 0x02
 318#define MDM_RTR                 0x04
 319#define MDM_IC                  0x20
 320#define MDM_DV                  0x40
 321
 322#define GSM0_SOF                0xF9
 323#define GSM1_SOF                0x7E
 324#define GSM1_ESCAPE             0x7D
 325#define GSM1_ESCAPE_BITS        0x20
 326#define XON                     0x11
 327#define XOFF                    0x13
 328
 329static const struct tty_port_operations gsm_port_ops;
 330
 331/*
 332 *      CRC table for GSM 0710
 333 */
 334
 335static const u8 gsm_fcs8[256] = {
 336        0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75,
 337        0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
 338        0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69,
 339        0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
 340        0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D,
 341        0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
 342        0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51,
 343        0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
 344        0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05,
 345        0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
 346        0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19,
 347        0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
 348        0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D,
 349        0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
 350        0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21,
 351        0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
 352        0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95,
 353        0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
 354        0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89,
 355        0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
 356        0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD,
 357        0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
 358        0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1,
 359        0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
 360        0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5,
 361        0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
 362        0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9,
 363        0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
 364        0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD,
 365        0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
 366        0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1,
 367        0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF
 368};
 369
 370#define INIT_FCS        0xFF
 371#define GOOD_FCS        0xCF
 372
 373/**
 374 *      gsm_fcs_add     -       update FCS
 375 *      @fcs: Current FCS
 376 *      @c: Next data
 377 *
 378 *      Update the FCS to include c. Uses the algorithm in the specification
 379 *      notes.
 380 */
 381
 382static inline u8 gsm_fcs_add(u8 fcs, u8 c)
 383{
 384        return gsm_fcs8[fcs ^ c];
 385}
 386
 387/**
 388 *      gsm_fcs_add_block       -       update FCS for a block
 389 *      @fcs: Current FCS
 390 *      @c: buffer of data
 391 *      @len: length of buffer
 392 *
 393 *      Update the FCS to include c. Uses the algorithm in the specification
 394 *      notes.
 395 */
 396
 397static inline u8 gsm_fcs_add_block(u8 fcs, u8 *c, int len)
 398{
 399        while (len--)
 400                fcs = gsm_fcs8[fcs ^ *c++];
 401        return fcs;
 402}
 403
 404/**
 405 *      gsm_read_ea             -       read a byte into an EA
 406 *      @val: variable holding value
 407 *      c: byte going into the EA
 408 *
 409 *      Processes one byte of an EA. Updates the passed variable
 410 *      and returns 1 if the EA is now completely read
 411 */
 412
 413static int gsm_read_ea(unsigned int *val, u8 c)
 414{
 415        /* Add the next 7 bits into the value */
 416        *val <<= 7;
 417        *val |= c >> 1;
 418        /* Was this the last byte of the EA 1 = yes*/
 419        return c & EA;
 420}
 421
 422/**
 423 *      gsm_encode_modem        -       encode modem data bits
 424 *      @dlci: DLCI to encode from
 425 *
 426 *      Returns the correct GSM encoded modem status bits (6 bit field) for
 427 *      the current status of the DLCI and attached tty object
 428 */
 429
 430static u8 gsm_encode_modem(const struct gsm_dlci *dlci)
 431{
 432        u8 modembits = 0;
 433        /* FC is true flow control not modem bits */
 434        if (dlci->throttled)
 435                modembits |= MDM_FC;
 436        if (dlci->modem_tx & TIOCM_DTR)
 437                modembits |= MDM_RTC;
 438        if (dlci->modem_tx & TIOCM_RTS)
 439                modembits |= MDM_RTR;
 440        if (dlci->modem_tx & TIOCM_RI)
 441                modembits |= MDM_IC;
 442        if (dlci->modem_tx & TIOCM_CD)
 443                modembits |= MDM_DV;
 444        return modembits;
 445}
 446
 447/**
 448 *      gsm_print_packet        -       display a frame for debug
 449 *      @hdr: header to print before decode
 450 *      @addr: address EA from the frame
 451 *      @cr: C/R bit from the frame
 452 *      @control: control including PF bit
 453 *      @data: following data bytes
 454 *      @dlen: length of data
 455 *
 456 *      Displays a packet in human readable format for debugging purposes. The
 457 *      style is based on amateur radio LAP-B dump display.
 458 */
 459
 460static void gsm_print_packet(const char *hdr, int addr, int cr,
 461                                        u8 control, const u8 *data, int dlen)
 462{
 463        if (!(debug & 1))
 464                return;
 465
 466        pr_info("%s %d) %c: ", hdr, addr, "RC"[cr]);
 467
 468        switch (control & ~PF) {
 469        case SABM:
 470                pr_cont("SABM");
 471                break;
 472        case UA:
 473                pr_cont("UA");
 474                break;
 475        case DISC:
 476                pr_cont("DISC");
 477                break;
 478        case DM:
 479                pr_cont("DM");
 480                break;
 481        case UI:
 482                pr_cont("UI");
 483                break;
 484        case UIH:
 485                pr_cont("UIH");
 486                break;
 487        default:
 488                if (!(control & 0x01)) {
 489                        pr_cont("I N(S)%d N(R)%d",
 490                                (control & 0x0E) >> 1, (control & 0xE0) >> 5);
 491                } else switch (control & 0x0F) {
 492                        case RR:
 493                                pr_cont("RR(%d)", (control & 0xE0) >> 5);
 494                                break;
 495                        case RNR:
 496                                pr_cont("RNR(%d)", (control & 0xE0) >> 5);
 497                                break;
 498                        case REJ:
 499                                pr_cont("REJ(%d)", (control & 0xE0) >> 5);
 500                                break;
 501                        default:
 502                                pr_cont("[%02X]", control);
 503                }
 504        }
 505
 506        if (control & PF)
 507                pr_cont("(P)");
 508        else
 509                pr_cont("(F)");
 510
 511        if (dlen) {
 512                int ct = 0;
 513                while (dlen--) {
 514                        if (ct % 8 == 0) {
 515                                pr_cont("\n");
 516                                pr_debug("    ");
 517                        }
 518                        pr_cont("%02X ", *data++);
 519                        ct++;
 520                }
 521        }
 522        pr_cont("\n");
 523}
 524
 525
 526/*
 527 *      Link level transmission side
 528 */
 529
 530/**
 531 *      gsm_stuff_packet        -       bytestuff a packet
 532 *      @ibuf: input
 533 *      @obuf: output
 534 *      @len: length of input
 535 *
 536 *      Expand a buffer by bytestuffing it. The worst case size change
 537 *      is doubling and the caller is responsible for handing out
 538 *      suitable sized buffers.
 539 */
 540
 541static int gsm_stuff_frame(const u8 *input, u8 *output, int len)
 542{
 543        int olen = 0;
 544        while (len--) {
 545                if (*input == GSM1_SOF || *input == GSM1_ESCAPE
 546                    || *input == XON || *input == XOFF) {
 547                        *output++ = GSM1_ESCAPE;
 548                        *output++ = *input++ ^ GSM1_ESCAPE_BITS;
 549                        olen++;
 550                } else
 551                        *output++ = *input++;
 552                olen++;
 553        }
 554        return olen;
 555}
 556
 557/**
 558 *      gsm_send        -       send a control frame
 559 *      @gsm: our GSM mux
 560 *      @addr: address for control frame
 561 *      @cr: command/response bit
 562 *      @control:  control byte including PF bit
 563 *
 564 *      Format up and transmit a control frame. These do not go via the
 565 *      queueing logic as they should be transmitted ahead of data when
 566 *      they are needed.
 567 *
 568 *      FIXME: Lock versus data TX path
 569 */
 570
 571static void gsm_send(struct gsm_mux *gsm, int addr, int cr, int control)
 572{
 573        int len;
 574        u8 cbuf[10];
 575        u8 ibuf[3];
 576
 577        switch (gsm->encoding) {
 578        case 0:
 579                cbuf[0] = GSM0_SOF;
 580                cbuf[1] = (addr << 2) | (cr << 1) | EA;
 581                cbuf[2] = control;
 582                cbuf[3] = EA;   /* Length of data = 0 */
 583                cbuf[4] = 0xFF - gsm_fcs_add_block(INIT_FCS, cbuf + 1, 3);
 584                cbuf[5] = GSM0_SOF;
 585                len = 6;
 586                break;
 587        case 1:
 588        case 2:
 589                /* Control frame + packing (but not frame stuffing) in mode 1 */
 590                ibuf[0] = (addr << 2) | (cr << 1) | EA;
 591                ibuf[1] = control;
 592                ibuf[2] = 0xFF - gsm_fcs_add_block(INIT_FCS, ibuf, 2);
 593                /* Stuffing may double the size worst case */
 594                len = gsm_stuff_frame(ibuf, cbuf + 1, 3);
 595                /* Now add the SOF markers */
 596                cbuf[0] = GSM1_SOF;
 597                cbuf[len + 1] = GSM1_SOF;
 598                /* FIXME: we can omit the lead one in many cases */
 599                len += 2;
 600                break;
 601        default:
 602                WARN_ON(1);
 603                return;
 604        }
 605        gsm->output(gsm, cbuf, len);
 606        gsm_print_packet("-->", addr, cr, control, NULL, 0);
 607}
 608
 609/**
 610 *      gsm_response    -       send a control response
 611 *      @gsm: our GSM mux
 612 *      @addr: address for control frame
 613 *      @control:  control byte including PF bit
 614 *
 615 *      Format up and transmit a link level response frame.
 616 */
 617
 618static inline void gsm_response(struct gsm_mux *gsm, int addr, int control)
 619{
 620        gsm_send(gsm, addr, 0, control);
 621}
 622
 623/**
 624 *      gsm_command     -       send a control command
 625 *      @gsm: our GSM mux
 626 *      @addr: address for control frame
 627 *      @control:  control byte including PF bit
 628 *
 629 *      Format up and transmit a link level command frame.
 630 */
 631
 632static inline void gsm_command(struct gsm_mux *gsm, int addr, int control)
 633{
 634        gsm_send(gsm, addr, 1, control);
 635}
 636
 637/* Data transmission */
 638
 639#define HDR_LEN         6       /* ADDR CTRL [LEN.2] DATA FCS */
 640
 641/**
 642 *      gsm_data_alloc          -       allocate data frame
 643 *      @gsm: GSM mux
 644 *      @addr: DLCI address
 645 *      @len: length excluding header and FCS
 646 *      @ctrl: control byte
 647 *
 648 *      Allocate a new data buffer for sending frames with data. Space is left
 649 *      at the front for header bytes but that is treated as an implementation
 650 *      detail and not for the high level code to use
 651 */
 652
 653static struct gsm_msg *gsm_data_alloc(struct gsm_mux *gsm, u8 addr, int len,
 654                                                                u8 ctrl)
 655{
 656        struct gsm_msg *m = kmalloc(sizeof(struct gsm_msg) + len + HDR_LEN,
 657                                                                GFP_ATOMIC);
 658        if (m == NULL)
 659                return NULL;
 660        m->data = m->buffer + HDR_LEN - 1;      /* Allow for FCS */
 661        m->len = len;
 662        m->addr = addr;
 663        m->ctrl = ctrl;
 664        INIT_LIST_HEAD(&m->list);
 665        return m;
 666}
 667
 668/**
 669 *      gsm_data_kick           -       poke the queue
 670 *      @gsm: GSM Mux
 671 *
 672 *      The tty device has called us to indicate that room has appeared in
 673 *      the transmit queue. Ram more data into the pipe if we have any
 674 *      If we have been flow-stopped by a CMD_FCOFF, then we can only
 675 *      send messages on DLCI0 until CMD_FCON
 676 *
 677 *      FIXME: lock against link layer control transmissions
 678 */
 679
 680static void gsm_data_kick(struct gsm_mux *gsm)
 681{
 682        struct gsm_msg *msg, *nmsg;
 683        int len;
 684        int skip_sof = 0;
 685
 686        list_for_each_entry_safe(msg, nmsg, &gsm->tx_list, list) {
 687                if (gsm->constipated && msg->addr)
 688                        continue;
 689                if (gsm->encoding != 0) {
 690                        gsm->txframe[0] = GSM1_SOF;
 691                        len = gsm_stuff_frame(msg->data,
 692                                                gsm->txframe + 1, msg->len);
 693                        gsm->txframe[len + 1] = GSM1_SOF;
 694                        len += 2;
 695                } else {
 696                        gsm->txframe[0] = GSM0_SOF;
 697                        memcpy(gsm->txframe + 1 , msg->data, msg->len);
 698                        gsm->txframe[msg->len + 1] = GSM0_SOF;
 699                        len = msg->len + 2;
 700                }
 701
 702                if (debug & 4)
 703                        print_hex_dump_bytes("gsm_data_kick: ",
 704                                             DUMP_PREFIX_OFFSET,
 705                                             gsm->txframe, len);
 706
 707                if (gsm->output(gsm, gsm->txframe + skip_sof,
 708                                                len - skip_sof) < 0)
 709                        break;
 710                /* FIXME: Can eliminate one SOF in many more cases */
 711                gsm->tx_bytes -= msg->len;
 712                /* For a burst of frames skip the extra SOF within the
 713                   burst */
 714                skip_sof = 1;
 715
 716                list_del(&msg->list);
 717                kfree(msg);
 718        }
 719}
 720
 721/**
 722 *      __gsm_data_queue                -       queue a UI or UIH frame
 723 *      @dlci: DLCI sending the data
 724 *      @msg: message queued
 725 *
 726 *      Add data to the transmit queue and try and get stuff moving
 727 *      out of the mux tty if not already doing so. The Caller must hold
 728 *      the gsm tx lock.
 729 */
 730
 731static void __gsm_data_queue(struct gsm_dlci *dlci, struct gsm_msg *msg)
 732{
 733        struct gsm_mux *gsm = dlci->gsm;
 734        u8 *dp = msg->data;
 735        u8 *fcs = dp + msg->len;
 736
 737        /* Fill in the header */
 738        if (gsm->encoding == 0) {
 739                if (msg->len < 128)
 740                        *--dp = (msg->len << 1) | EA;
 741                else {
 742                        *--dp = (msg->len >> 7);        /* bits 7 - 15 */
 743                        *--dp = (msg->len & 127) << 1;  /* bits 0 - 6 */
 744                }
 745        }
 746
 747        *--dp = msg->ctrl;
 748        if (gsm->initiator)
 749                *--dp = (msg->addr << 2) | 2 | EA;
 750        else
 751                *--dp = (msg->addr << 2) | EA;
 752        *fcs = gsm_fcs_add_block(INIT_FCS, dp , msg->data - dp);
 753        /* Ugly protocol layering violation */
 754        if (msg->ctrl == UI || msg->ctrl == (UI|PF))
 755                *fcs = gsm_fcs_add_block(*fcs, msg->data, msg->len);
 756        *fcs = 0xFF - *fcs;
 757
 758        gsm_print_packet("Q> ", msg->addr, gsm->initiator, msg->ctrl,
 759                                                        msg->data, msg->len);
 760
 761        /* Move the header back and adjust the length, also allow for the FCS
 762           now tacked on the end */
 763        msg->len += (msg->data - dp) + 1;
 764        msg->data = dp;
 765
 766        /* Add to the actual output queue */
 767        list_add_tail(&msg->list, &gsm->tx_list);
 768        gsm->tx_bytes += msg->len;
 769        gsm_data_kick(gsm);
 770}
 771
 772/**
 773 *      gsm_data_queue          -       queue a UI or UIH frame
 774 *      @dlci: DLCI sending the data
 775 *      @msg: message queued
 776 *
 777 *      Add data to the transmit queue and try and get stuff moving
 778 *      out of the mux tty if not already doing so. Take the
 779 *      the gsm tx lock and dlci lock.
 780 */
 781
 782static void gsm_data_queue(struct gsm_dlci *dlci, struct gsm_msg *msg)
 783{
 784        unsigned long flags;
 785        spin_lock_irqsave(&dlci->gsm->tx_lock, flags);
 786        __gsm_data_queue(dlci, msg);
 787        spin_unlock_irqrestore(&dlci->gsm->tx_lock, flags);
 788}
 789
 790/**
 791 *      gsm_dlci_data_output    -       try and push data out of a DLCI
 792 *      @gsm: mux
 793 *      @dlci: the DLCI to pull data from
 794 *
 795 *      Pull data from a DLCI and send it into the transmit queue if there
 796 *      is data. Keep to the MRU of the mux. This path handles the usual tty
 797 *      interface which is a byte stream with optional modem data.
 798 *
 799 *      Caller must hold the tx_lock of the mux.
 800 */
 801
 802static int gsm_dlci_data_output(struct gsm_mux *gsm, struct gsm_dlci *dlci)
 803{
 804        struct gsm_msg *msg;
 805        u8 *dp;
 806        int len, total_size, size;
 807        int h = dlci->adaption - 1;
 808
 809        total_size = 0;
 810        while(1) {
 811                len = kfifo_len(dlci->fifo);
 812                if (len == 0)
 813                        return total_size;
 814
 815                /* MTU/MRU count only the data bits */
 816                if (len > gsm->mtu)
 817                        len = gsm->mtu;
 818
 819                size = len + h;
 820
 821                msg = gsm_data_alloc(gsm, dlci->addr, size, gsm->ftype);
 822                /* FIXME: need a timer or something to kick this so it can't
 823                   get stuck with no work outstanding and no buffer free */
 824                if (msg == NULL)
 825                        return -ENOMEM;
 826                dp = msg->data;
 827                switch (dlci->adaption) {
 828                case 1: /* Unstructured */
 829                        break;
 830                case 2: /* Unstructed with modem bits. Always one byte as we never
 831                           send inline break data */
 832                        *dp++ = gsm_encode_modem(dlci);
 833                        break;
 834                }
 835                WARN_ON(kfifo_out_locked(dlci->fifo, dp , len, &dlci->lock) != len);
 836                __gsm_data_queue(dlci, msg);
 837                total_size += size;
 838        }
 839        /* Bytes of data we used up */
 840        return total_size;
 841}
 842
 843/**
 844 *      gsm_dlci_data_output_framed  -  try and push data out of a DLCI
 845 *      @gsm: mux
 846 *      @dlci: the DLCI to pull data from
 847 *
 848 *      Pull data from a DLCI and send it into the transmit queue if there
 849 *      is data. Keep to the MRU of the mux. This path handles framed data
 850 *      queued as skbuffs to the DLCI.
 851 *
 852 *      Caller must hold the tx_lock of the mux.
 853 */
 854
 855static int gsm_dlci_data_output_framed(struct gsm_mux *gsm,
 856                                                struct gsm_dlci *dlci)
 857{
 858        struct gsm_msg *msg;
 859        u8 *dp;
 860        int len, size;
 861        int last = 0, first = 0;
 862        int overhead = 0;
 863
 864        /* One byte per frame is used for B/F flags */
 865        if (dlci->adaption == 4)
 866                overhead = 1;
 867
 868        /* dlci->skb is locked by tx_lock */
 869        if (dlci->skb == NULL) {
 870                dlci->skb = skb_dequeue_tail(&dlci->skb_list);
 871                if (dlci->skb == NULL)
 872                        return 0;
 873                first = 1;
 874        }
 875        len = dlci->skb->len + overhead;
 876
 877        /* MTU/MRU count only the data bits */
 878        if (len > gsm->mtu) {
 879                if (dlci->adaption == 3) {
 880                        /* Over long frame, bin it */
 881                        dev_kfree_skb_any(dlci->skb);
 882                        dlci->skb = NULL;
 883                        return 0;
 884                }
 885                len = gsm->mtu;
 886        } else
 887                last = 1;
 888
 889        size = len + overhead;
 890        msg = gsm_data_alloc(gsm, dlci->addr, size, gsm->ftype);
 891
 892        /* FIXME: need a timer or something to kick this so it can't
 893           get stuck with no work outstanding and no buffer free */
 894        if (msg == NULL) {
 895                skb_queue_tail(&dlci->skb_list, dlci->skb);
 896                dlci->skb = NULL;
 897                return -ENOMEM;
 898        }
 899        dp = msg->data;
 900
 901        if (dlci->adaption == 4) { /* Interruptible framed (Packetised Data) */
 902                /* Flag byte to carry the start/end info */
 903                *dp++ = last << 7 | first << 6 | 1;     /* EA */
 904                len--;
 905        }
 906        memcpy(dp, dlci->skb->data, len);
 907        skb_pull(dlci->skb, len);
 908        __gsm_data_queue(dlci, msg);
 909        if (last) {
 910                dev_kfree_skb_any(dlci->skb);
 911                dlci->skb = NULL;
 912        }
 913        return size;
 914}
 915
 916/**
 917 *      gsm_dlci_data_sweep             -       look for data to send
 918 *      @gsm: the GSM mux
 919 *
 920 *      Sweep the GSM mux channels in priority order looking for ones with
 921 *      data to send. We could do with optimising this scan a bit. We aim
 922 *      to fill the queue totally or up to TX_THRESH_HI bytes. Once we hit
 923 *      TX_THRESH_LO we get called again
 924 *
 925 *      FIXME: We should round robin between groups and in theory you can
 926 *      renegotiate DLCI priorities with optional stuff. Needs optimising.
 927 */
 928
 929static void gsm_dlci_data_sweep(struct gsm_mux *gsm)
 930{
 931        int len;
 932        /* Priority ordering: We should do priority with RR of the groups */
 933        int i = 1;
 934
 935        while (i < NUM_DLCI) {
 936                struct gsm_dlci *dlci;
 937
 938                if (gsm->tx_bytes > TX_THRESH_HI)
 939                        break;
 940                dlci = gsm->dlci[i];
 941                if (dlci == NULL || dlci->constipated) {
 942                        i++;
 943                        continue;
 944                }
 945                if (dlci->adaption < 3 && !dlci->net)
 946                        len = gsm_dlci_data_output(gsm, dlci);
 947                else
 948                        len = gsm_dlci_data_output_framed(gsm, dlci);
 949                if (len < 0)
 950                        break;
 951                /* DLCI empty - try the next */
 952                if (len == 0)
 953                        i++;
 954        }
 955}
 956
 957/**
 958 *      gsm_dlci_data_kick      -       transmit if possible
 959 *      @dlci: DLCI to kick
 960 *
 961 *      Transmit data from this DLCI if the queue is empty. We can't rely on
 962 *      a tty wakeup except when we filled the pipe so we need to fire off
 963 *      new data ourselves in other cases.
 964 */
 965
 966static void gsm_dlci_data_kick(struct gsm_dlci *dlci)
 967{
 968        unsigned long flags;
 969        int sweep;
 970
 971        if (dlci->constipated) 
 972                return;
 973
 974        spin_lock_irqsave(&dlci->gsm->tx_lock, flags);
 975        /* If we have nothing running then we need to fire up */
 976        sweep = (dlci->gsm->tx_bytes < TX_THRESH_LO);
 977        if (dlci->gsm->tx_bytes == 0) {
 978                if (dlci->net)
 979                        gsm_dlci_data_output_framed(dlci->gsm, dlci);
 980                else
 981                        gsm_dlci_data_output(dlci->gsm, dlci);
 982        }
 983        if (sweep)
 984                gsm_dlci_data_sweep(dlci->gsm);
 985        spin_unlock_irqrestore(&dlci->gsm->tx_lock, flags);
 986}
 987
 988/*
 989 *      Control message processing
 990 */
 991
 992
 993/**
 994 *      gsm_control_reply       -       send a response frame to a control
 995 *      @gsm: gsm channel
 996 *      @cmd: the command to use
 997 *      @data: data to follow encoded info
 998 *      @dlen: length of data
 999 *
1000 *      Encode up and queue a UI/UIH frame containing our response.
1001 */
1002
1003static void gsm_control_reply(struct gsm_mux *gsm, int cmd, u8 *data,
1004                                        int dlen)
1005{
1006        struct gsm_msg *msg;
1007        msg = gsm_data_alloc(gsm, 0, dlen + 2, gsm->ftype);
1008        if (msg == NULL)
1009                return;
1010        msg->data[0] = (cmd & 0xFE) << 1 | EA;  /* Clear C/R */
1011        msg->data[1] = (dlen << 1) | EA;
1012        memcpy(msg->data + 2, data, dlen);
1013        gsm_data_queue(gsm->dlci[0], msg);
1014}
1015
1016/**
1017 *      gsm_process_modem       -       process received modem status
1018 *      @tty: virtual tty bound to the DLCI
1019 *      @dlci: DLCI to affect
1020 *      @modem: modem bits (full EA)
1021 *
1022 *      Used when a modem control message or line state inline in adaption
1023 *      layer 2 is processed. Sort out the local modem state and throttles
1024 */
1025
1026static void gsm_process_modem(struct tty_struct *tty, struct gsm_dlci *dlci,
1027                                                        u32 modem, int clen)
1028{
1029        int  mlines = 0;
1030        u8 brk = 0;
1031        int fc;
1032
1033        /* The modem status command can either contain one octet (v.24 signals)
1034           or two octets (v.24 signals + break signals). The length field will
1035           either be 2 or 3 respectively. This is specified in section
1036           5.4.6.3.7 of the  27.010 mux spec. */
1037
1038        if (clen == 2)
1039                modem = modem & 0x7f;
1040        else {
1041                brk = modem & 0x7f;
1042                modem = (modem >> 7) & 0x7f;
1043        }
1044
1045        /* Flow control/ready to communicate */
1046        fc = (modem & MDM_FC) || !(modem & MDM_RTR);
1047        if (fc && !dlci->constipated) {
1048                /* Need to throttle our output on this device */
1049                dlci->constipated = 1;
1050        } else if (!fc && dlci->constipated) {
1051                dlci->constipated = 0;
1052                gsm_dlci_data_kick(dlci);
1053        }
1054
1055        /* Map modem bits */
1056        if (modem & MDM_RTC)
1057                mlines |= TIOCM_DSR | TIOCM_DTR;
1058        if (modem & MDM_RTR)
1059                mlines |= TIOCM_RTS | TIOCM_CTS;
1060        if (modem & MDM_IC)
1061                mlines |= TIOCM_RI;
1062        if (modem & MDM_DV)
1063                mlines |= TIOCM_CD;
1064
1065        /* Carrier drop -> hangup */
1066        if (tty) {
1067                if ((mlines & TIOCM_CD) == 0 && (dlci->modem_rx & TIOCM_CD))
1068                        if (!(tty->termios.c_cflag & CLOCAL))
1069                                tty_hangup(tty);
1070                if (brk & 0x01)
1071                        tty_insert_flip_char(tty, 0, TTY_BREAK);
1072        }
1073        dlci->modem_rx = mlines;
1074}
1075
1076/**
1077 *      gsm_control_modem       -       modem status received
1078 *      @gsm: GSM channel
1079 *      @data: data following command
1080 *      @clen: command length
1081 *
1082 *      We have received a modem status control message. This is used by
1083 *      the GSM mux protocol to pass virtual modem line status and optionally
1084 *      to indicate break signals. Unpack it, convert to Linux representation
1085 *      and if need be stuff a break message down the tty.
1086 */
1087
1088static void gsm_control_modem(struct gsm_mux *gsm, u8 *data, int clen)
1089{
1090        unsigned int addr = 0;
1091        unsigned int modem = 0;
1092        struct gsm_dlci *dlci;
1093        int len = clen;
1094        u8 *dp = data;
1095        struct tty_struct *tty;
1096
1097        while (gsm_read_ea(&addr, *dp++) == 0) {
1098                len--;
1099                if (len == 0)
1100                        return;
1101        }
1102        /* Must be at least one byte following the EA */
1103        len--;
1104        if (len <= 0)
1105                return;
1106
1107        addr >>= 1;
1108        /* Closed port, or invalid ? */
1109        if (addr == 0 || addr >= NUM_DLCI || gsm->dlci[addr] == NULL)
1110                return;
1111        dlci = gsm->dlci[addr];
1112
1113        while (gsm_read_ea(&modem, *dp++) == 0) {
1114                len--;
1115                if (len == 0)
1116                        return;
1117        }
1118        tty = tty_port_tty_get(&dlci->port);
1119        gsm_process_modem(tty, dlci, modem, clen);
1120        if (tty) {
1121                tty_wakeup(tty);
1122                tty_kref_put(tty);
1123        }
1124        gsm_control_reply(gsm, CMD_MSC, data, clen);
1125}
1126
1127/**
1128 *      gsm_control_rls         -       remote line status
1129 *      @gsm: GSM channel
1130 *      @data: data bytes
1131 *      @clen: data length
1132 *
1133 *      The modem sends us a two byte message on the control channel whenever
1134 *      it wishes to send us an error state from the virtual link. Stuff
1135 *      this into the uplink tty if present
1136 */
1137
1138static void gsm_control_rls(struct gsm_mux *gsm, u8 *data, int clen)
1139{
1140        struct tty_struct *tty;
1141        unsigned int addr = 0 ;
1142        u8 bits;
1143        int len = clen;
1144        u8 *dp = data;
1145
1146        while (gsm_read_ea(&addr, *dp++) == 0) {
1147                len--;
1148                if (len == 0)
1149                        return;
1150        }
1151        /* Must be at least one byte following ea */
1152        len--;
1153        if (len <= 0)
1154                return;
1155        addr >>= 1;
1156        /* Closed port, or invalid ? */
1157        if (addr == 0 || addr >= NUM_DLCI || gsm->dlci[addr] == NULL)
1158                return;
1159        /* No error ? */
1160        bits = *dp;
1161        if ((bits & 1) == 0)
1162                return;
1163        /* See if we have an uplink tty */
1164        tty = tty_port_tty_get(&gsm->dlci[addr]->port);
1165
1166        if (tty) {
1167                if (bits & 2)
1168                        tty_insert_flip_char(tty, 0, TTY_OVERRUN);
1169                if (bits & 4)
1170                        tty_insert_flip_char(tty, 0, TTY_PARITY);
1171                if (bits & 8)
1172                        tty_insert_flip_char(tty, 0, TTY_FRAME);
1173                tty_flip_buffer_push(tty);
1174                tty_kref_put(tty);
1175        }
1176        gsm_control_reply(gsm, CMD_RLS, data, clen);
1177}
1178
1179static void gsm_dlci_begin_close(struct gsm_dlci *dlci);
1180
1181/**
1182 *      gsm_control_message     -       DLCI 0 control processing
1183 *      @gsm: our GSM mux
1184 *      @command:  the command EA
1185 *      @data: data beyond the command/length EAs
1186 *      @clen: length
1187 *
1188 *      Input processor for control messages from the other end of the link.
1189 *      Processes the incoming request and queues a response frame or an
1190 *      NSC response if not supported
1191 */
1192
1193static void gsm_control_message(struct gsm_mux *gsm, unsigned int command,
1194                                                        u8 *data, int clen)
1195{
1196        u8 buf[1];
1197        unsigned long flags;
1198
1199        switch (command) {
1200        case CMD_CLD: {
1201                struct gsm_dlci *dlci = gsm->dlci[0];
1202                /* Modem wishes to close down */
1203                if (dlci) {
1204                        dlci->dead = 1;
1205                        gsm->dead = 1;
1206                        gsm_dlci_begin_close(dlci);
1207                }
1208                }
1209                break;
1210        case CMD_TEST:
1211                /* Modem wishes to test, reply with the data */
1212                gsm_control_reply(gsm, CMD_TEST, data, clen);
1213                break;
1214        case CMD_FCON:
1215                /* Modem can accept data again */
1216                gsm->constipated = 0;
1217                gsm_control_reply(gsm, CMD_FCON, NULL, 0);
1218                /* Kick the link in case it is idling */
1219                spin_lock_irqsave(&gsm->tx_lock, flags);
1220                gsm_data_kick(gsm);
1221                spin_unlock_irqrestore(&gsm->tx_lock, flags);
1222                break;
1223        case CMD_FCOFF:
1224                /* Modem wants us to STFU */
1225                gsm->constipated = 1;
1226                gsm_control_reply(gsm, CMD_FCOFF, NULL, 0);
1227                break;
1228        case CMD_MSC:
1229                /* Out of band modem line change indicator for a DLCI */
1230                gsm_control_modem(gsm, data, clen);
1231                break;
1232        case CMD_RLS:
1233                /* Out of band error reception for a DLCI */
1234                gsm_control_rls(gsm, data, clen);
1235                break;
1236        case CMD_PSC:
1237                /* Modem wishes to enter power saving state */
1238                gsm_control_reply(gsm, CMD_PSC, NULL, 0);
1239                break;
1240                /* Optional unsupported commands */
1241        case CMD_PN:    /* Parameter negotiation */
1242        case CMD_RPN:   /* Remote port negotiation */
1243        case CMD_SNC:   /* Service negotiation command */
1244        default:
1245                /* Reply to bad commands with an NSC */
1246                buf[0] = command;
1247                gsm_control_reply(gsm, CMD_NSC, buf, 1);
1248                break;
1249        }
1250}
1251
1252/**
1253 *      gsm_control_response    -       process a response to our control
1254 *      @gsm: our GSM mux
1255 *      @command: the command (response) EA
1256 *      @data: data beyond the command/length EA
1257 *      @clen: length
1258 *
1259 *      Process a response to an outstanding command. We only allow a single
1260 *      control message in flight so this is fairly easy. All the clean up
1261 *      is done by the caller, we just update the fields, flag it as done
1262 *      and return
1263 */
1264
1265static void gsm_control_response(struct gsm_mux *gsm, unsigned int command,
1266                                                        u8 *data, int clen)
1267{
1268        struct gsm_control *ctrl;
1269        unsigned long flags;
1270
1271        spin_lock_irqsave(&gsm->control_lock, flags);
1272
1273        ctrl = gsm->pending_cmd;
1274        /* Does the reply match our command */
1275        command |= 1;
1276        if (ctrl != NULL && (command == ctrl->cmd || command == CMD_NSC)) {
1277                /* Our command was replied to, kill the retry timer */
1278                del_timer(&gsm->t2_timer);
1279                gsm->pending_cmd = NULL;
1280                /* Rejected by the other end */
1281                if (command == CMD_NSC)
1282                        ctrl->error = -EOPNOTSUPP;
1283                ctrl->done = 1;
1284                wake_up(&gsm->event);
1285        }
1286        spin_unlock_irqrestore(&gsm->control_lock, flags);
1287}
1288
1289/**
1290 *      gsm_control_transmit    -       send control packet
1291 *      @gsm: gsm mux
1292 *      @ctrl: frame to send
1293 *
1294 *      Send out a pending control command (called under control lock)
1295 */
1296
1297static void gsm_control_transmit(struct gsm_mux *gsm, struct gsm_control *ctrl)
1298{
1299        struct gsm_msg *msg = gsm_data_alloc(gsm, 0, ctrl->len + 1, gsm->ftype);
1300        if (msg == NULL)
1301                return;
1302        msg->data[0] = (ctrl->cmd << 1) | 2 | EA;       /* command */
1303        memcpy(msg->data + 1, ctrl->data, ctrl->len);
1304        gsm_data_queue(gsm->dlci[0], msg);
1305}
1306
1307/**
1308 *      gsm_control_retransmit  -       retransmit a control frame
1309 *      @data: pointer to our gsm object
1310 *
1311 *      Called off the T2 timer expiry in order to retransmit control frames
1312 *      that have been lost in the system somewhere. The control_lock protects
1313 *      us from colliding with another sender or a receive completion event.
1314 *      In that situation the timer may still occur in a small window but
1315 *      gsm->pending_cmd will be NULL and we just let the timer expire.
1316 */
1317
1318static void gsm_control_retransmit(unsigned long data)
1319{
1320        struct gsm_mux *gsm = (struct gsm_mux *)data;
1321        struct gsm_control *ctrl;
1322        unsigned long flags;
1323        spin_lock_irqsave(&gsm->control_lock, flags);
1324        ctrl = gsm->pending_cmd;
1325        if (ctrl) {
1326                gsm->cretries--;
1327                if (gsm->cretries == 0) {
1328                        gsm->pending_cmd = NULL;
1329                        ctrl->error = -ETIMEDOUT;
1330                        ctrl->done = 1;
1331                        spin_unlock_irqrestore(&gsm->control_lock, flags);
1332                        wake_up(&gsm->event);
1333                        return;
1334                }
1335                gsm_control_transmit(gsm, ctrl);
1336                mod_timer(&gsm->t2_timer, jiffies + gsm->t2 * HZ / 100);
1337        }
1338        spin_unlock_irqrestore(&gsm->control_lock, flags);
1339}
1340
1341/**
1342 *      gsm_control_send        -       send a control frame on DLCI 0
1343 *      @gsm: the GSM channel
1344 *      @command: command  to send including CR bit
1345 *      @data: bytes of data (must be kmalloced)
1346 *      @len: length of the block to send
1347 *
1348 *      Queue and dispatch a control command. Only one command can be
1349 *      active at a time. In theory more can be outstanding but the matching
1350 *      gets really complicated so for now stick to one outstanding.
1351 */
1352
1353static struct gsm_control *gsm_control_send(struct gsm_mux *gsm,
1354                unsigned int command, u8 *data, int clen)
1355{
1356        struct gsm_control *ctrl = kzalloc(sizeof(struct gsm_control),
1357                                                GFP_KERNEL);
1358        unsigned long flags;
1359        if (ctrl == NULL)
1360                return NULL;
1361retry:
1362        wait_event(gsm->event, gsm->pending_cmd == NULL);
1363        spin_lock_irqsave(&gsm->control_lock, flags);
1364        if (gsm->pending_cmd != NULL) {
1365                spin_unlock_irqrestore(&gsm->control_lock, flags);
1366                goto retry;
1367        }
1368        ctrl->cmd = command;
1369        ctrl->data = data;
1370        ctrl->len = clen;
1371        gsm->pending_cmd = ctrl;
1372        gsm->cretries = gsm->n2;
1373        mod_timer(&gsm->t2_timer, jiffies + gsm->t2 * HZ / 100);
1374        gsm_control_transmit(gsm, ctrl);
1375        spin_unlock_irqrestore(&gsm->control_lock, flags);
1376        return ctrl;
1377}
1378
1379/**
1380 *      gsm_control_wait        -       wait for a control to finish
1381 *      @gsm: GSM mux
1382 *      @control: control we are waiting on
1383 *
1384 *      Waits for the control to complete or time out. Frees any used
1385 *      resources and returns 0 for success, or an error if the remote
1386 *      rejected or ignored the request.
1387 */
1388
1389static int gsm_control_wait(struct gsm_mux *gsm, struct gsm_control *control)
1390{
1391        int err;
1392        wait_event(gsm->event, control->done == 1);
1393        err = control->error;
1394        kfree(control);
1395        return err;
1396}
1397
1398
1399/*
1400 *      DLCI level handling: Needs krefs
1401 */
1402
1403/*
1404 *      State transitions and timers
1405 */
1406
1407/**
1408 *      gsm_dlci_close          -       a DLCI has closed
1409 *      @dlci: DLCI that closed
1410 *
1411 *      Perform processing when moving a DLCI into closed state. If there
1412 *      is an attached tty this is hung up
1413 */
1414
1415static void gsm_dlci_close(struct gsm_dlci *dlci)
1416{
1417        del_timer(&dlci->t1);
1418        if (debug & 8)
1419                pr_debug("DLCI %d goes closed.\n", dlci->addr);
1420        dlci->state = DLCI_CLOSED;
1421        if (dlci->addr != 0) {
1422                struct tty_struct  *tty = tty_port_tty_get(&dlci->port);
1423                if (tty) {
1424                        tty_hangup(tty);
1425                        tty_kref_put(tty);
1426                }
1427                kfifo_reset(dlci->fifo);
1428        } else
1429                dlci->gsm->dead = 1;
1430        wake_up(&dlci->gsm->event);
1431        /* A DLCI 0 close is a MUX termination so we need to kick that
1432           back to userspace somehow */
1433}
1434
1435/**
1436 *      gsm_dlci_open           -       a DLCI has opened
1437 *      @dlci: DLCI that opened
1438 *
1439 *      Perform processing when moving a DLCI into open state.
1440 */
1441
1442static void gsm_dlci_open(struct gsm_dlci *dlci)
1443{
1444        /* Note that SABM UA .. SABM UA first UA lost can mean that we go
1445           open -> open */
1446        del_timer(&dlci->t1);
1447        /* This will let a tty open continue */
1448        dlci->state = DLCI_OPEN;
1449        if (debug & 8)
1450                pr_debug("DLCI %d goes open.\n", dlci->addr);
1451        wake_up(&dlci->gsm->event);
1452}
1453
1454/**
1455 *      gsm_dlci_t1             -       T1 timer expiry
1456 *      @dlci: DLCI that opened
1457 *
1458 *      The T1 timer handles retransmits of control frames (essentially of
1459 *      SABM and DISC). We resend the command until the retry count runs out
1460 *      in which case an opening port goes back to closed and a closing port
1461 *      is simply put into closed state (any further frames from the other
1462 *      end will get a DM response)
1463 */
1464
1465static void gsm_dlci_t1(unsigned long data)
1466{
1467        struct gsm_dlci *dlci = (struct gsm_dlci *)data;
1468        struct gsm_mux *gsm = dlci->gsm;
1469
1470        switch (dlci->state) {
1471        case DLCI_OPENING:
1472                dlci->retries--;
1473                if (dlci->retries) {
1474                        gsm_command(dlci->gsm, dlci->addr, SABM|PF);
1475                        mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
1476                } else
1477                        gsm_dlci_close(dlci);
1478                break;
1479        case DLCI_CLOSING:
1480                dlci->retries--;
1481                if (dlci->retries) {
1482                        gsm_command(dlci->gsm, dlci->addr, DISC|PF);
1483                        mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
1484                } else
1485                        gsm_dlci_close(dlci);
1486                break;
1487        }
1488}
1489
1490/**
1491 *      gsm_dlci_begin_open     -       start channel open procedure
1492 *      @dlci: DLCI to open
1493 *
1494 *      Commence opening a DLCI from the Linux side. We issue SABM messages
1495 *      to the modem which should then reply with a UA, at which point we
1496 *      will move into open state. Opening is done asynchronously with retry
1497 *      running off timers and the responses.
1498 */
1499
1500static void gsm_dlci_begin_open(struct gsm_dlci *dlci)
1501{
1502        struct gsm_mux *gsm = dlci->gsm;
1503        if (dlci->state == DLCI_OPEN || dlci->state == DLCI_OPENING)
1504                return;
1505        dlci->retries = gsm->n2;
1506        dlci->state = DLCI_OPENING;
1507        gsm_command(dlci->gsm, dlci->addr, SABM|PF);
1508        mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
1509}
1510
1511/**
1512 *      gsm_dlci_begin_close    -       start channel open procedure
1513 *      @dlci: DLCI to open
1514 *
1515 *      Commence closing a DLCI from the Linux side. We issue DISC messages
1516 *      to the modem which should then reply with a UA, at which point we
1517 *      will move into closed state. Closing is done asynchronously with retry
1518 *      off timers. We may also receive a DM reply from the other end which
1519 *      indicates the channel was already closed.
1520 */
1521
1522static void gsm_dlci_begin_close(struct gsm_dlci *dlci)
1523{
1524        struct gsm_mux *gsm = dlci->gsm;
1525        if (dlci->state == DLCI_CLOSED || dlci->state == DLCI_CLOSING)
1526                return;
1527        dlci->retries = gsm->n2;
1528        dlci->state = DLCI_CLOSING;
1529        gsm_command(dlci->gsm, dlci->addr, DISC|PF);
1530        mod_timer(&dlci->t1, jiffies + gsm->t1 * HZ / 100);
1531}
1532
1533/**
1534 *      gsm_dlci_data           -       data arrived
1535 *      @dlci: channel
1536 *      @data: block of bytes received
1537 *      @len: length of received block
1538 *
1539 *      A UI or UIH frame has arrived which contains data for a channel
1540 *      other than the control channel. If the relevant virtual tty is
1541 *      open we shovel the bits down it, if not we drop them.
1542 */
1543
1544static void gsm_dlci_data(struct gsm_dlci *dlci, u8 *data, int clen)
1545{
1546        /* krefs .. */
1547        struct tty_port *port = &dlci->port;
1548        struct tty_struct *tty = tty_port_tty_get(port);
1549        unsigned int modem = 0;
1550        int len = clen;
1551
1552        if (debug & 16)
1553                pr_debug("%d bytes for tty %p\n", len, tty);
1554        if (tty) {
1555                switch (dlci->adaption)  {
1556                /* Unsupported types */
1557                /* Packetised interruptible data */
1558                case 4:
1559                        break;
1560                /* Packetised uininterruptible voice/data */
1561                case 3:
1562                        break;
1563                /* Asynchronous serial with line state in each frame */
1564                case 2:
1565                        while (gsm_read_ea(&modem, *data++) == 0) {
1566                                len--;
1567                                if (len == 0)
1568                                        return;
1569                        }
1570                        gsm_process_modem(tty, dlci, modem, clen);
1571                /* Line state will go via DLCI 0 controls only */
1572                case 1:
1573                default:
1574                        tty_insert_flip_string(tty, data, len);
1575                        tty_flip_buffer_push(tty);
1576                }
1577                tty_kref_put(tty);
1578        }
1579}
1580
1581/**
1582 *      gsm_dlci_control        -       data arrived on control channel
1583 *      @dlci: channel
1584 *      @data: block of bytes received
1585 *      @len: length of received block
1586 *
1587 *      A UI or UIH frame has arrived which contains data for DLCI 0 the
1588 *      control channel. This should contain a command EA followed by
1589 *      control data bytes. The command EA contains a command/response bit
1590 *      and we divide up the work accordingly.
1591 */
1592
1593static void gsm_dlci_command(struct gsm_dlci *dlci, u8 *data, int len)
1594{
1595        /* See what command is involved */
1596        unsigned int command = 0;
1597        while (len-- > 0) {
1598                if (gsm_read_ea(&command, *data++) == 1) {
1599                        int clen = *data++;
1600                        len--;
1601                        /* FIXME: this is properly an EA */
1602                        clen >>= 1;
1603                        /* Malformed command ? */
1604                        if (clen > len)
1605                                return;
1606                        if (command & 1)
1607                                gsm_control_message(dlci->gsm, command,
1608                                                                data, clen);
1609                        else
1610                                gsm_control_response(dlci->gsm, command,
1611                                                                data, clen);
1612                        return;
1613                }
1614        }
1615}
1616
1617/*
1618 *      Allocate/Free DLCI channels
1619 */
1620
1621/**
1622 *      gsm_dlci_alloc          -       allocate a DLCI
1623 *      @gsm: GSM mux
1624 *      @addr: address of the DLCI
1625 *
1626 *      Allocate and install a new DLCI object into the GSM mux.
1627 *
1628 *      FIXME: review locking races
1629 */
1630
1631static struct gsm_dlci *gsm_dlci_alloc(struct gsm_mux *gsm, int addr)
1632{
1633        struct gsm_dlci *dlci = kzalloc(sizeof(struct gsm_dlci), GFP_ATOMIC);
1634        if (dlci == NULL)
1635                return NULL;
1636        spin_lock_init(&dlci->lock);
1637        mutex_init(&dlci->mutex);
1638        dlci->fifo = &dlci->_fifo;
1639        if (kfifo_alloc(&dlci->_fifo, 4096, GFP_KERNEL) < 0) {
1640                kfree(dlci);
1641                return NULL;
1642        }
1643
1644        skb_queue_head_init(&dlci->skb_list);
1645        init_timer(&dlci->t1);
1646        dlci->t1.function = gsm_dlci_t1;
1647        dlci->t1.data = (unsigned long)dlci;
1648        tty_port_init(&dlci->port);
1649        dlci->port.ops = &gsm_port_ops;
1650        dlci->gsm = gsm;
1651        dlci->addr = addr;
1652        dlci->adaption = gsm->adaption;
1653        dlci->state = DLCI_CLOSED;
1654        if (addr)
1655                dlci->data = gsm_dlci_data;
1656        else
1657                dlci->data = gsm_dlci_command;
1658        gsm->dlci[addr] = dlci;
1659        return dlci;
1660}
1661
1662/**
1663 *      gsm_dlci_free           -       free DLCI
1664 *      @dlci: DLCI to free
1665 *
1666 *      Free up a DLCI.
1667 *
1668 *      Can sleep.
1669 */
1670static void gsm_dlci_free(struct tty_port *port)
1671{
1672        struct gsm_dlci *dlci = container_of(port, struct gsm_dlci, port);
1673
1674        del_timer_sync(&dlci->t1);
1675        dlci->gsm->dlci[dlci->addr] = NULL;
1676        kfifo_free(dlci->fifo);
1677        while ((dlci->skb = skb_dequeue(&dlci->skb_list)))
1678                dev_kfree_skb(dlci->skb);
1679        kfree(dlci);
1680}
1681
1682static inline void dlci_get(struct gsm_dlci *dlci)
1683{
1684        tty_port_get(&dlci->port);
1685}
1686
1687static inline void dlci_put(struct gsm_dlci *dlci)
1688{
1689        tty_port_put(&dlci->port);
1690}
1691
1692static void gsm_destroy_network(struct gsm_dlci *dlci);
1693
1694/**
1695 *      gsm_dlci_release                -       release DLCI
1696 *      @dlci: DLCI to destroy
1697 *
1698 *      Release a DLCI. Actual free is deferred until either
1699 *      mux is closed or tty is closed - whichever is last.
1700 *
1701 *      Can sleep.
1702 */
1703static void gsm_dlci_release(struct gsm_dlci *dlci)
1704{
1705        struct tty_struct *tty = tty_port_tty_get(&dlci->port);
1706        if (tty) {
1707                mutex_lock(&dlci->mutex);
1708                gsm_destroy_network(dlci);
1709                mutex_unlock(&dlci->mutex);
1710
1711                /* tty_vhangup needs the tty_lock, so unlock and
1712                   relock after doing the hangup. */
1713                tty_unlock(tty);
1714                tty_vhangup(tty);
1715                tty_lock(tty);
1716                tty_port_tty_set(&dlci->port, NULL);
1717                tty_kref_put(tty);
1718        }
1719        dlci->state = DLCI_CLOSED;
1720        dlci_put(dlci);
1721}
1722
1723/*
1724 *      LAPBish link layer logic
1725 */
1726
1727/**
1728 *      gsm_queue               -       a GSM frame is ready to process
1729 *      @gsm: pointer to our gsm mux
1730 *
1731 *      At this point in time a frame has arrived and been demangled from
1732 *      the line encoding. All the differences between the encodings have
1733 *      been handled below us and the frame is unpacked into the structures.
1734 *      The fcs holds the header FCS but any data FCS must be added here.
1735 */
1736
1737static void gsm_queue(struct gsm_mux *gsm)
1738{
1739        struct gsm_dlci *dlci;
1740        u8 cr;
1741        int address;
1742        /* We have to sneak a look at the packet body to do the FCS.
1743           A somewhat layering violation in the spec */
1744
1745        if ((gsm->control & ~PF) == UI)
1746                gsm->fcs = gsm_fcs_add_block(gsm->fcs, gsm->buf, gsm->len);
1747        if (gsm->encoding == 0){
1748                /* WARNING: gsm->received_fcs is used for gsm->encoding = 0 only.
1749                            In this case it contain the last piece of data
1750                            required to generate final CRC */
1751                gsm->fcs = gsm_fcs_add(gsm->fcs, gsm->received_fcs);
1752        }
1753        if (gsm->fcs != GOOD_FCS) {
1754                gsm->bad_fcs++;
1755                if (debug & 4)
1756                        pr_debug("BAD FCS %02x\n", gsm->fcs);
1757                return;
1758        }
1759        address = gsm->address >> 1;
1760        if (address >= NUM_DLCI)
1761                goto invalid;
1762
1763        cr = gsm->address & 1;          /* C/R bit */
1764
1765        gsm_print_packet("<--", address, cr, gsm->control, gsm->buf, gsm->len);
1766
1767        cr ^= 1 - gsm->initiator;       /* Flip so 1 always means command */
1768        dlci = gsm->dlci[address];
1769
1770        switch (gsm->control) {
1771        case SABM|PF:
1772                if (cr == 0)
1773                        goto invalid;
1774                if (dlci == NULL)
1775                        dlci = gsm_dlci_alloc(gsm, address);
1776                if (dlci == NULL)
1777                        return;
1778                if (dlci->dead)
1779                        gsm_response(gsm, address, DM);
1780                else {
1781                        gsm_response(gsm, address, UA);
1782                        gsm_dlci_open(dlci);
1783                }
1784                break;
1785        case DISC|PF:
1786                if (cr == 0)
1787                        goto invalid;
1788                if (dlci == NULL || dlci->state == DLCI_CLOSED) {
1789                        gsm_response(gsm, address, DM);
1790                        return;
1791                }
1792                /* Real close complete */
1793                gsm_response(gsm, address, UA);
1794                gsm_dlci_close(dlci);
1795                break;
1796        case UA:
1797        case UA|PF:
1798                if (cr == 0 || dlci == NULL)
1799                        break;
1800                switch (dlci->state) {
1801                case DLCI_CLOSING:
1802                        gsm_dlci_close(dlci);
1803                        break;
1804                case DLCI_OPENING:
1805                        gsm_dlci_open(dlci);
1806                        break;
1807                }
1808                break;
1809        case DM:        /* DM can be valid unsolicited */
1810        case DM|PF:
1811                if (cr)
1812                        goto invalid;
1813                if (dlci == NULL)
1814                        return;
1815                gsm_dlci_close(dlci);
1816                break;
1817        case UI:
1818        case UI|PF:
1819        case UIH:
1820        case UIH|PF:
1821#if 0
1822                if (cr)
1823                        goto invalid;
1824#endif
1825                if (dlci == NULL || dlci->state != DLCI_OPEN) {
1826                        gsm_command(gsm, address, DM|PF);
1827                        return;
1828                }
1829                dlci->data(dlci, gsm->buf, gsm->len);
1830                break;
1831        default:
1832                goto invalid;
1833        }
1834        return;
1835invalid:
1836        gsm->malformed++;
1837        return;
1838}
1839
1840
1841/**
1842 *      gsm0_receive    -       perform processing for non-transparency
1843 *      @gsm: gsm data for this ldisc instance
1844 *      @c: character
1845 *
1846 *      Receive bytes in gsm mode 0
1847 */
1848
1849static void gsm0_receive(struct gsm_mux *gsm, unsigned char c)
1850{
1851        unsigned int len;
1852
1853        switch (gsm->state) {
1854        case GSM_SEARCH:        /* SOF marker */
1855                if (c == GSM0_SOF) {
1856                        gsm->state = GSM_ADDRESS;
1857                        gsm->address = 0;
1858                        gsm->len = 0;
1859                        gsm->fcs = INIT_FCS;
1860                }
1861                break;
1862        case GSM_ADDRESS:       /* Address EA */
1863                gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1864                if (gsm_read_ea(&gsm->address, c))
1865                        gsm->state = GSM_CONTROL;
1866                break;
1867        case GSM_CONTROL:       /* Control Byte */
1868                gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1869                gsm->control = c;
1870                gsm->state = GSM_LEN0;
1871                break;
1872        case GSM_LEN0:          /* Length EA */
1873                gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1874                if (gsm_read_ea(&gsm->len, c)) {
1875                        if (gsm->len > gsm->mru) {
1876                                gsm->bad_size++;
1877                                gsm->state = GSM_SEARCH;
1878                                break;
1879                        }
1880                        gsm->count = 0;
1881                        if (!gsm->len)
1882                                gsm->state = GSM_FCS;
1883                        else
1884                                gsm->state = GSM_DATA;
1885                        break;
1886                }
1887                gsm->state = GSM_LEN1;
1888                break;
1889        case GSM_LEN1:
1890                gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1891                len = c;
1892                gsm->len |= len << 7;
1893                if (gsm->len > gsm->mru) {
1894                        gsm->bad_size++;
1895                        gsm->state = GSM_SEARCH;
1896                        break;
1897                }
1898                gsm->count = 0;
1899                if (!gsm->len)
1900                        gsm->state = GSM_FCS;
1901                else
1902                        gsm->state = GSM_DATA;
1903                break;
1904        case GSM_DATA:          /* Data */
1905                gsm->buf[gsm->count++] = c;
1906                if (gsm->count == gsm->len)
1907                        gsm->state = GSM_FCS;
1908                break;
1909        case GSM_FCS:           /* FCS follows the packet */
1910                gsm->received_fcs = c;
1911                gsm_queue(gsm);
1912                gsm->state = GSM_SSOF;
1913                break;
1914        case GSM_SSOF:
1915                if (c == GSM0_SOF) {
1916                        gsm->state = GSM_SEARCH;
1917                        break;
1918                }
1919                break;
1920        }
1921}
1922
1923/**
1924 *      gsm1_receive    -       perform processing for non-transparency
1925 *      @gsm: gsm data for this ldisc instance
1926 *      @c: character
1927 *
1928 *      Receive bytes in mode 1 (Advanced option)
1929 */
1930
1931static void gsm1_receive(struct gsm_mux *gsm, unsigned char c)
1932{
1933        if (c == GSM1_SOF) {
1934                /* EOF is only valid in frame if we have got to the data state
1935                   and received at least one byte (the FCS) */
1936                if (gsm->state == GSM_DATA && gsm->count) {
1937                        /* Extract the FCS */
1938                        gsm->count--;
1939                        gsm->fcs = gsm_fcs_add(gsm->fcs, gsm->buf[gsm->count]);
1940                        gsm->len = gsm->count;
1941                        gsm_queue(gsm);
1942                        gsm->state  = GSM_START;
1943                        return;
1944                }
1945                /* Any partial frame was a runt so go back to start */
1946                if (gsm->state != GSM_START) {
1947                        gsm->malformed++;
1948                        gsm->state = GSM_START;
1949                }
1950                /* A SOF in GSM_START means we are still reading idling or
1951                   framing bytes */
1952                return;
1953        }
1954
1955        if (c == GSM1_ESCAPE) {
1956                gsm->escape = 1;
1957                return;
1958        }
1959
1960        /* Only an unescaped SOF gets us out of GSM search */
1961        if (gsm->state == GSM_SEARCH)
1962                return;
1963
1964        if (gsm->escape) {
1965                c ^= GSM1_ESCAPE_BITS;
1966                gsm->escape = 0;
1967        }
1968        switch (gsm->state) {
1969        case GSM_START:         /* First byte after SOF */
1970                gsm->address = 0;
1971                gsm->state = GSM_ADDRESS;
1972                gsm->fcs = INIT_FCS;
1973                /* Drop through */
1974        case GSM_ADDRESS:       /* Address continuation */
1975                gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1976                if (gsm_read_ea(&gsm->address, c))
1977                        gsm->state = GSM_CONTROL;
1978                break;
1979        case GSM_CONTROL:       /* Control Byte */
1980                gsm->fcs = gsm_fcs_add(gsm->fcs, c);
1981                gsm->control = c;
1982                gsm->count = 0;
1983                gsm->state = GSM_DATA;
1984                break;
1985        case GSM_DATA:          /* Data */
1986                if (gsm->count > gsm->mru) {    /* Allow one for the FCS */
1987                        gsm->state = GSM_OVERRUN;
1988                        gsm->bad_size++;
1989                } else
1990                        gsm->buf[gsm->count++] = c;
1991                break;
1992        case GSM_OVERRUN:       /* Over-long - eg a dropped SOF */
1993                break;
1994        }
1995}
1996
1997/**
1998 *      gsm_error               -       handle tty error
1999 *      @gsm: ldisc data
2000 *      @data: byte received (may be invalid)
2001 *      @flag: error received
2002 *
2003 *      Handle an error in the receipt of data for a frame. Currently we just
2004 *      go back to hunting for a SOF.
2005 *
2006 *      FIXME: better diagnostics ?
2007 */
2008
2009static void gsm_error(struct gsm_mux *gsm,
2010                                unsigned char data, unsigned char flag)
2011{
2012        gsm->state = GSM_SEARCH;
2013        gsm->io_error++;
2014}
2015
2016/**
2017 *      gsm_cleanup_mux         -       generic GSM protocol cleanup
2018 *      @gsm: our mux
2019 *
2020 *      Clean up the bits of the mux which are the same for all framing
2021 *      protocols. Remove the mux from the mux table, stop all the timers
2022 *      and then shut down each device hanging up the channels as we go.
2023 */
2024
2025void gsm_cleanup_mux(struct gsm_mux *gsm)
2026{
2027        int i;
2028        struct gsm_dlci *dlci = gsm->dlci[0];
2029        struct gsm_msg *txq, *ntxq;
2030        struct gsm_control *gc;
2031
2032        gsm->dead = 1;
2033
2034        spin_lock(&gsm_mux_lock);
2035        for (i = 0; i < MAX_MUX; i++) {
2036                if (gsm_mux[i] == gsm) {
2037                        gsm_mux[i] = NULL;
2038                        break;
2039                }
2040        }
2041        spin_unlock(&gsm_mux_lock);
2042        WARN_ON(i == MAX_MUX);
2043
2044        /* In theory disconnecting DLCI 0 is sufficient but for some
2045           modems this is apparently not the case. */
2046        if (dlci) {
2047                gc = gsm_control_send(gsm, CMD_CLD, NULL, 0);
2048                if (gc)
2049                        gsm_control_wait(gsm, gc);
2050        }
2051        del_timer_sync(&gsm->t2_timer);
2052        /* Now we are sure T2 has stopped */
2053        if (dlci) {
2054                dlci->dead = 1;
2055                gsm_dlci_begin_close(dlci);
2056                wait_event_interruptible(gsm->event,
2057                                        dlci->state == DLCI_CLOSED);
2058        }
2059        /* Free up any link layer users */
2060        for (i = 0; i < NUM_DLCI; i++)
2061                if (gsm->dlci[i])
2062                        gsm_dlci_release(gsm->dlci[i]);
2063        /* Now wipe the queues */
2064        list_for_each_entry_safe(txq, ntxq, &gsm->tx_list, list)
2065                kfree(txq);
2066        INIT_LIST_HEAD(&gsm->tx_list);
2067}
2068EXPORT_SYMBOL_GPL(gsm_cleanup_mux);
2069
2070/**
2071 *      gsm_activate_mux        -       generic GSM setup
2072 *      @gsm: our mux
2073 *
2074 *      Set up the bits of the mux which are the same for all framing
2075 *      protocols. Add the mux to the mux table so it can be opened and
2076 *      finally kick off connecting to DLCI 0 on the modem.
2077 */
2078
2079int gsm_activate_mux(struct gsm_mux *gsm)
2080{
2081        struct gsm_dlci *dlci;
2082        int i = 0;
2083
2084        init_timer(&gsm->t2_timer);
2085        gsm->t2_timer.function = gsm_control_retransmit;
2086        gsm->t2_timer.data = (unsigned long)gsm;
2087        init_waitqueue_head(&gsm->event);
2088        spin_lock_init(&gsm->control_lock);
2089        spin_lock_init(&gsm->tx_lock);
2090
2091        if (gsm->encoding == 0)
2092                gsm->receive = gsm0_receive;
2093        else
2094                gsm->receive = gsm1_receive;
2095        gsm->error = gsm_error;
2096
2097        spin_lock(&gsm_mux_lock);
2098        for (i = 0; i < MAX_MUX; i++) {
2099                if (gsm_mux[i] == NULL) {
2100                        gsm->num = i;
2101                        gsm_mux[i] = gsm;
2102                        break;
2103                }
2104        }
2105        spin_unlock(&gsm_mux_lock);
2106        if (i == MAX_MUX)
2107                return -EBUSY;
2108
2109        dlci = gsm_dlci_alloc(gsm, 0);
2110        if (dlci == NULL)
2111                return -ENOMEM;
2112        gsm->dead = 0;          /* Tty opens are now permissible */
2113        return 0;
2114}
2115EXPORT_SYMBOL_GPL(gsm_activate_mux);
2116
2117/**
2118 *      gsm_free_mux            -       free up a mux
2119 *      @mux: mux to free
2120 *
2121 *      Dispose of allocated resources for a dead mux
2122 */
2123void gsm_free_mux(struct gsm_mux *gsm)
2124{
2125        kfree(gsm->txframe);
2126        kfree(gsm->buf);
2127        kfree(gsm);
2128}
2129EXPORT_SYMBOL_GPL(gsm_free_mux);
2130
2131/**
2132 *      gsm_free_muxr           -       free up a mux
2133 *      @mux: mux to free
2134 *
2135 *      Dispose of allocated resources for a dead mux
2136 */
2137static void gsm_free_muxr(struct kref *ref)
2138{
2139        struct gsm_mux *gsm = container_of(ref, struct gsm_mux, ref);
2140        gsm_free_mux(gsm);
2141}
2142
2143static inline void mux_get(struct gsm_mux *gsm)
2144{
2145        kref_get(&gsm->ref);
2146}
2147
2148static inline void mux_put(struct gsm_mux *gsm)
2149{
2150        kref_put(&gsm->ref, gsm_free_muxr);
2151}
2152
2153/**
2154 *      gsm_alloc_mux           -       allocate a mux
2155 *
2156 *      Creates a new mux ready for activation.
2157 */
2158
2159struct gsm_mux *gsm_alloc_mux(void)
2160{
2161        struct gsm_mux *gsm = kzalloc(sizeof(struct gsm_mux), GFP_KERNEL);
2162        if (gsm == NULL)
2163                return NULL;
2164        gsm->buf = kmalloc(MAX_MRU + 1, GFP_KERNEL);
2165        if (gsm->buf == NULL) {
2166                kfree(gsm);
2167                return NULL;
2168        }
2169        gsm->txframe = kmalloc(2 * MAX_MRU + 2, GFP_KERNEL);
2170        if (gsm->txframe == NULL) {
2171                kfree(gsm->buf);
2172                kfree(gsm);
2173                return NULL;
2174        }
2175        spin_lock_init(&gsm->lock);
2176        kref_init(&gsm->ref);
2177        INIT_LIST_HEAD(&gsm->tx_list);
2178
2179        gsm->t1 = T1;
2180        gsm->t2 = T2;
2181        gsm->n2 = N2;
2182        gsm->ftype = UIH;
2183        gsm->adaption = 1;
2184        gsm->encoding = 1;
2185        gsm->mru = 64;  /* Default to encoding 1 so these should be 64 */
2186        gsm->mtu = 64;
2187        gsm->dead = 1;  /* Avoid early tty opens */
2188
2189        return gsm;
2190}
2191EXPORT_SYMBOL_GPL(gsm_alloc_mux);
2192
2193/**
2194 *      gsmld_output            -       write to link
2195 *      @gsm: our mux
2196 *      @data: bytes to output
2197 *      @len: size
2198 *
2199 *      Write a block of data from the GSM mux to the data channel. This
2200 *      will eventually be serialized from above but at the moment isn't.
2201 */
2202
2203static int gsmld_output(struct gsm_mux *gsm, u8 *data, int len)
2204{
2205        if (tty_write_room(gsm->tty) < len) {
2206                set_bit(TTY_DO_WRITE_WAKEUP, &gsm->tty->flags);
2207                return -ENOSPC;
2208        }
2209        if (debug & 4)
2210                print_hex_dump_bytes("gsmld_output: ", DUMP_PREFIX_OFFSET,
2211                                     data, len);
2212        gsm->tty->ops->write(gsm->tty, data, len);
2213        return len;
2214}
2215
2216/**
2217 *      gsmld_attach_gsm        -       mode set up
2218 *      @tty: our tty structure
2219 *      @gsm: our mux
2220 *
2221 *      Set up the MUX for basic mode and commence connecting to the
2222 *      modem. Currently called from the line discipline set up but
2223 *      will need moving to an ioctl path.
2224 */
2225
2226static int gsmld_attach_gsm(struct tty_struct *tty, struct gsm_mux *gsm)
2227{
2228        int ret, i;
2229        int base = gsm->num << 6; /* Base for this MUX */
2230
2231        gsm->tty = tty_kref_get(tty);
2232        gsm->output = gsmld_output;
2233        ret =  gsm_activate_mux(gsm);
2234        if (ret != 0)
2235                tty_kref_put(gsm->tty);
2236        else {
2237                /* Don't register device 0 - this is the control channel and not
2238                   a usable tty interface */
2239                for (i = 1; i < NUM_DLCI; i++)
2240                        tty_register_device(gsm_tty_driver, base + i, NULL);
2241        }
2242        return ret;
2243}
2244
2245
2246/**
2247 *      gsmld_detach_gsm        -       stop doing 0710 mux
2248 *      @tty: tty attached to the mux
2249 *      @gsm: mux
2250 *
2251 *      Shutdown and then clean up the resources used by the line discipline
2252 */
2253
2254static void gsmld_detach_gsm(struct tty_struct *tty, struct gsm_mux *gsm)
2255{
2256        int i;
2257        int base = gsm->num << 6; /* Base for this MUX */
2258
2259        WARN_ON(tty != gsm->tty);
2260        for (i = 1; i < NUM_DLCI; i++)
2261                tty_unregister_device(gsm_tty_driver, base + i);
2262        gsm_cleanup_mux(gsm);
2263        tty_kref_put(gsm->tty);
2264        gsm->tty = NULL;
2265}
2266
2267static void gsmld_receive_buf(struct tty_struct *tty, const unsigned char *cp,
2268                              char *fp, int count)
2269{
2270        struct gsm_mux *gsm = tty->disc_data;
2271        const unsigned char *dp;
2272        char *f;
2273        int i;
2274        char buf[64];
2275        char flags;
2276
2277        if (debug & 4)
2278                print_hex_dump_bytes("gsmld_receive: ", DUMP_PREFIX_OFFSET,
2279                                     cp, count);
2280
2281        for (i = count, dp = cp, f = fp; i; i--, dp++) {
2282                flags = *f++;
2283                switch (flags) {
2284                case TTY_NORMAL:
2285                        gsm->receive(gsm, *dp);
2286                        break;
2287                case TTY_OVERRUN:
2288                case TTY_BREAK:
2289                case TTY_PARITY:
2290                case TTY_FRAME:
2291                        gsm->error(gsm, *dp, flags);
2292                        break;
2293                default:
2294                        WARN_ONCE(1, "%s: unknown flag %d\n",
2295                               tty_name(tty, buf), flags);
2296                        break;
2297                }
2298        }
2299        /* FASYNC if needed ? */
2300        /* If clogged call tty_throttle(tty); */
2301}
2302
2303/**
2304 *      gsmld_chars_in_buffer   -       report available bytes
2305 *      @tty: tty device
2306 *
2307 *      Report the number of characters buffered to be delivered to user
2308 *      at this instant in time.
2309 *
2310 *      Locking: gsm lock
2311 */
2312
2313static ssize_t gsmld_chars_in_buffer(struct tty_struct *tty)
2314{
2315        return 0;
2316}
2317
2318/**
2319 *      gsmld_flush_buffer      -       clean input queue
2320 *      @tty:   terminal device
2321 *
2322 *      Flush the input buffer. Called when the line discipline is
2323 *      being closed, when the tty layer wants the buffer flushed (eg
2324 *      at hangup).
2325 */
2326
2327static void gsmld_flush_buffer(struct tty_struct *tty)
2328{
2329}
2330
2331/**
2332 *      gsmld_close             -       close the ldisc for this tty
2333 *      @tty: device
2334 *
2335 *      Called from the terminal layer when this line discipline is
2336 *      being shut down, either because of a close or becsuse of a
2337 *      discipline change. The function will not be called while other
2338 *      ldisc methods are in progress.
2339 */
2340
2341static void gsmld_close(struct tty_struct *tty)
2342{
2343        struct gsm_mux *gsm = tty->disc_data;
2344
2345        gsmld_detach_gsm(tty, gsm);
2346
2347        gsmld_flush_buffer(tty);
2348        /* Do other clean up here */
2349        mux_put(gsm);
2350}
2351
2352/**
2353 *      gsmld_open              -       open an ldisc
2354 *      @tty: terminal to open
2355 *
2356 *      Called when this line discipline is being attached to the
2357 *      terminal device. Can sleep. Called serialized so that no
2358 *      other events will occur in parallel. No further open will occur
2359 *      until a close.
2360 */
2361
2362static int gsmld_open(struct tty_struct *tty)
2363{
2364        struct gsm_mux *gsm;
2365
2366        if (tty->ops->write == NULL)
2367                return -EINVAL;
2368
2369        /* Attach our ldisc data */
2370        gsm = gsm_alloc_mux();
2371        if (gsm == NULL)
2372                return -ENOMEM;
2373
2374        tty->disc_data = gsm;
2375        tty->receive_room = 65536;
2376
2377        /* Attach the initial passive connection */
2378        gsm->encoding = 1;
2379        return gsmld_attach_gsm(tty, gsm);
2380}
2381
2382/**
2383 *      gsmld_write_wakeup      -       asynchronous I/O notifier
2384 *      @tty: tty device
2385 *
2386 *      Required for the ptys, serial driver etc. since processes
2387 *      that attach themselves to the master and rely on ASYNC
2388 *      IO must be woken up
2389 */
2390
2391static void gsmld_write_wakeup(struct tty_struct *tty)
2392{
2393        struct gsm_mux *gsm = tty->disc_data;
2394        unsigned long flags;
2395
2396        /* Queue poll */
2397        clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
2398        spin_lock_irqsave(&gsm->tx_lock, flags);
2399        gsm_data_kick(gsm);
2400        if (gsm->tx_bytes < TX_THRESH_LO) {
2401                gsm_dlci_data_sweep(gsm);
2402        }
2403        spin_unlock_irqrestore(&gsm->tx_lock, flags);
2404}
2405
2406/**
2407 *      gsmld_read              -       read function for tty
2408 *      @tty: tty device
2409 *      @file: file object
2410 *      @buf: userspace buffer pointer
2411 *      @nr: size of I/O
2412 *
2413 *      Perform reads for the line discipline. We are guaranteed that the
2414 *      line discipline will not be closed under us but we may get multiple
2415 *      parallel readers and must handle this ourselves. We may also get
2416 *      a hangup. Always called in user context, may sleep.
2417 *
2418 *      This code must be sure never to sleep through a hangup.
2419 */
2420
2421static ssize_t gsmld_read(struct tty_struct *tty, struct file *file,
2422                         unsigned char __user *buf, size_t nr)
2423{
2424        return -EOPNOTSUPP;
2425}
2426
2427/**
2428 *      gsmld_write             -       write function for tty
2429 *      @tty: tty device
2430 *      @file: file object
2431 *      @buf: userspace buffer pointer
2432 *      @nr: size of I/O
2433 *
2434 *      Called when the owner of the device wants to send a frame
2435 *      itself (or some other control data). The data is transferred
2436 *      as-is and must be properly framed and checksummed as appropriate
2437 *      by userspace. Frames are either sent whole or not at all as this
2438 *      avoids pain user side.
2439 */
2440
2441static ssize_t gsmld_write(struct tty_struct *tty, struct file *file,
2442                           const unsigned char *buf, size_t nr)
2443{
2444        int space = tty_write_room(tty);
2445        if (space >= nr)
2446                return tty->ops->write(tty, buf, nr);
2447        set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
2448        return -ENOBUFS;
2449}
2450
2451/**
2452 *      gsmld_poll              -       poll method for N_GSM0710
2453 *      @tty: terminal device
2454 *      @file: file accessing it
2455 *      @wait: poll table
2456 *
2457 *      Called when the line discipline is asked to poll() for data or
2458 *      for special events. This code is not serialized with respect to
2459 *      other events save open/close.
2460 *
2461 *      This code must be sure never to sleep through a hangup.
2462 *      Called without the kernel lock held - fine
2463 */
2464
2465static unsigned int gsmld_poll(struct tty_struct *tty, struct file *file,
2466                                                        poll_table *wait)
2467{
2468        unsigned int mask = 0;
2469        struct gsm_mux *gsm = tty->disc_data;
2470
2471        poll_wait(file, &tty->read_wait, wait);
2472        poll_wait(file, &tty->write_wait, wait);
2473        if (tty_hung_up_p(file))
2474                mask |= POLLHUP;
2475        if (!tty_is_writelocked(tty) && tty_write_room(tty) > 0)
2476                mask |= POLLOUT | POLLWRNORM;
2477        if (gsm->dead)
2478                mask |= POLLHUP;
2479        return mask;
2480}
2481
2482static int gsmld_config(struct tty_struct *tty, struct gsm_mux *gsm,
2483                                                        struct gsm_config *c)
2484{
2485        int need_close = 0;
2486        int need_restart = 0;
2487
2488        /* Stuff we don't support yet - UI or I frame transport, windowing */
2489        if ((c->adaption != 1 && c->adaption != 2) || c->k)
2490                return -EOPNOTSUPP;
2491        /* Check the MRU/MTU range looks sane */
2492        if (c->mru > MAX_MRU || c->mtu > MAX_MTU || c->mru < 8 || c->mtu < 8)
2493                return -EINVAL;
2494        if (c->n2 < 3)
2495                return -EINVAL;
2496        if (c->encapsulation > 1)       /* Basic, advanced, no I */
2497                return -EINVAL;
2498        if (c->initiator > 1)
2499                return -EINVAL;
2500        if (c->i == 0 || c->i > 2)      /* UIH and UI only */
2501                return -EINVAL;
2502        /*
2503         *      See what is needed for reconfiguration
2504         */
2505
2506        /* Timing fields */
2507        if (c->t1 != 0 && c->t1 != gsm->t1)
2508                need_restart = 1;
2509        if (c->t2 != 0 && c->t2 != gsm->t2)
2510                need_restart = 1;
2511        if (c->encapsulation != gsm->encoding)
2512                need_restart = 1;
2513        if (c->adaption != gsm->adaption)
2514                need_restart = 1;
2515        /* Requires care */
2516        if (c->initiator != gsm->initiator)
2517                need_close = 1;
2518        if (c->mru != gsm->mru)
2519                need_restart = 1;
2520        if (c->mtu != gsm->mtu)
2521                need_restart = 1;
2522
2523        /*
2524         *      Close down what is needed, restart and initiate the new
2525         *      configuration
2526         */
2527
2528        if (need_close || need_restart) {
2529                gsm_dlci_begin_close(gsm->dlci[0]);
2530                /* This will timeout if the link is down due to N2 expiring */
2531                wait_event_interruptible(gsm->event,
2532                                gsm->dlci[0]->state == DLCI_CLOSED);
2533                if (signal_pending(current))
2534                        return -EINTR;
2535        }
2536        if (need_restart)
2537                gsm_cleanup_mux(gsm);
2538
2539        gsm->initiator = c->initiator;
2540        gsm->mru = c->mru;
2541        gsm->mtu = c->mtu;
2542        gsm->encoding = c->encapsulation;
2543        gsm->adaption = c->adaption;
2544        gsm->n2 = c->n2;
2545
2546        if (c->i == 1)
2547                gsm->ftype = UIH;
2548        else if (c->i == 2)
2549                gsm->ftype = UI;
2550
2551        if (c->t1)
2552                gsm->t1 = c->t1;
2553        if (c->t2)
2554                gsm->t2 = c->t2;
2555
2556        /* FIXME: We need to separate activation/deactivation from adding
2557           and removing from the mux array */
2558        if (need_restart)
2559                gsm_activate_mux(gsm);
2560        if (gsm->initiator && need_close)
2561                gsm_dlci_begin_open(gsm->dlci[0]);
2562        return 0;
2563}
2564
2565static int gsmld_ioctl(struct tty_struct *tty, struct file *file,
2566                       unsigned int cmd, unsigned long arg)
2567{
2568        struct gsm_config c;
2569        struct gsm_mux *gsm = tty->disc_data;
2570
2571        switch (cmd) {
2572        case GSMIOC_GETCONF:
2573                memset(&c, 0, sizeof(c));
2574                c.adaption = gsm->adaption;
2575                c.encapsulation = gsm->encoding;
2576                c.initiator = gsm->initiator;
2577                c.t1 = gsm->t1;
2578                c.t2 = gsm->t2;
2579                c.t3 = 0;       /* Not supported */
2580                c.n2 = gsm->n2;
2581                if (gsm->ftype == UIH)
2582                        c.i = 1;
2583                else
2584                        c.i = 2;
2585                pr_debug("Ftype %d i %d\n", gsm->ftype, c.i);
2586                c.mru = gsm->mru;
2587                c.mtu = gsm->mtu;
2588                c.k = 0;
2589                if (copy_to_user((void *)arg, &c, sizeof(c)))
2590                        return -EFAULT;
2591                return 0;
2592        case GSMIOC_SETCONF:
2593                if (copy_from_user(&c, (void *)arg, sizeof(c)))
2594                        return -EFAULT;
2595                return gsmld_config(tty, gsm, &c);
2596        default:
2597                return n_tty_ioctl_helper(tty, file, cmd, arg);
2598        }
2599}
2600
2601/*
2602 *      Network interface
2603 *
2604 */
2605
2606static int gsm_mux_net_open(struct net_device *net)
2607{
2608        pr_debug("%s called\n", __func__);
2609        netif_start_queue(net);
2610        return 0;
2611}
2612
2613static int gsm_mux_net_close(struct net_device *net)
2614{
2615        netif_stop_queue(net);
2616        return 0;
2617}
2618
2619static struct net_device_stats *gsm_mux_net_get_stats(struct net_device *net)
2620{
2621        return &((struct gsm_mux_net *)netdev_priv(net))->stats;
2622}
2623static void dlci_net_free(struct gsm_dlci *dlci)
2624{
2625        if (!dlci->net) {
2626                WARN_ON(1);
2627                return;
2628        }
2629        dlci->adaption = dlci->prev_adaption;
2630        dlci->data = dlci->prev_data;
2631        free_netdev(dlci->net);
2632        dlci->net = NULL;
2633}
2634static void net_free(struct kref *ref)
2635{
2636        struct gsm_mux_net *mux_net;
2637        struct gsm_dlci *dlci;
2638
2639        mux_net = container_of(ref, struct gsm_mux_net, ref);
2640        dlci = mux_net->dlci;
2641
2642        if (dlci->net) {
2643                unregister_netdev(dlci->net);
2644                dlci_net_free(dlci);
2645        }
2646}
2647
2648static inline void muxnet_get(struct gsm_mux_net *mux_net)
2649{
2650        kref_get(&mux_net->ref);
2651}
2652
2653static inline void muxnet_put(struct gsm_mux_net *mux_net)
2654{
2655        kref_put(&mux_net->ref, net_free);
2656}
2657
2658static int gsm_mux_net_start_xmit(struct sk_buff *skb,
2659                                      struct net_device *net)
2660{
2661        struct gsm_mux_net *mux_net = (struct gsm_mux_net *)netdev_priv(net);
2662        struct gsm_dlci *dlci = mux_net->dlci;
2663        muxnet_get(mux_net);
2664
2665        skb_queue_head(&dlci->skb_list, skb);
2666        STATS(net).tx_packets++;
2667        STATS(net).tx_bytes += skb->len;
2668        gsm_dlci_data_kick(dlci);
2669        /* And tell the kernel when the last transmit started. */
2670        net->trans_start = jiffies;
2671        muxnet_put(mux_net);
2672        return NETDEV_TX_OK;
2673}
2674
2675/* called when a packet did not ack after watchdogtimeout */
2676static void gsm_mux_net_tx_timeout(struct net_device *net)
2677{
2678        /* Tell syslog we are hosed. */
2679        dev_dbg(&net->dev, "Tx timed out.\n");
2680
2681        /* Update statistics */
2682        STATS(net).tx_errors++;
2683}
2684
2685static void gsm_mux_rx_netchar(struct gsm_dlci *dlci,
2686                                   unsigned char *in_buf, int size)
2687{
2688        struct net_device *net = dlci->net;
2689        struct sk_buff *skb;
2690        struct gsm_mux_net *mux_net = (struct gsm_mux_net *)netdev_priv(net);
2691        muxnet_get(mux_net);
2692
2693        /* Allocate an sk_buff */
2694        skb = dev_alloc_skb(size + NET_IP_ALIGN);
2695        if (!skb) {
2696                /* We got no receive buffer. */
2697                STATS(net).rx_dropped++;
2698                muxnet_put(mux_net);
2699                return;
2700        }
2701        skb_reserve(skb, NET_IP_ALIGN);
2702        memcpy(skb_put(skb, size), in_buf, size);
2703
2704        skb->dev = net;
2705        skb->protocol = __constant_htons(ETH_P_IP);
2706
2707        /* Ship it off to the kernel */
2708        netif_rx(skb);
2709
2710        /* update out statistics */
2711        STATS(net).rx_packets++;
2712        STATS(net).rx_bytes += size;
2713        muxnet_put(mux_net);
2714        return;
2715}
2716
2717int gsm_change_mtu(struct net_device *net, int new_mtu)
2718{
2719        struct gsm_mux_net *mux_net = (struct gsm_mux_net *)netdev_priv(net);
2720        if ((new_mtu < 8) || (new_mtu > mux_net->dlci->gsm->mtu))
2721                return -EINVAL;
2722        net->mtu = new_mtu;
2723        return 0;
2724}
2725
2726static void gsm_mux_net_init(struct net_device *net)
2727{
2728        static const struct net_device_ops gsm_netdev_ops = {
2729                .ndo_open               = gsm_mux_net_open,
2730                .ndo_stop               = gsm_mux_net_close,
2731                .ndo_start_xmit         = gsm_mux_net_start_xmit,
2732                .ndo_tx_timeout         = gsm_mux_net_tx_timeout,
2733                .ndo_get_stats          = gsm_mux_net_get_stats,
2734                .ndo_change_mtu         = gsm_change_mtu,
2735        };
2736
2737        net->netdev_ops = &gsm_netdev_ops;
2738
2739        /* fill in the other fields */
2740        net->watchdog_timeo = GSM_NET_TX_TIMEOUT;
2741        net->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
2742        net->type = ARPHRD_NONE;
2743        net->tx_queue_len = 10;
2744}
2745
2746
2747/* caller holds the dlci mutex */
2748static void gsm_destroy_network(struct gsm_dlci *dlci)
2749{
2750        struct gsm_mux_net *mux_net;
2751
2752        pr_debug("destroy network interface");
2753        if (!dlci->net)
2754                return;
2755        mux_net = (struct gsm_mux_net *)netdev_priv(dlci->net);
2756        muxnet_put(mux_net);
2757}
2758
2759
2760/* caller holds the dlci mutex */
2761static int gsm_create_network(struct gsm_dlci *dlci, struct gsm_netconfig *nc)
2762{
2763        char *netname;
2764        int retval = 0;
2765        struct net_device *net;
2766        struct gsm_mux_net *mux_net;
2767
2768        if (!capable(CAP_NET_ADMIN))
2769                return -EPERM;
2770
2771        /* Already in a non tty mode */
2772        if (dlci->adaption > 2)
2773                return -EBUSY;
2774
2775        if (nc->protocol != htons(ETH_P_IP))
2776                return -EPROTONOSUPPORT;
2777
2778        if (nc->adaption != 3 && nc->adaption != 4)
2779                return -EPROTONOSUPPORT;
2780
2781        pr_debug("create network interface");
2782
2783        netname = "gsm%d";
2784        if (nc->if_name[0] != '\0')
2785                netname = nc->if_name;
2786        net = alloc_netdev(sizeof(struct gsm_mux_net),
2787                        netname,
2788                        gsm_mux_net_init);
2789        if (!net) {
2790                pr_err("alloc_netdev failed");
2791                return -ENOMEM;
2792        }
2793        net->mtu = dlci->gsm->mtu;
2794        mux_net = (struct gsm_mux_net *)netdev_priv(net);
2795        mux_net->dlci = dlci;
2796        kref_init(&mux_net->ref);
2797        strncpy(nc->if_name, net->name, IFNAMSIZ); /* return net name */
2798
2799        /* reconfigure dlci for network */
2800        dlci->prev_adaption = dlci->adaption;
2801        dlci->prev_data = dlci->data;
2802        dlci->adaption = nc->adaption;
2803        dlci->data = gsm_mux_rx_netchar;
2804        dlci->net = net;
2805
2806        pr_debug("register netdev");
2807        retval = register_netdev(net);
2808        if (retval) {
2809                pr_err("network register fail %d\n", retval);
2810                dlci_net_free(dlci);
2811                return retval;
2812        }
2813        return net->ifindex;    /* return network index */
2814}
2815
2816/* Line discipline for real tty */
2817struct tty_ldisc_ops tty_ldisc_packet = {
2818        .owner           = THIS_MODULE,
2819        .magic           = TTY_LDISC_MAGIC,
2820        .name            = "n_gsm",
2821        .open            = gsmld_open,
2822        .close           = gsmld_close,
2823        .flush_buffer    = gsmld_flush_buffer,
2824        .chars_in_buffer = gsmld_chars_in_buffer,
2825        .read            = gsmld_read,
2826        .write           = gsmld_write,
2827        .ioctl           = gsmld_ioctl,
2828        .poll            = gsmld_poll,
2829        .receive_buf     = gsmld_receive_buf,
2830        .write_wakeup    = gsmld_write_wakeup
2831};
2832
2833/*
2834 *      Virtual tty side
2835 */
2836
2837#define TX_SIZE         512
2838
2839static int gsmtty_modem_update(struct gsm_dlci *dlci, u8 brk)
2840{
2841        u8 modembits[5];
2842        struct gsm_control *ctrl;
2843        int len = 2;
2844
2845        if (brk)
2846                len++;
2847
2848        modembits[0] = len << 1 | EA;           /* Data bytes */
2849        modembits[1] = dlci->addr << 2 | 3;     /* DLCI, EA, 1 */
2850        modembits[2] = gsm_encode_modem(dlci) << 1 | EA;
2851        if (brk)
2852                modembits[3] = brk << 4 | 2 | EA;       /* Valid, EA */
2853        ctrl = gsm_control_send(dlci->gsm, CMD_MSC, modembits, len + 1);
2854        if (ctrl == NULL)
2855                return -ENOMEM;
2856        return gsm_control_wait(dlci->gsm, ctrl);
2857}
2858
2859static int gsm_carrier_raised(struct tty_port *port)
2860{
2861        struct gsm_dlci *dlci = container_of(port, struct gsm_dlci, port);
2862        /* Not yet open so no carrier info */
2863        if (dlci->state != DLCI_OPEN)
2864                return 0;
2865        if (debug & 2)
2866                return 1;
2867        return dlci->modem_rx & TIOCM_CD;
2868}
2869
2870static void gsm_dtr_rts(struct tty_port *port, int onoff)
2871{
2872        struct gsm_dlci *dlci = container_of(port, struct gsm_dlci, port);
2873        unsigned int modem_tx = dlci->modem_tx;
2874        if (onoff)
2875                modem_tx |= TIOCM_DTR | TIOCM_RTS;
2876        else
2877                modem_tx &= ~(TIOCM_DTR | TIOCM_RTS);
2878        if (modem_tx != dlci->modem_tx) {
2879                dlci->modem_tx = modem_tx;
2880                gsmtty_modem_update(dlci, 0);
2881        }
2882}
2883
2884static const struct tty_port_operations gsm_port_ops = {
2885        .carrier_raised = gsm_carrier_raised,
2886        .dtr_rts = gsm_dtr_rts,
2887        .destruct = gsm_dlci_free,
2888};
2889
2890static int gsmtty_install(struct tty_driver *driver, struct tty_struct *tty)
2891{
2892        struct gsm_mux *gsm;
2893        struct gsm_dlci *dlci;
2894        unsigned int line = tty->index;
2895        unsigned int mux = line >> 6;
2896        bool alloc = false;
2897        int ret;
2898
2899        line = line & 0x3F;
2900
2901        if (mux >= MAX_MUX)
2902                return -ENXIO;
2903        /* FIXME: we need to lock gsm_mux for lifetimes of ttys eventually */
2904        if (gsm_mux[mux] == NULL)
2905                return -EUNATCH;
2906        if (line == 0 || line > 61)     /* 62/63 reserved */
2907                return -ECHRNG;
2908        gsm = gsm_mux[mux];
2909        if (gsm->dead)
2910                return -EL2HLT;
2911        /* If DLCI 0 is not yet fully open return an error. This is ok from a locking
2912           perspective as we don't have to worry about this if DLCI0 is lost */
2913        if (gsm->dlci[0] && gsm->dlci[0]->state != DLCI_OPEN) 
2914                return -EL2NSYNC;
2915        dlci = gsm->dlci[line];
2916        if (dlci == NULL) {
2917                alloc = true;
2918                dlci = gsm_dlci_alloc(gsm, line);
2919        }
2920        if (dlci == NULL)
2921                return -ENOMEM;
2922        ret = tty_port_install(&dlci->port, driver, tty);
2923        if (ret) {
2924                if (alloc)
2925                        dlci_put(dlci);
2926                return ret;
2927        }
2928
2929        tty->driver_data = dlci;
2930
2931        return 0;
2932}
2933
2934static int gsmtty_open(struct tty_struct *tty, struct file *filp)
2935{
2936        struct gsm_dlci *dlci = tty->driver_data;
2937        struct tty_port *port = &dlci->port;
2938
2939        port->count++;
2940        dlci_get(dlci);
2941        dlci_get(dlci->gsm->dlci[0]);
2942        mux_get(dlci->gsm);
2943        tty_port_tty_set(port, tty);
2944
2945        dlci->modem_rx = 0;
2946        /* We could in theory open and close before we wait - eg if we get
2947           a DM straight back. This is ok as that will have caused a hangup */
2948        set_bit(ASYNCB_INITIALIZED, &port->flags);
2949        /* Start sending off SABM messages */
2950        gsm_dlci_begin_open(dlci);
2951        /* And wait for virtual carrier */
2952        return tty_port_block_til_ready(port, tty, filp);
2953}
2954
2955static void gsmtty_close(struct tty_struct *tty, struct file *filp)
2956{
2957        struct gsm_dlci *dlci = tty->driver_data;
2958        struct gsm_mux *gsm;
2959
2960        if (dlci == NULL)
2961                return;
2962        if (dlci->state == DLCI_CLOSED)
2963                return;
2964        mutex_lock(&dlci->mutex);
2965        gsm_destroy_network(dlci);
2966        mutex_unlock(&dlci->mutex);
2967        gsm = dlci->gsm;
2968        if (tty_port_close_start(&dlci->port, tty, filp) == 0)
2969                goto out;
2970        gsm_dlci_begin_close(dlci);
2971        tty_port_close_end(&dlci->port, tty);
2972        tty_port_tty_set(&dlci->port, NULL);
2973out:
2974        dlci_put(dlci);
2975        dlci_put(gsm->dlci[0]);
2976        mux_put(gsm);
2977}
2978
2979static void gsmtty_hangup(struct tty_struct *tty)
2980{
2981        struct gsm_dlci *dlci = tty->driver_data;
2982        if (dlci->state == DLCI_CLOSED)
2983                return;
2984        tty_port_hangup(&dlci->port);
2985        gsm_dlci_begin_close(dlci);
2986}
2987
2988static int gsmtty_write(struct tty_struct *tty, const unsigned char *buf,
2989                                                                    int len)
2990{
2991        int sent;
2992        struct gsm_dlci *dlci = tty->driver_data;
2993        if (dlci->state == DLCI_CLOSED)
2994                return -EINVAL;
2995        /* Stuff the bytes into the fifo queue */
2996        sent = kfifo_in_locked(dlci->fifo, buf, len, &dlci->lock);
2997        /* Need to kick the channel */
2998        gsm_dlci_data_kick(dlci);
2999        return sent;
3000}
3001
3002static int gsmtty_write_room(struct tty_struct *tty)
3003{
3004        struct gsm_dlci *dlci = tty->driver_data;
3005        if (dlci->state == DLCI_CLOSED)
3006                return -EINVAL;
3007        return TX_SIZE - kfifo_len(dlci->fifo);
3008}
3009
3010static int gsmtty_chars_in_buffer(struct tty_struct *tty)
3011{
3012        struct gsm_dlci *dlci = tty->driver_data;
3013        if (dlci->state == DLCI_CLOSED)
3014                return -EINVAL;
3015        return kfifo_len(dlci->fifo);
3016}
3017
3018static void gsmtty_flush_buffer(struct tty_struct *tty)
3019{
3020        struct gsm_dlci *dlci = tty->driver_data;
3021        if (dlci->state == DLCI_CLOSED)
3022                return;
3023        /* Caution needed: If we implement reliable transport classes
3024           then the data being transmitted can't simply be junked once
3025           it has first hit the stack. Until then we can just blow it
3026           away */
3027        kfifo_reset(dlci->fifo);
3028        /* Need to unhook this DLCI from the transmit queue logic */
3029}
3030
3031static void gsmtty_wait_until_sent(struct tty_struct *tty, int timeout)
3032{
3033        /* The FIFO handles the queue so the kernel will do the right
3034           thing waiting on chars_in_buffer before calling us. No work
3035           to do here */
3036}
3037
3038static int gsmtty_tiocmget(struct tty_struct *tty)
3039{
3040        struct gsm_dlci *dlci = tty->driver_data;
3041        if (dlci->state == DLCI_CLOSED)
3042                return -EINVAL;
3043        return dlci->modem_rx;
3044}
3045
3046static int gsmtty_tiocmset(struct tty_struct *tty,
3047        unsigned int set, unsigned int clear)
3048{
3049        struct gsm_dlci *dlci = tty->driver_data;
3050        unsigned int modem_tx = dlci->modem_tx;
3051
3052        if (dlci->state == DLCI_CLOSED)
3053                return -EINVAL;
3054        modem_tx &= ~clear;
3055        modem_tx |= set;
3056
3057        if (modem_tx != dlci->modem_tx) {
3058                dlci->modem_tx = modem_tx;
3059                return gsmtty_modem_update(dlci, 0);
3060        }
3061        return 0;
3062}
3063
3064
3065static int gsmtty_ioctl(struct tty_struct *tty,
3066                        unsigned int cmd, unsigned long arg)
3067{
3068        struct gsm_dlci *dlci = tty->driver_data;
3069        struct gsm_netconfig nc;
3070        int index;
3071
3072        if (dlci->state == DLCI_CLOSED)
3073                return -EINVAL;
3074        switch (cmd) {
3075        case GSMIOC_ENABLE_NET:
3076                if (copy_from_user(&nc, (void __user *)arg, sizeof(nc)))
3077                        return -EFAULT;
3078                nc.if_name[IFNAMSIZ-1] = '\0';
3079                /* return net interface index or error code */
3080                mutex_lock(&dlci->mutex);
3081                index = gsm_create_network(dlci, &nc);
3082                mutex_unlock(&dlci->mutex);
3083                if (copy_to_user((void __user *)arg, &nc, sizeof(nc)))
3084                        return -EFAULT;
3085                return index;
3086        case GSMIOC_DISABLE_NET:
3087                if (!capable(CAP_NET_ADMIN))
3088                        return -EPERM;
3089                mutex_lock(&dlci->mutex);
3090                gsm_destroy_network(dlci);
3091                mutex_unlock(&dlci->mutex);
3092                return 0;
3093        default:
3094                return -ENOIOCTLCMD;
3095        }
3096}
3097
3098static void gsmtty_set_termios(struct tty_struct *tty, struct ktermios *old)
3099{
3100        struct gsm_dlci *dlci = tty->driver_data;
3101        if (dlci->state == DLCI_CLOSED)
3102                return;
3103        /* For the moment its fixed. In actual fact the speed information
3104           for the virtual channel can be propogated in both directions by
3105           the RPN control message. This however rapidly gets nasty as we
3106           then have to remap modem signals each way according to whether
3107           our virtual cable is null modem etc .. */
3108        tty_termios_copy_hw(&tty->termios, old);
3109}
3110
3111static void gsmtty_throttle(struct tty_struct *tty)
3112{
3113        struct gsm_dlci *dlci = tty->driver_data;
3114        if (dlci->state == DLCI_CLOSED)
3115                return;
3116        if (tty->termios.c_cflag & CRTSCTS)
3117                dlci->modem_tx &= ~TIOCM_DTR;
3118        dlci->throttled = 1;
3119        /* Send an MSC with DTR cleared */
3120        gsmtty_modem_update(dlci, 0);
3121}
3122
3123static void gsmtty_unthrottle(struct tty_struct *tty)
3124{
3125        struct gsm_dlci *dlci = tty->driver_data;
3126        if (dlci->state == DLCI_CLOSED)
3127                return;
3128        if (tty->termios.c_cflag & CRTSCTS)
3129                dlci->modem_tx |= TIOCM_DTR;
3130        dlci->throttled = 0;
3131        /* Send an MSC with DTR set */
3132        gsmtty_modem_update(dlci, 0);
3133}
3134
3135static int gsmtty_break_ctl(struct tty_struct *tty, int state)
3136{
3137        struct gsm_dlci *dlci = tty->driver_data;
3138        int encode = 0; /* Off */
3139        if (dlci->state == DLCI_CLOSED)
3140                return -EINVAL;
3141
3142        if (state == -1)        /* "On indefinitely" - we can't encode this
3143                                    properly */
3144                encode = 0x0F;
3145        else if (state > 0) {
3146                encode = state / 200;   /* mS to encoding */
3147                if (encode > 0x0F)
3148                        encode = 0x0F;  /* Best effort */
3149        }
3150        return gsmtty_modem_update(dlci, encode);
3151}
3152
3153
3154/* Virtual ttys for the demux */
3155static const struct tty_operations gsmtty_ops = {
3156        .install                = gsmtty_install,
3157        .open                   = gsmtty_open,
3158        .close                  = gsmtty_close,
3159        .write                  = gsmtty_write,
3160        .write_room             = gsmtty_write_room,
3161        .chars_in_buffer        = gsmtty_chars_in_buffer,
3162        .flush_buffer           = gsmtty_flush_buffer,
3163        .ioctl                  = gsmtty_ioctl,
3164        .throttle               = gsmtty_throttle,
3165        .unthrottle             = gsmtty_unthrottle,
3166        .set_termios            = gsmtty_set_termios,
3167        .hangup                 = gsmtty_hangup,
3168        .wait_until_sent        = gsmtty_wait_until_sent,
3169        .tiocmget               = gsmtty_tiocmget,
3170        .tiocmset               = gsmtty_tiocmset,
3171        .break_ctl              = gsmtty_break_ctl,
3172};
3173
3174
3175
3176static int __init gsm_init(void)
3177{
3178        /* Fill in our line protocol discipline, and register it */
3179        int status = tty_register_ldisc(N_GSM0710, &tty_ldisc_packet);
3180        if (status != 0) {
3181                pr_err("n_gsm: can't register line discipline (err = %d)\n",
3182                                                                status);
3183                return status;
3184        }
3185
3186        gsm_tty_driver = alloc_tty_driver(256);
3187        if (!gsm_tty_driver) {
3188                tty_unregister_ldisc(N_GSM0710);
3189                pr_err("gsm_init: tty allocation failed.\n");
3190                return -EINVAL;
3191        }
3192        gsm_tty_driver->driver_name     = "gsmtty";
3193        gsm_tty_driver->name            = "gsmtty";
3194        gsm_tty_driver->major           = 0;    /* Dynamic */
3195        gsm_tty_driver->minor_start     = 0;
3196        gsm_tty_driver->type            = TTY_DRIVER_TYPE_SERIAL;
3197        gsm_tty_driver->subtype = SERIAL_TYPE_NORMAL;
3198        gsm_tty_driver->flags   = TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV
3199                                                | TTY_DRIVER_HARDWARE_BREAK;
3200        gsm_tty_driver->init_termios    = tty_std_termios;
3201        /* Fixme */
3202        gsm_tty_driver->init_termios.c_lflag &= ~ECHO;
3203        tty_set_operations(gsm_tty_driver, &gsmtty_ops);
3204
3205        spin_lock_init(&gsm_mux_lock);
3206
3207        if (tty_register_driver(gsm_tty_driver)) {
3208                put_tty_driver(gsm_tty_driver);
3209                tty_unregister_ldisc(N_GSM0710);
3210                pr_err("gsm_init: tty registration failed.\n");
3211                return -EBUSY;
3212        }
3213        pr_debug("gsm_init: loaded as %d,%d.\n",
3214                        gsm_tty_driver->major, gsm_tty_driver->minor_start);
3215        return 0;
3216}
3217
3218static void __exit gsm_exit(void)
3219{
3220        int status = tty_unregister_ldisc(N_GSM0710);
3221        if (status != 0)
3222                pr_err("n_gsm: can't unregister line discipline (err = %d)\n",
3223                                                                status);
3224        tty_unregister_driver(gsm_tty_driver);
3225        put_tty_driver(gsm_tty_driver);
3226}
3227
3228module_init(gsm_init);
3229module_exit(gsm_exit);
3230
3231
3232MODULE_LICENSE("GPL");
3233MODULE_ALIAS_LDISC(N_GSM0710);
3234
lxr.linux.no kindly hosted by Redpill Linpro AS, provider of Linux consulting and operations services since 1995.