sllin/sllin.c

   1 /*
   2  * sllin.c - serial line LIN interface driver (using tty line discipline)
   3  *
   4  * This file is derived from drivers/net/can/slcan.c
   5  * slcan.c Author: Oliver Hartkopp <socketcan@hartkopp.net>
   6  *
   7  * This program is free software; you can redistribute it and/or modify it
   8  * under the terms of the GNU General Public License as published by the
   9  * Free Software Foundation; either version 2 of the License, or (at your
  10  * option) any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful, but
  13  * WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  15  * General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License along
  18  * with this program; if not, write to the Free Software Foundation, Inc.,
  19  * 59 Temple Place, Suite 330, Boston, MA 02111-1307. You can also get it
  20  * at http://www.gnu.org/licenses/gpl.html
  21  *
  22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  25  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  26  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  27  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  28  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  29  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  30  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  31  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  32  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  33  * DAMAGE.
  34  *
  35  * Idea:       Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
  36  * Copyright:  (c) 2011 Czech Technical University in Prague
  37  *             (c) 2011 Volkswagen Group Research
  38  * Authors:    Pavel Pisa <pisa@cmp.felk.cvut.cz>
  39  *             Rostislav Lisovy <lisovy@kormus.cz>
  40  *             Michal Sojka <sojkam1@fel.cvut.cz>
  41  * Funded by:  Volkswagen Group Research
  42  */
  43
  44 #define DEBUG                   1 /* Enables pr_debug() printouts */
  45
  46 #include <linux/module.h>
  47 #include <linux/moduleparam.h>
  48
  49 #include <asm/system.h>
  50 #include <linux/uaccess.h>
  51 #include <linux/bitops.h>
  52 #include <linux/string.h>
  53 #include <linux/tty.h>
  54 #include <linux/errno.h>
  55 #include <linux/netdevice.h>
  56 #include <linux/skbuff.h>
  57 #include <linux/rtnetlink.h>
  58 #include <linux/if_arp.h>
  59 #include <linux/if_ether.h>
  60 #include <linux/sched.h>
  61 #include <linux/delay.h>
  62 #include <linux/init.h>
  63 #include <linux/can.h>
  64 #include <linux/kthread.h>
  65 #include <linux/hrtimer.h>
  66 #include "linux/lin_bus.h"
  67
  68 /* Should be in include/linux/tty.h */
  69 #define N_SLLIN                 25
  70 /* -------------------------------- */
  71
  72 static __initdata const char banner[] =
  73         KERN_INFO "sllin: serial line LIN interface driver\n";
  74
  75 MODULE_ALIAS_LDISC(N_SLLIN);
  76 MODULE_DESCRIPTION("serial line LIN interface");
  77 MODULE_LICENSE("GPL");
  78 MODULE_AUTHOR("Pavel Pisa <pisa@cmp.felk.cvut.cz>");
  79
  80 #define SLLIN_MAGIC             0x53CA
  81 /* #define BREAK_BY_BAUD */
  82
  83 static int master = true;
  84 static int baudrate; /* Use LIN_DEFAULT_BAUDRATE when not set */
  85
  86 module_param(master, bool, 0);
  87 MODULE_PARM_DESC(master, "LIN interface is Master device");
  88 module_param(baudrate, int, 0);
  89 MODULE_PARM_DESC(baudrate, "Baudrate of LIN interface");
  90
  91 static int maxdev = 10;         /* MAX number of SLLIN channels;
  92                                    This can be overridden with
  93                                    insmod sllin.ko maxdev=nnn   */
  94 module_param(maxdev, int, 0);
  95 MODULE_PARM_DESC(maxdev, "Maximum number of sllin interfaces");
  96
  97 /* maximum buffer len to store whole LIN message*/
  98 #define SLLIN_DATA_MAX          8
  99 #define SLLIN_BUFF_LEN          (1 /*break*/ + 1 /*sync*/ + 1 /*ID*/ + \
 100                                 SLLIN_DATA_MAX + 1 /*checksum*/)
 101 #define SLLIN_BUFF_BREAK        0
 102 #define SLLIN_BUFF_SYNC         1
 103 #define SLLIN_BUFF_ID           2
 104 #define SLLIN_BUFF_DATA         3
 105
 106 #define SLLIN_SAMPLES_PER_CHAR  10
 107 #define SLLIN_CHARS_TO_TIMEOUT  24
 108
 109 enum slstate {
 110         SLSTATE_IDLE = 0,
 111         SLSTATE_BREAK_SENT,
 112         SLSTATE_ID_SENT,
 113         SLSTATE_RESPONSE_WAIT, /* Wait for response */
 114         SLSTATE_RESPONSE_WAIT_BUS, /* Wait for response from LIN bus
 115                                 only (CAN frames from network stack
 116                                 are not processed in this moment) */
 117         SLSTATE_RESPONSE_SENT,
 118 };
 119
 120 struct sllin_conf_entry {
 121         int dlc;                /* Length of data in LIN frame */
 122         canid_t frame_fl;       /* LIN frame flags. Passed from userspace as
 123                                    canid_t data type */
 124         u8 data[8];             /* LIN frame data payload */
 125 };
 126
 127 struct sllin {
 128         int                     magic;
 129
 130         /* Various fields. */
 131         struct tty_struct       *tty;           /* ptr to TTY structure      */
 132         struct net_device       *dev;           /* easy for intr handling    */
 133         spinlock_t              lock;
 134
 135         /* LIN message buffer and actual processed data counts */
 136         unsigned char           rx_buff[SLLIN_BUFF_LEN]; /* LIN Rx buffer */
 137         unsigned char           tx_buff[SLLIN_BUFF_LEN]; /* LIN Tx buffer */
 138         int                     rx_expect;      /* expected number of Rx chars */
 139         int                     rx_lim;         /* maximum Rx chars for current frame */
 140         int                     rx_cnt;         /* message buffer Rx fill level  */
 141         int                     tx_lim;         /* actual limit of bytes to Tx */
 142         int                     tx_cnt;         /* number of already Tx bytes */
 143         char                    lin_master;     /* node is a master node */
 144         int                     lin_baud;       /* LIN baudrate */
 145         int                     lin_state;      /* state */
 146         char                    id_to_send;     /* there is ID to be sent */
 147         char                    data_to_send;   /* there are data to be sent */
 148         char                    resp_len_known; /* Length of the response is known */
 149         char                    header_received;/* In Slave mode, set when header was already
 150                                                    received */
 151         char                    rx_len_unknown; /* We are not sure how much data will be sent to us --
 152                                                    we just guess the length */
 153
 154         unsigned long           flags;          /* Flag values/ mode etc     */
 155 #define SLF_INUSE               0               /* Channel in use            */
 156 #define SLF_ERROR               1               /* Parity, etc. error        */
 157 #define SLF_RXEVENT             2               /* Rx wake event             */
 158 #define SLF_TXEVENT             3               /* Tx wake event             */
 159 #define SLF_MSGEVENT            4               /* CAN message to sent       */
 160 #define SLF_TMOUTEVENT          5               /* Timeout on received data  */
 161 #define SLF_TXBUFF_RQ           6               /* Req. to send buffer to UART*/
 162 #define SLF_TXBUFF_INPR         7               /* Above request in progress */
 163
 164         dev_t                   line;
 165         struct task_struct      *kwthread;
 166         wait_queue_head_t       kwt_wq;         /* Wait queue used by kwthread */
 167         struct hrtimer          rx_timer;       /* RX timeout timer */
 168         ktime_t                 rx_timer_timeout; /* RX timeout timer value */
 169         struct sk_buff          *tx_req_skb;    /* Socket buffer with CAN frame
 170                                                 received from network stack*/
 171
 172         /* List with configurations for each of 0 to LIN_ID_MAX LIN IDs */
 173         struct sllin_conf_entry linfr_cache[LIN_ID_MAX + 1];
 174         spinlock_t              linfr_lock;     /* frame cache and buffers lock */
 175 };
 176
 177 static struct net_device **sllin_devs;
 178 static int sllin_configure_frame_cache(struct sllin *sl, struct can_frame *cf);
 179
 180
 181 /* Values of two parity bits in LIN Protected
 182    Identifier for each particular LIN ID */
 183 const unsigned char sllin_id_parity_table[] = {
 184         0x80, 0xc0, 0x40, 0x00, 0xc0, 0x80, 0x00, 0x40,
 185         0x00, 0x40, 0xc0, 0x80, 0x40, 0x00, 0x80, 0xc0,
 186         0x40, 0x00, 0x80, 0xc0, 0x00, 0x40, 0xc0, 0x80,
 187         0xc0, 0x80, 0x00, 0x40, 0x80, 0xc0, 0x40, 0x00,
 188         0x00, 0x40, 0xc0, 0x80, 0x40, 0x00, 0x80, 0xc0,
 189         0x80, 0xc0, 0x40, 0x00, 0xc0, 0x80, 0x00, 0x40,
 190         0xc0, 0x80, 0x00, 0x40, 0x80, 0xc0, 0x40, 0x00,
 191         0x40, 0x00, 0x80, 0xc0, 0x00, 0x40, 0xc0, 0x80
 192 };
 193
 194 /**
 195  * sltty_change_speed() -- Change baudrate of Serial device belonging
 196  *                         to particular @tty
 197  *
 198  * @tty:        Pointer to TTY to change speed for.
 199  * @speed:      Integer value of new speed. It is possible to
 200  *              assign non-standard values, i.e. those which
 201  *              are not defined in termbits.h.
 202  */
 203 static int sltty_change_speed(struct tty_struct *tty, unsigned speed)
 204 {
 205         struct ktermios old_termios;
 206         int cflag;
 207
 208         mutex_lock(&tty->termios_mutex);
 209
 210         old_termios = *(tty->termios);
 211
 212         cflag = CS8 | CREAD | CLOCAL | HUPCL;
 213         cflag &= ~(CBAUD | CIBAUD);
 214         cflag |= BOTHER;
 215         tty->termios->c_cflag = cflag;
 216         tty->termios->c_oflag = 0;
 217         tty->termios->c_lflag = 0;
 218
 219         /* Enable interrupt when UART-Break or Framing error received */
 220         tty->termios->c_iflag = BRKINT | INPCK;
 221
 222         tty_encode_baud_rate(tty, speed, speed);
 223
 224         if (tty->ops->set_termios)
 225                 tty->ops->set_termios(tty, &old_termios);
 226
 227         mutex_unlock(&tty->termios_mutex);
 228
 229         return 0;
 230 }
 231
 232 /* Send one can_frame to the network layer */
 233 static void sllin_send_canfr(struct sllin *sl, canid_t id, char *data, int len)
 234 {
 235         struct sk_buff *skb;
 236         struct can_frame cf;
 237
 238         cf.can_id = id;
 239         cf.can_dlc = len;
 240         if (cf.can_dlc > 0)
 241                 memcpy(&cf.data, data, cf.can_dlc);
 242
 243         skb = dev_alloc_skb(sizeof(struct can_frame));
 244         if (!skb)
 245                 return;
 246
 247         skb->dev = sl->dev;
 248         skb->protocol = htons(ETH_P_CAN);
 249         skb->pkt_type = PACKET_BROADCAST;
 250         skb->ip_summed = CHECKSUM_UNNECESSARY;
 251         memcpy(skb_put(skb, sizeof(struct can_frame)),
 252                &cf, sizeof(struct can_frame));
 253         netif_rx(skb);
 254
 255         sl->dev->stats.rx_packets++;
 256         sl->dev->stats.rx_bytes += cf.can_dlc;
 257 }
 258
 259 /**
 260  * sll_bump() -- Send data of received LIN frame (existing in sl->rx_buff)
 261  *               as CAN frame
 262  *
 263  * @sl:
 264  */
 265 static void sll_bump(struct sllin *sl)
 266 {
 267         int len = sl->rx_cnt - SLLIN_BUFF_DATA - 1; /* without checksum */
 268         len = (len < 0) ? 0 : len;
 269
 270         sllin_send_canfr(sl, sl->rx_buff[SLLIN_BUFF_ID] & LIN_ID_MASK,
 271                 sl->rx_buff + SLLIN_BUFF_DATA, len);
 272 }
 273
 274 static void sll_send_rtr(struct sllin *sl)
 275 {
 276         sllin_send_canfr(sl, (sl->rx_buff[SLLIN_BUFF_ID] & LIN_ID_MASK) |
 277                 CAN_RTR_FLAG, NULL, 0);
 278 }
 279
 280 /*
 281  * Called by the driver when there's room for more data.  If we have
 282  * more packets to send, we send them here.
 283  */
 284 static void sllin_write_wakeup(struct tty_struct *tty)
 285 {
 286         int actual = 0;
 287         int remains;
 288         struct sllin *sl = (struct sllin *) tty->disc_data;
 289
 290         /* First make sure we're connected. */
 291         if (!sl || sl->magic != SLLIN_MAGIC || !netif_running(sl->dev))
 292                 return;
 293
 294         set_bit(SLF_TXBUFF_RQ, &sl->flags);
 295         do {
 296                 if (unlikely(test_and_set_bit(SLF_TXBUFF_INPR, &sl->flags)))
 297                         return; /* ongoing concurrent processing */
 298
 299                 clear_bit(SLF_TXBUFF_RQ, &sl->flags);
 300                 smp_mb__after_clear_bit();
 301
 302                 if (sl->lin_state != SLSTATE_BREAK_SENT)
 303                         remains = sl->tx_lim - sl->tx_cnt;
 304                 else
 305                         remains = SLLIN_BUFF_BREAK + 1 - sl->tx_cnt;
 306
 307                 if (remains > 0) {
 308                         actual = tty->ops->write(tty, sl->tx_buff + sl->tx_cnt,
 309                                 sl->tx_cnt - sl->tx_lim);
 310                         sl->tx_cnt += actual;
 311                         remains -= actual;
 312                 }
 313                 clear_bit(SLF_TXBUFF_INPR, &sl->flags);
 314                 smp_mb__after_clear_bit();
 315
 316         } while (unlikely(test_bit(SLF_TXBUFF_RQ, &sl->flags)));
 317
 318         if ((remains > 0) && (actual >= 0)) {
 319                 pr_debug("sllin: sllin_write_wakeup sent %d, "
 320                         "remains %d, waiting\n",
 321                         sl->tx_cnt, sl->tx_lim - sl->tx_cnt);
 322                 return;
 323         }
 324
 325         clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
 326         set_bit(SLF_TXEVENT, &sl->flags);
 327         wake_up(&sl->kwt_wq);
 328
 329         pr_debug("sllin: sllin_write_wakeup sent %d, wakeup\n", sl->tx_cnt);
 330 }
 331
 332 /**
 333  * sll_xmit() -- Send a can_frame to a TTY queue.
 334  *
 335  * @skb: Pointer to Socket buffer to be sent.
 336  * @dev: Network device where @skb will be sent.
 337  */
 338 static netdev_tx_t sll_xmit(struct sk_buff *skb, struct net_device *dev)
 339 {
 340         struct sllin *sl = netdev_priv(dev);
 341         struct can_frame *cf;
 342
 343         if (skb->len != sizeof(struct can_frame))
 344                 goto err_out;
 345
 346         spin_lock(&sl->lock);
 347         if (!netif_running(dev))  {
 348                 printk(KERN_WARNING "%s: xmit: iface is down\n", dev->name);
 349                 goto err_out_unlock;
 350         }
 351         if (sl->tty == NULL) {
 352                 printk(KERN_WARNING "%s: xmit: no tty device connected\n", dev->name);
 353                 goto err_out_unlock;
 354         }
 355
 356         cf = (struct can_frame *) skb->data;
 357         if (cf->can_id & LIN_CTRL_FRAME) {
 358                 sllin_configure_frame_cache(sl, cf);
 359                 goto free_out_unlock;
 360         }
 361
 362         netif_stop_queue(sl->dev);
 363
 364         sl->tx_req_skb = skb;
 365         set_bit(SLF_MSGEVENT, &sl->flags);
 366         wake_up(&sl->kwt_wq);
 367         spin_unlock(&sl->lock);
 368         return NETDEV_TX_OK;
 369
 370 free_out_unlock:
 371 err_out_unlock:
 372         spin_unlock(&sl->lock);
 373 err_out:
 374         kfree_skb(skb);
 375         return NETDEV_TX_OK;
 376 }
 377
 378
 379 /******************************************
 380  *   Routines looking at netdevice side.
 381  ******************************************/
 382
 383 /* Netdevice UP -> DOWN routine */
 384 static int sll_close(struct net_device *dev)
 385 {
 386         struct sllin *sl = netdev_priv(dev);
 387
 388         spin_lock_bh(&sl->lock);
 389         if (sl->tty) {
 390                 /* TTY discipline is running. */
 391                 clear_bit(TTY_DO_WRITE_WAKEUP, &sl->tty->flags);
 392         }
 393         netif_stop_queue(dev);
 394         sl->rx_expect = 0;
 395         sl->tx_lim    = 0;
 396         spin_unlock_bh(&sl->lock);
 397
 398         return 0;
 399 }
 400
 401 /* Netdevice DOWN -> UP routine */
 402 static int sll_open(struct net_device *dev)
 403 {
 404         struct sllin *sl = netdev_priv(dev);
 405
 406         pr_debug("sllin: %s() invoked\n", __func__);
 407
 408         if (sl->tty == NULL)
 409                 return -ENODEV;
 410
 411         sl->flags &= (1 << SLF_INUSE);
 412         netif_start_queue(dev);
 413         return 0;
 414 }
 415
 416 /* Hook the destructor so we can free sllin devs at the right point in time */
 417 static void sll_free_netdev(struct net_device *dev)
 418 {
 419         int i = dev->base_addr;
 420         free_netdev(dev);
 421         sllin_devs[i] = NULL;
 422 }
 423
 424 static const struct net_device_ops sll_netdev_ops = {
 425         .ndo_open               = sll_open,
 426         .ndo_stop               = sll_close,
 427         .ndo_start_xmit         = sll_xmit,
 428 };
 429
 430 static void sll_setup(struct net_device *dev)
 431 {
 432         dev->netdev_ops         = &sll_netdev_ops;
 433         dev->destructor         = sll_free_netdev;
 434
 435         dev->hard_header_len    = 0;
 436         dev->addr_len           = 0;
 437         dev->tx_queue_len       = 10;
 438
 439         dev->mtu                = sizeof(struct can_frame);
 440         dev->type               = ARPHRD_CAN;
 441
 442         /* New-style flags. */
 443         dev->flags              = IFF_NOARP;
 444         dev->features           = NETIF_F_NO_CSUM;
 445 }
 446
 447 /******************************************
 448   Routines looking at TTY side.
 449  ******************************************/
 450 static void sllin_receive_buf(struct tty_struct *tty,
 451                               const unsigned char *cp, char *fp, int count)
 452 {
 453         struct sllin *sl = (struct sllin *) tty->disc_data;
 454         pr_debug("sllin: sllin_receive_buf invoked, count = %u\n", count);
 455
 456         if (!sl || sl->magic != SLLIN_MAGIC || !netif_running(sl->dev))
 457                 return;
 458
 459         /* Read the characters out of the buffer */
 460         while (count--) {
 461                 if (fp && *fp++) {
 462                         pr_debug("sllin: sllin_receive_buf char 0x%02x ignored "
 463                                 "due marker 0x%02x, flags 0x%lx\n",
 464                                 *cp, *(fp-1), sl->flags);
 465
 466                         if (sl->lin_master == true) {
 467                                 if (sl->rx_cnt > SLLIN_BUFF_BREAK) {
 468                                         /* Report error */
 469                                         set_bit(SLF_ERROR, &sl->flags);
 470                                         wake_up(&sl->kwt_wq);
 471                                         return;
 472                                 }
 473                         } else { /* Received Break */
 474                                 sl->rx_cnt = 0;
 475                                 sl->rx_expect = SLLIN_BUFF_ID + 1;
 476                                 sl->rx_len_unknown = false; /* We do know exact length of the header */
 477                                 sl->header_received = false;
 478                         }
 479                 }
 480
 481 #ifndef BREAK_BY_BAUD
 482                 /* We didn't receive Break character -- fake it! */
 483                 if ((sl->rx_cnt == SLLIN_BUFF_BREAK) && (*cp == 0x55)) {
 484                         pr_debug("sllin: LIN_RX[%d]: 0x00\n", sl->rx_cnt);
 485                         sl->rx_buff[sl->rx_cnt++] = 0x00;
 486                 }
 487 #endif
 488                 if (sl->rx_cnt < SLLIN_BUFF_LEN) {
 489                         pr_debug("sllin: LIN_RX[%d]: 0x%02x\n", sl->rx_cnt, *cp);
 490                         sl->rx_buff[sl->rx_cnt++] = *cp++;
 491                 }
 492
 493                 if (sl->lin_master == true) {
 494                         if ((sl->rx_cnt >= sl->rx_expect) && (count == 0)) {
 495                                 set_bit(SLF_RXEVENT, &sl->flags);
 496                                 wake_up(&sl->kwt_wq);
 497                                 pr_debug("sllin: sllin_receive_buf count %d, wakeup\n", sl->rx_cnt);
 498                         } else {
 499                                 pr_debug("sllin: sllin_receive_buf count %d, waiting\n", sl->rx_cnt);
 500                         }
 501                 } else { /* LIN slave */
 502                         int lin_id;
 503                         struct sllin_conf_entry *sce;
 504
 505                         pr_debug("sllin: rx_cnt = %u; header_received = %u\n",
 506                                         sl->rx_cnt, sl->header_received);
 507
 508                         /* Header received */
 509                         if ((sl->header_received == false) && (sl->rx_cnt >= (SLLIN_BUFF_ID + 1))) {
 510                                 unsigned long flags;
 511
 512                                 lin_id = sl->rx_buff[SLLIN_BUFF_ID] & LIN_ID_MASK;
 513                                 sce = &sl->linfr_cache[lin_id];
 514
 515                                 spin_lock_irqsave(&sl->linfr_lock, flags);
 516                                 /* Is the length of data set in frame cache? */
 517                                 if (sce->frame_fl & LIN_LOC_SLAVE_CACHE) {
 518                                         sl->rx_expect += sce->dlc;
 519                                         sl->rx_len_unknown = false;
 520                                 } else {
 521                                         sl->rx_expect += SLLIN_DATA_MAX + 1; /* + checksum */
 522                                         sl->rx_len_unknown = true;
 523                                 }
 524                                 spin_unlock_irqrestore(&sl->linfr_lock, flags);
 525
 526                                 sl->header_received = true;
 527                                 sll_send_rtr(sl);
 528                                 continue;
 529                         }
 530
 531                         /* Response received */
 532                         if ((sl->header_received == true) &&
 533                                 ((sl->rx_cnt >= sl->rx_expect) ||
 534                                 ((sl->rx_len_unknown == true) && (count == 0)))) {
 535
 536                                 sll_bump(sl);
 537                                 pr_debug("sllin: Received LIN header & LIN response. "
 538                                                 "rx_cnt = %u, rx_expect = %u\n", sl->rx_cnt,
 539                                                 sl->rx_expect);
 540
 541                                 /* Prepare for reception of new header */
 542                                 sl->rx_cnt = 0;
 543                                 sl->rx_expect = SLLIN_BUFF_ID + 1;
 544                                 sl->rx_len_unknown = false; /* We do know exact length of the header */
 545                                 sl->header_received = false;
 546                         }
 547                 }
 548         }
 549 }
 550
 551 /*****************************************
 552  *  sllin message helper routines
 553  *****************************************/
 554 /**
 555  * sllin_report_error() -- Report an error by sending CAN frame
 556  *      with particular error flag set in can_id
 557  *
 558  * @sl:
 559  * @err: Error flag to be sent.
 560  */
 561 void sllin_report_error(struct sllin *sl, int err)
 562 {
 563         switch (err) {
 564         case LIN_ERR_CHECKSUM:
 565                 sl->dev->stats.rx_crc_errors++;
 566                 break;
 567
 568         case LIN_ERR_RX_TIMEOUT:
 569                 sl->dev->stats.rx_errors++;
 570                 break;
 571
 572         case LIN_ERR_FRAMING:
 573                 sl->dev->stats.rx_frame_errors++;
 574                 break;
 575         }
 576
 577         sllin_send_canfr(sl, 0 | CAN_EFF_FLAG |
 578                 (err & ~LIN_ID_MASK), NULL, 0);
 579 }
 580
 581 /**
 582  * sllin_configure_frame_cache() -- Configure particular entry in linfr_cache
 583  *
 584  * @sl:
 585  * @cf: Pointer to CAN frame sent to this driver
 586  *      holding configuration information
 587  */
 588 static int sllin_configure_frame_cache(struct sllin *sl, struct can_frame *cf)
 589 {
 590         unsigned long flags;
 591         struct sllin_conf_entry *sce;
 592
 593         if (!(cf->can_id & LIN_ID_CONF))
 594                 return -1;
 595
 596         sce = &sl->linfr_cache[cf->can_id & LIN_ID_MASK];
 597         pr_debug("sllin: Setting frame cache with EFF CAN frame. "
 598                 "LIN ID = %d\n", cf->can_id & LIN_ID_MASK);
 599
 600         spin_lock_irqsave(&sl->linfr_lock, flags);
 601
 602         sce->dlc = cf->can_dlc;
 603         if (sce->dlc > SLLIN_DATA_MAX)
 604                 sce->dlc = SLLIN_DATA_MAX;
 605
 606         sce->frame_fl = (cf->can_id & ~LIN_ID_MASK) & CAN_EFF_MASK;
 607         memcpy(sce->data, cf->data, cf->can_dlc);
 608
 609         spin_unlock_irqrestore(&sl->linfr_lock, flags);
 610
 611         return 0;
 612 }
 613
 614 /**
 615  * sllin_checksum() -- Count checksum for particular data
 616  *
 617  * @data:        Pointer to the buffer containing whole LIN
 618  *               frame (i.e. including break and sync bytes).
 619  * @length:      Length of the buffer.
 620  * @enhanced_fl: Flag determining whether Enhanced or Classic
 621  *               checksum should be counted.
 622  */
 623 static inline unsigned sllin_checksum(unsigned char *data, int length, int enhanced_fl)
 624 {
 625         unsigned csum = 0;
 626         int i;
 627
 628         if (enhanced_fl)
 629                 i = SLLIN_BUFF_ID;
 630         else
 631                 i = SLLIN_BUFF_DATA;
 632
 633         for (; i < length; i++) {
 634                 csum += data[i];
 635                 if (csum > 255)
 636                         csum -= 255;
 637         }
 638
 639         return ~csum & 0xff;
 640 }
 641
 642 #define SLLIN_STPMSG_RESPONLY           (1) /* Message will be LIN Response only */
 643 #define SLLIN_STPMSG_CHCKSUM_CLS        (1 << 1)
 644 #define SLLIN_STPMSG_CHCKSUM_ENH        (1 << 2)
 645
 646 int sllin_setup_msg(struct sllin *sl, int mode, int id,
 647                 unsigned char *data, int len)
 648 {
 649         if (id > LIN_ID_MASK)
 650                 return -1;
 651
 652         if (!(mode & SLLIN_STPMSG_RESPONLY)) {
 653                 sl->rx_cnt = 0;
 654                 sl->tx_cnt = 0;
 655                 sl->rx_expect = 0;
 656                 sl->rx_lim = SLLIN_BUFF_LEN;
 657         }
 658
 659         sl->tx_buff[SLLIN_BUFF_BREAK] = 0;
 660         sl->tx_buff[SLLIN_BUFF_SYNC]  = 0x55;
 661         sl->tx_buff[SLLIN_BUFF_ID]    = id | sllin_id_parity_table[id];
 662         sl->tx_lim = SLLIN_BUFF_DATA;
 663
 664         if ((data != NULL) && len) {
 665                 sl->tx_lim += len;
 666                 memcpy(sl->tx_buff + SLLIN_BUFF_DATA, data, len);
 667                 sl->tx_buff[sl->tx_lim++] = sllin_checksum(sl->tx_buff,
 668                                 sl->tx_lim, mode & SLLIN_STPMSG_CHCKSUM_ENH);
 669         }
 670         if (len != 0)
 671                 sl->rx_lim = SLLIN_BUFF_DATA + len + 1;
 672
 673         return 0;
 674 }
 675
 676 static void sllin_reset_buffs(struct sllin *sl)
 677 {
 678         sl->rx_cnt = 0;
 679         sl->rx_expect = 0;
 680         sl->rx_lim = sl->lin_master ? 0 : SLLIN_BUFF_LEN;
 681         sl->tx_cnt = 0;
 682         sl->tx_lim = 0;
 683         sl->id_to_send = false;
 684         sl->data_to_send = false;
 685 }
 686
 687 int sllin_send_tx_buff(struct sllin *sl)
 688 {
 689         struct tty_struct *tty = sl->tty;
 690         int remains;
 691         int res;
 692
 693         set_bit(SLF_TXBUFF_RQ, &sl->flags);
 694         do {
 695                 if (unlikely(test_and_set_bit(SLF_TXBUFF_INPR, &sl->flags)))
 696                         return 0;       /* ongoing concurrent processing */
 697
 698                 clear_bit(SLF_TXBUFF_RQ, &sl->flags);
 699                 smp_mb__after_clear_bit();
 700
 701 #ifdef BREAK_BY_BAUD
 702                 if (sl->lin_state != SLSTATE_BREAK_SENT)
 703                         remains = sl->tx_lim - sl->tx_cnt;
 704                 else
 705                         remains = 1;
 706 #else
 707                 remains = sl->tx_lim - sl->tx_cnt;
 708 #endif
 709
 710                 res = tty->ops->write(tty, sl->tx_buff + sl->tx_cnt, remains);
 711                 if (res < 0)
 712                         goto error_in_write;
 713
 714                 remains -= res;
 715                 sl->tx_cnt += res;
 716
 717                 if (remains > 0) {
 718                         set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
 719                         res = tty->ops->write(tty, sl->tx_buff + sl->tx_cnt, remains);
 720                         if (res < 0) {
 721                                 clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
 722                                 goto error_in_write;
 723                         }
 724
 725                         remains -= res;
 726                         sl->tx_cnt += res;
 727                 }
 728
 729                 pr_debug("sllin: sllin_send_tx_buff sent %d, remains %d\n",
 730                                 sl->tx_cnt, remains);
 731
 732                 clear_bit(SLF_TXBUFF_INPR, &sl->flags);
 733                 smp_mb__after_clear_bit();
 734
 735         } while (unlikely(test_bit(SLF_TXBUFF_RQ, &sl->flags)));
 736
 737         return 0;
 738
 739 error_in_write:
 740         clear_bit(SLF_TXBUFF_INPR, &sl->flags);
 741         return -1;
 742
 743 }
 744
 745 #ifdef BREAK_BY_BAUD
 746 int sllin_send_break(struct sllin *sl)
 747 {
 748         struct tty_struct *tty = sl->tty;
 749         unsigned long break_baud;
 750         int res;
 751
 752         break_baud = ((sl->lin_baud * 2) / 3);
 753         sltty_change_speed(tty, break_baud);
 754
 755         tty->ops->flush_buffer(tty);
 756         sl->rx_cnt = SLLIN_BUFF_BREAK;
 757
 758         sl->rx_expect = SLLIN_BUFF_BREAK + 1;
 759         sl->lin_state = SLSTATE_BREAK_SENT;
 760
 761         res = sllin_send_tx_buff(sl);
 762         if (res < 0) {
 763                 sl->lin_state = SLSTATE_IDLE;
 764                 return res;
 765         }
 766
 767         return 0;
 768 }
 769 #else /* BREAK_BY_BAUD */
 770
 771 int sllin_send_break(struct sllin *sl)
 772 {
 773         struct tty_struct *tty = sl->tty;
 774         int retval;
 775         unsigned long break_baud;
 776         unsigned long usleep_range_min;
 777         unsigned long usleep_range_max;
 778
 779         break_baud = ((sl->lin_baud * 2) / 3);
 780         sl->rx_cnt = SLLIN_BUFF_BREAK;
 781         sl->rx_expect = SLLIN_BUFF_BREAK + 1;
 782         sl->lin_state = SLSTATE_BREAK_SENT;
 783
 784         /* Do the break ourselves; Inspired by
 785            http://lxr.linux.no/#linux+v3.1.2/drivers/tty/tty_io.c#L2452 */
 786         retval = tty->ops->break_ctl(tty, -1);
 787         if (retval)
 788                 return retval;
 789
 790         /* udelay(712); */
 791         usleep_range_min = (1000000l * SLLIN_SAMPLES_PER_CHAR) / break_baud;
 792         usleep_range_max = usleep_range_min + 50;
 793         usleep_range(usleep_range_min, usleep_range_max);
 794
 795         retval = tty->ops->break_ctl(tty, 0);
 796         usleep_range_min = (1000000l * 1 /* 1 bit */) / break_baud;
 797         usleep_range_max = usleep_range_min + 30;
 798         usleep_range(usleep_range_min, usleep_range_max);
 799
 800         tty->ops->flush_buffer(tty);
 801
 802         sl->tx_cnt = SLLIN_BUFF_SYNC;
 803
 804         pr_debug("sllin: Break sent.\n");
 805         set_bit(SLF_RXEVENT, &sl->flags);
 806         wake_up(&sl->kwt_wq);
 807
 808         return 0;
 809 }
 810 #endif /* BREAK_BY_BAUD */
 811
 812
 813 static enum hrtimer_restart sllin_rx_timeout_handler(struct hrtimer *hrtimer)
 814 {
 815         struct sllin *sl = container_of(hrtimer, struct sllin, rx_timer);
 816
 817         sllin_report_error(sl, LIN_ERR_RX_TIMEOUT);
 818         set_bit(SLF_TMOUTEVENT, &sl->flags);
 819         wake_up(&sl->kwt_wq);
 820
 821         return HRTIMER_NORESTART;
 822 }
 823
 824 /**
 825  * sllin_rx_validate() -- Validate received frame, i,e. check checksum
 826  *
 827  * @sl:
 828  */
 829 static int sllin_rx_validate(struct sllin *sl)
 830 {
 831         unsigned long flags;
 832         int actual_id;
 833         int ext_chcks_fl;
 834         int lin_dlc;
 835         unsigned char rec_chcksm = sl->rx_buff[sl->rx_cnt - 1];
 836         struct sllin_conf_entry *sce;
 837
 838         actual_id = sl->rx_buff[SLLIN_BUFF_ID] & LIN_ID_MASK;
 839         sce = &sl->linfr_cache[actual_id];
 840
 841         spin_lock_irqsave(&sl->linfr_lock, flags);
 842         lin_dlc = sce->dlc;
 843         ext_chcks_fl = sce->frame_fl & LIN_CHECKSUM_EXTENDED;
 844         spin_unlock_irqrestore(&sl->linfr_lock, flags);
 845
 846         if (sllin_checksum(sl->rx_buff, sl->rx_cnt - 1, ext_chcks_fl) !=
 847                 rec_chcksm) {
 848
 849                 /* Type of checksum is configured for particular frame */
 850                 if (lin_dlc > 0) {
 851                         return -1;
 852                 } else {
 853                         if (sllin_checksum(sl->rx_buff, sl->rx_cnt - 1,
 854                                 !ext_chcks_fl) != rec_chcksm) {
 855                                 return -1;
 856                         }
 857                 }
 858         }
 859
 860         return 0;
 861 }
 862
 863 /*****************************************
 864  *  sllin_kwthread - kernel worker thread
 865  *****************************************/
 866
 867 int sllin_kwthread(void *ptr)
 868 {
 869         struct sllin *sl = (struct sllin *)ptr;
 870         struct tty_struct *tty = sl->tty;
 871         struct sched_param schparam = { .sched_priority = 40 };
 872         int tx_bytes = 0; /* Used for Network statistics */
 873
 874
 875         pr_debug("sllin: sllin_kwthread started.\n");
 876         sched_setscheduler(current, SCHED_FIFO, &schparam);
 877
 878         clear_bit(SLF_ERROR, &sl->flags);
 879         sltty_change_speed(tty, sl->lin_baud);
 880
 881         while (!kthread_should_stop()) {
 882                 struct can_frame *cf;
 883                 u8 *lin_data;
 884                 int lin_dlc;
 885                 u8 lin_data_buff[SLLIN_DATA_MAX];
 886
 887
 888                 if ((sl->lin_state == SLSTATE_IDLE) && sl->lin_master &&
 889                         sl->id_to_send) {
 890                         if (sllin_send_break(sl) < 0) {
 891                                 /* error processing */
 892                         }
 893                 }
 894
 895                 wait_event_killable(sl->kwt_wq, kthread_should_stop() ||
 896                         test_bit(SLF_RXEVENT, &sl->flags) ||
 897                         test_bit(SLF_TXEVENT, &sl->flags) ||
 898                         test_bit(SLF_TMOUTEVENT, &sl->flags) ||
 899                         test_bit(SLF_ERROR, &sl->flags) ||
 900                         (((sl->lin_state == SLSTATE_IDLE) ||
 901                                 (sl->lin_state == SLSTATE_RESPONSE_WAIT))
 902                                 && test_bit(SLF_MSGEVENT, &sl->flags)));
 903
 904                 if (test_and_clear_bit(SLF_RXEVENT, &sl->flags)) {
 905                         pr_debug("sllin: sllin_kthread RXEVENT\n");
 906                 }
 907
 908                 if (test_and_clear_bit(SLF_ERROR, &sl->flags)) {
 909                         unsigned long usleep_range_min;
 910                         unsigned long usleep_range_max;
 911                         hrtimer_cancel(&sl->rx_timer);
 912                         pr_debug("sllin: sllin_kthread ERROR\n");
 913
 914                         if (sl->lin_state != SLSTATE_IDLE)
 915                                 sllin_report_error(sl, LIN_ERR_FRAMING);
 916
 917                         usleep_range_min = (1000000l * SLLIN_SAMPLES_PER_CHAR * 10) /
 918                                                 sl->lin_baud;
 919                         usleep_range_max = usleep_range_min + 50;
 920                         usleep_range(usleep_range_min, usleep_range_max);
 921                         sllin_reset_buffs(sl);
 922                         sl->lin_state = SLSTATE_IDLE;
 923                 }
 924
 925                 if (test_and_clear_bit(SLF_TXEVENT, &sl->flags)) {
 926                         pr_debug("sllin: sllin_kthread TXEVENT\n");
 927                 }
 928
 929                 if (test_and_clear_bit(SLF_TMOUTEVENT, &sl->flags)) {
 930                         pr_debug("sllin: sllin_kthread TMOUTEVENT\n");
 931                         sllin_reset_buffs(sl);
 932
 933                         sl->lin_state = SLSTATE_IDLE;
 934                 }
 935
 936                 switch (sl->lin_state) {
 937                 case SLSTATE_IDLE:
 938                         if (!test_bit(SLF_MSGEVENT, &sl->flags))
 939                                 break;
 940
 941                         cf = (struct can_frame *)sl->tx_req_skb->data;
 942
 943                         /* SFF RTR CAN frame -> LIN header */
 944                         if (cf->can_id & CAN_RTR_FLAG) {
 945                                 unsigned long flags;
 946                                 struct sllin_conf_entry *sce;
 947
 948                                 pr_debug("sllin: %s: RTR SFF CAN frame, ID = %x\n",
 949                                         __func__, cf->can_id & LIN_ID_MASK);
 950
 951                                 sce = &sl->linfr_cache[cf->can_id & LIN_ID_MASK];
 952                                 spin_lock_irqsave(&sl->linfr_lock, flags);
 953
 954                                 /* Is there Slave response in linfr_cache to be sent? */
 955                                 if ((sce->frame_fl & LIN_LOC_SLAVE_CACHE)
 956                                         && (sce->dlc > 0)) {
 957
 958                                         pr_debug("sllin: Sending LIN response from linfr_cache\n");
 959
 960                                         lin_data = sce->data;
 961                                         lin_dlc = sce->dlc;
 962                                         if (lin_dlc > SLLIN_DATA_MAX)
 963                                                 lin_dlc = SLLIN_DATA_MAX;
 964                                         memcpy(lin_data_buff, lin_data, lin_dlc);
 965                                         lin_data = lin_data_buff;
 966                                 } else {
 967                                         lin_data = NULL;
 968                                         lin_dlc = sce->dlc;
 969                                 }
 970                                 spin_unlock_irqrestore(&sl->linfr_lock, flags);
 971
 972                         } else { /* SFF NON-RTR CAN frame -> LIN header + LIN response */
 973                                 pr_debug("sllin: %s: NON-RTR SFF CAN frame, ID = %x\n",
 974                                         __func__, (int)cf->can_id & LIN_ID_MASK);
 975
 976                                 lin_data = cf->data;
 977                                 lin_dlc = cf->can_dlc;
 978                                 if (lin_dlc > SLLIN_DATA_MAX)
 979                                         lin_dlc = SLLIN_DATA_MAX;
 980                                 tx_bytes = lin_dlc;
 981                         }
 982
 983                         if (sllin_setup_msg(sl, 0, cf->can_id & LIN_ID_MASK,
 984                                 lin_data, lin_dlc) != -1) {
 985
 986                                 sl->id_to_send = true;
 987                                 sl->data_to_send = (lin_data != NULL) ? true : false;
 988                                 sl->resp_len_known = (lin_dlc > 0) ? true : false;
 989                                 sl->dev->stats.tx_packets++;
 990                                 sl->dev->stats.tx_bytes += tx_bytes;
 991                         }
 992
 993                         clear_bit(SLF_MSGEVENT, &sl->flags);
 994                         kfree_skb(sl->tx_req_skb);
 995                         netif_wake_queue(sl->dev);
 996                         hrtimer_start(&sl->rx_timer,
 997                                 ktime_add(ktime_get(), sl->rx_timer_timeout),
 998                                 HRTIMER_MODE_ABS);
 999                         break;
1000
1001                 case SLSTATE_BREAK_SENT:
1002 #ifdef BREAK_BY_BAUD
1003                         if (sl->rx_cnt <= SLLIN_BUFF_BREAK)
1004                                 continue;
1005
1006                         res = sltty_change_speed(tty, sl->lin_baud);
1007 #endif
1008
1009                         sl->lin_state = SLSTATE_ID_SENT;
1010                         sllin_send_tx_buff(sl);
1011                         break;
1012
1013                 case SLSTATE_ID_SENT:
1014                         hrtimer_cancel(&sl->rx_timer);
1015                         sl->id_to_send = false;
1016                         if (sl->data_to_send) {
1017                                 sllin_send_tx_buff(sl);
1018                                 sl->lin_state = SLSTATE_RESPONSE_SENT;
1019                                 sl->rx_expect = sl->tx_lim;
1020                                 goto slstate_response_sent;
1021                         } else {
1022                                 if (sl->resp_len_known) {
1023                                         sl->rx_expect = sl->rx_lim;
1024                                 } else {
1025                                         sl->rx_expect = SLLIN_BUFF_DATA + 2;
1026                                 }
1027                                 sl->lin_state = SLSTATE_RESPONSE_WAIT;
1028                                 /* If we don't receive anything, timer will "unblock" us */
1029                                 hrtimer_start(&sl->rx_timer,
1030                                         ktime_add(ktime_get(), sl->rx_timer_timeout),
1031                                         HRTIMER_MODE_ABS);
1032                                 goto slstate_response_wait;
1033                         }
1034                         break;
1035
1036                 case SLSTATE_RESPONSE_WAIT:
1037 slstate_response_wait:
1038                         if (test_bit(SLF_MSGEVENT, &sl->flags)) {
1039                                 unsigned char *lin_buff;
1040                                 cf = (struct can_frame *)sl->tx_req_skb->data;
1041
1042                                 lin_buff = (sl->lin_master) ? sl->tx_buff : sl->rx_buff;
1043                                 if (cf->can_id == (lin_buff[SLLIN_BUFF_ID] & LIN_ID_MASK)) {
1044                                         hrtimer_cancel(&sl->rx_timer);
1045                                         pr_debug("sllin: received LIN response in a CAN frame.\n");
1046                                         if (sllin_setup_msg(sl, SLLIN_STPMSG_RESPONLY,
1047                                                 cf->can_id & LIN_ID_MASK,
1048                                                 cf->data, cf->can_dlc) != -1) {
1049
1050                                                 sl->rx_expect = sl->tx_lim;
1051                                                 sl->data_to_send = true;
1052                                                 sl->dev->stats.tx_packets++;
1053                                                 sl->dev->stats.tx_bytes += tx_bytes;
1054
1055                                                 if (!sl->lin_master) {
1056                                                         sl->tx_cnt = SLLIN_BUFF_DATA;
1057                                                 }
1058
1059                                                 sllin_send_tx_buff(sl);
1060                                                 clear_bit(SLF_MSGEVENT, &sl->flags);
1061                                                 kfree_skb(sl->tx_req_skb);
1062                                                 netif_wake_queue(sl->dev);
1063
1064                                                 sl->lin_state = SLSTATE_RESPONSE_SENT;
1065                                                 goto slstate_response_sent;
1066                                         }
1067                                 } else {
1068                                         sl->lin_state = SLSTATE_RESPONSE_WAIT_BUS;
1069                                 }
1070                         }
1071
1072                         /* Be aware, no BREAK here */
1073                 case SLSTATE_RESPONSE_WAIT_BUS:
1074                         if (sl->rx_cnt < sl->rx_expect)
1075                                 continue;
1076
1077                         hrtimer_cancel(&sl->rx_timer);
1078                         pr_debug("sllin: response received ID %d len %d\n",
1079                                 sl->rx_buff[SLLIN_BUFF_ID], sl->rx_cnt - SLLIN_BUFF_DATA - 1);
1080
1081                         if (sllin_rx_validate(sl) == -1) {
1082                                 pr_debug("sllin: RX validation failed.\n");
1083                                 sllin_report_error(sl, LIN_ERR_CHECKSUM);
1084                         } else {
1085                                 /* Send CAN non-RTR frame with data */
1086                                 pr_debug("sllin: sending NON-RTR CAN"
1087                                         "frame with LIN payload.");
1088                                 sll_bump(sl); /* send packet to the network layer */
1089                         }
1090
1091                         sl->id_to_send = false;
1092                         sl->lin_state = SLSTATE_IDLE;
1093                         break;
1094
1095                 case SLSTATE_RESPONSE_SENT:
1096 slstate_response_sent:
1097                         if (sl->rx_cnt < sl->tx_lim)
1098                                 continue;
1099
1100                         hrtimer_cancel(&sl->rx_timer);
1101                         sll_bump(sl); /* send packet to the network layer */
1102                         pr_debug("sllin: response sent ID %d len %d\n",
1103                                 sl->rx_buff[SLLIN_BUFF_ID], sl->rx_cnt - SLLIN_BUFF_DATA - 1);
1104
1105                         sl->id_to_send = false;
1106                         sl->lin_state = SLSTATE_IDLE;
1107                         break;
1108                 }
1109         }
1110
1111         hrtimer_cancel(&sl->rx_timer);
1112         pr_debug("sllin: sllin_kwthread stopped.\n");
1113
1114         return 0;
1115 }
1116
1117
1118 /************************************
1119  *  sllin_open helper routines.
1120  ************************************/
1121
1122 /* Collect hanged up channels */
1123 static void sll_sync(void)
1124 {
1125         int i;
1126         struct net_device *dev;
1127         struct sllin      *sl;
1128
1129         for (i = 0; i < maxdev; i++) {
1130                 dev = sllin_devs[i];
1131                 if (dev == NULL)
1132                         break;
1133
1134                 sl = netdev_priv(dev);
1135                 if (sl->tty)
1136                         continue;
1137                 if (dev->flags & IFF_UP)
1138                         dev_close(dev);
1139         }
1140 }
1141
1142 /* Find a free SLLIN channel, and link in this `tty' line. */
1143 static struct sllin *sll_alloc(dev_t line)
1144 {
1145         int i;
1146         struct net_device *dev = NULL;
1147         struct sllin       *sl;
1148
1149         if (sllin_devs == NULL)
1150                 return NULL;    /* Master array missing ! */
1151
1152         for (i = 0; i < maxdev; i++) {
1153                 dev = sllin_devs[i];
1154                 if (dev == NULL)
1155                         break;
1156
1157         }
1158
1159         /* Sorry, too many, all slots in use */
1160         if (i >= maxdev)
1161                 return NULL;
1162
1163         if (dev) {
1164                 sl = netdev_priv(dev);
1165                 if (test_bit(SLF_INUSE, &sl->flags)) {
1166                         unregister_netdevice(dev);
1167                         dev = NULL;
1168                         sllin_devs[i] = NULL;
1169                 }
1170         }
1171
1172         if (!dev) {
1173                 char name[IFNAMSIZ];
1174                 sprintf(name, "sllin%d", i);
1175
1176                 dev = alloc_netdev(sizeof(*sl), name, sll_setup);
1177                 if (!dev)
1178                         return NULL;
1179                 dev->base_addr  = i;
1180         }
1181
1182         sl = netdev_priv(dev);
1183         /* Initialize channel control data */
1184         sl->magic = SLLIN_MAGIC;
1185         sl->dev = dev;
1186         spin_lock_init(&sl->lock);
1187         spin_lock_init(&sl->linfr_lock);
1188         sllin_devs[i] = dev;
1189
1190         return sl;
1191 }
1192
1193 /*
1194  * Open the high-level part of the SLLIN channel.
1195  * This function is called by the TTY module when the
1196  * SLLIN line discipline is called for.  Because we are
1197  * sure the tty line exists, we only have to link it to
1198  * a free SLLIN channel...
1199  *
1200  * Called in process context serialized from other ldisc calls.
1201  */
1202
1203 static int sllin_open(struct tty_struct *tty)
1204 {
1205         struct sllin *sl;
1206         int err;
1207         pr_debug("sllin: %s() invoked\n", __func__);
1208
1209         if (!capable(CAP_NET_ADMIN))
1210                 return -EPERM;
1211
1212         if (tty->ops->write == NULL)
1213                 return -EOPNOTSUPP;
1214
1215         /* RTnetlink lock is misused here to serialize concurrent
1216            opens of sllin channels. There are better ways, but it is
1217            the simplest one.
1218          */
1219         rtnl_lock();
1220
1221         /* Collect hanged up channels. */
1222         sll_sync();
1223
1224         sl = tty->disc_data;
1225
1226         err = -EEXIST;
1227         /* First make sure we're not already connected. */
1228         if (sl && sl->magic == SLLIN_MAGIC)
1229                 goto err_exit;
1230
1231         /* OK.  Find a free SLLIN channel to use. */
1232         err = -ENFILE;
1233         sl = sll_alloc(tty_devnum(tty));
1234         if (sl == NULL)
1235                 goto err_exit;
1236
1237         sl->tty = tty;
1238         tty->disc_data = sl;
1239         sl->line = tty_devnum(tty);
1240
1241         if (!test_bit(SLF_INUSE, &sl->flags)) {
1242                 /* Perform the low-level SLLIN initialization. */
1243                 sl->lin_master = master;
1244 #ifdef DEBUG
1245                 if (master)
1246                         pr_debug("sllin: Configured as MASTER\n");
1247                 else
1248                         pr_debug("sllin: Configured as SLAVE\n");
1249 #endif
1250
1251                 sllin_reset_buffs(sl);
1252
1253                 sl->lin_baud = (baudrate == 0) ? LIN_DEFAULT_BAUDRATE : baudrate;
1254                 pr_debug("sllin: Baudrate set to %u\n", sl->lin_baud);
1255
1256                 sl->lin_state = SLSTATE_IDLE;
1257
1258                 hrtimer_init(&sl->rx_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
1259                 sl->rx_timer.function = sllin_rx_timeout_handler;
1260                 /* timeval_to_ktime(msg_head->ival1); */
1261                 sl->rx_timer_timeout = ns_to_ktime(
1262                         (1000000000l / sl->lin_baud) *
1263                         SLLIN_SAMPLES_PER_CHAR * SLLIN_CHARS_TO_TIMEOUT);
1264
1265                 set_bit(SLF_INUSE, &sl->flags);
1266
1267                 init_waitqueue_head(&sl->kwt_wq);
1268                 sl->kwthread = kthread_run(sllin_kwthread, sl, "sllin");
1269                 if (sl->kwthread == NULL)
1270                         goto err_free_chan;
1271
1272                 err = register_netdevice(sl->dev);
1273                 if (err)
1274                         goto err_free_chan_and_thread;
1275         }
1276
1277         /* Done.  We have linked the TTY line to a channel. */
1278         rtnl_unlock();
1279         tty->receive_room = SLLIN_BUFF_LEN * 40; /* We don't flow control */
1280
1281         /* TTY layer expects 0 on success */
1282         return 0;
1283
1284 err_free_chan_and_thread:
1285         kthread_stop(sl->kwthread);
1286         sl->kwthread = NULL;
1287
1288 err_free_chan:
1289         sl->tty = NULL;
1290         tty->disc_data = NULL;
1291         clear_bit(SLF_INUSE, &sl->flags);
1292
1293 err_exit:
1294         rtnl_unlock();
1295
1296         /* Count references from TTY module */
1297         return err;
1298 }
1299
1300 /*
1301  * Close down a SLLIN channel.
1302  * This means flushing out any pending queues, and then returning. This
1303  * call is serialized against other ldisc functions.
1304  *
1305  * We also use this method for a hangup event.
1306  */
1307
1308 static void sllin_close(struct tty_struct *tty)
1309 {
1310         struct sllin *sl = (struct sllin *) tty->disc_data;
1311
1312         /* First make sure we're connected. */
1313         if (!sl || sl->magic != SLLIN_MAGIC || sl->tty != tty)
1314                 return;
1315
1316         kthread_stop(sl->kwthread);
1317         sl->kwthread = NULL;
1318
1319         tty->disc_data = NULL;
1320         sl->tty = NULL;
1321
1322         /* Flush network side */
1323         unregister_netdev(sl->dev);
1324         /* This will complete via sl_free_netdev */
1325 }
1326
1327 static int sllin_hangup(struct tty_struct *tty)
1328 {
1329         sllin_close(tty);
1330         return 0;
1331 }
1332
1333 /* Perform I/O control on an active SLLIN channel. */
1334 static int sllin_ioctl(struct tty_struct *tty, struct file *file,
1335                        unsigned int cmd, unsigned long arg)
1336 {
1337         struct sllin *sl = (struct sllin *) tty->disc_data;
1338         unsigned int tmp;
1339
1340         /* First make sure we're connected. */
1341         if (!sl || sl->magic != SLLIN_MAGIC)
1342                 return -EINVAL;
1343
1344         switch (cmd) {
1345         case SIOCGIFNAME:
1346                 tmp = strlen(sl->dev->name) + 1;
1347                 if (copy_to_user((void __user *)arg, sl->dev->name, tmp))
1348                         return -EFAULT;
1349                 return 0;
1350
1351         case SIOCSIFHWADDR:
1352                 return -EINVAL;
1353
1354         default:
1355                 return tty_mode_ioctl(tty, file, cmd, arg);
1356         }
1357 }
1358
1359 static struct tty_ldisc_ops sll_ldisc = {
1360         .owner          = THIS_MODULE,
1361         .magic          = TTY_LDISC_MAGIC,
1362         .name           = "sllin",
1363         .open           = sllin_open,
1364         .close          = sllin_close,
1365         .hangup         = sllin_hangup,
1366         .ioctl          = sllin_ioctl,
1367         .receive_buf    = sllin_receive_buf,
1368         .write_wakeup   = sllin_write_wakeup,
1369 };
1370
1371 static int __init sllin_init(void)
1372 {
1373         int status;
1374
1375         if (maxdev < 4)
1376                 maxdev = 4; /* Sanity */
1377
1378         printk(banner);
1379         pr_debug("sllin: %d dynamic interface channels.\n", maxdev);
1380
1381         sllin_devs = kzalloc(sizeof(struct net_device *)*maxdev, GFP_KERNEL);
1382         if (!sllin_devs) {
1383                 printk(KERN_ERR "sllin: can't allocate sllin device array!\n");
1384                 return -ENOMEM;
1385         }
1386
1387         /* Fill in our line protocol discipline, and register it */
1388         status = tty_register_ldisc(N_SLLIN, &sll_ldisc);
1389         if (status)  {
1390                 printk(KERN_ERR "sllin: can't register line discipline\n");
1391                 kfree(sllin_devs);
1392         }
1393
1394 #ifdef BREAK_BY_BAUD
1395         pr_debug("sllin: Break is generated by baud-rate change.");
1396 #else
1397         pr_debug("sllin: Break is generated manually with tiny sleep.");
1398 #endif
1399
1400         return status;
1401 }
1402
1403 static void __exit sllin_exit(void)
1404 {
1405         int i;
1406         struct net_device *dev;
1407         struct sllin *sl;
1408         unsigned long timeout = jiffies + HZ;
1409         int busy = 0;
1410
1411         if (sllin_devs == NULL)
1412                 return;
1413
1414         /* First of all: check for active disciplines and hangup them.
1415          */
1416         do {
1417                 if (busy)
1418                         msleep_interruptible(100);
1419
1420                 busy = 0;
1421                 for (i = 0; i < maxdev; i++) {
1422                         dev = sllin_devs[i];
1423                         if (!dev)
1424                                 continue;
1425                         sl = netdev_priv(dev);
1426                         spin_lock_bh(&sl->lock);
1427                         if (sl->tty) {
1428                                 busy++;
1429                                 tty_hangup(sl->tty);
1430                         }
1431                         spin_unlock_bh(&sl->lock);
1432                 }
1433         } while (busy && time_before(jiffies, timeout));
1434
1435         /* FIXME: hangup is async so we should wait when doing this second
1436            phase */
1437
1438         for (i = 0; i < maxdev; i++) {
1439                 dev = sllin_devs[i];
1440                 if (!dev)
1441                         continue;
1442                 sllin_devs[i] = NULL;
1443
1444                 sl = netdev_priv(dev);
1445                 if (sl->tty) {
1446                         printk(KERN_ERR "%s: tty discipline still running\n",
1447                                dev->name);
1448                         /* Intentionally leak the control block. */
1449                         dev->destructor = NULL;
1450                 }
1451
1452                 unregister_netdev(dev);
1453         }
1454
1455         kfree(sllin_devs);
1456         sllin_devs = NULL;
1457
1458         i = tty_unregister_ldisc(N_SLLIN);
1459         if (i)
1460                 printk(KERN_ERR "sllin: can't unregister ldisc (err %d)\n", i);
1461 }
1462
1463 module_init(sllin_init);
1464 module_exit(sllin_exit);