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