module/elmcan.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* ELM327 based CAN interface driver (tty line discipline)
   3  *
   4  * This driver started as a derivative of linux/drivers/net/can/slcan.c
   5  * and my thanks go to the original authors for their inspiration, even
   6  * after almost none of their code is left.
   7  */
   8
   9 #define pr_fmt(fmt) "[elmcan] " fmt
  10
  11 #include <linux/init.h>
  12 #include <linux/module.h>
  13 #include <linux/moduleparam.h>
  14
  15 #include <linux/atomic.h>
  16 #include <linux/bitops.h>
  17 #include <linux/ctype.h>
  18 #include <linux/delay.h>
  19 #include <linux/errno.h>
  20 #include <linux/if_ether.h>
  21 #include <linux/kernel.h>
  22 #include <linux/list.h>
  23 #include <linux/netdevice.h>
  24 #include <linux/skbuff.h>
  25 #include <linux/spinlock.h>
  26 #include <linux/string.h>
  27 #include <linux/tty.h>
  28 #include <linux/tty_ldisc.h>
  29 #include <linux/version.h>
  30 #include <linux/workqueue.h>
  31
  32 #include <uapi/linux/tty.h>
  33
  34 #include <linux/can.h>
  35 #include <linux/can/dev.h>
  36 #include <linux/can/error.h>
  37 #include <linux/can/led.h>
  38 #include <linux/can/rx-offload.h>
  39
  40 MODULE_ALIAS_LDISC(N_DEVELOPMENT);
  41 MODULE_DESCRIPTION("ELM327 based CAN interface");
  42 MODULE_LICENSE("GPL");
  43 MODULE_AUTHOR("Max Staudt <max-linux@enpas.org>");
  44
  45 /* Line discipline ID number.
  46  * N_DEVELOPMENT will likely be defined from Linux 5.18 onwards:
  47  * https://git.kernel.org/pub/scm/linux/kernel/git/gregkh/tty.git/commit/?h=tty-next&id=c2faf737abfb10f88f2d2612d573e9edc3c42c37
  48  */
  49 #ifndef N_DEVELOPMENT
  50 #define N_DEVELOPMENT 29
  51 #endif
  52
  53 /* Compatibility for Linux < 5.11 */
  54 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,11,0)
  55 #define len can_dlc
  56 #endif
  57
  58 #define ELM327_NAPI_WEIGHT 4
  59
  60 #define ELM327_SIZE_RXBUF 256
  61 #define ELM327_SIZE_TXBUF 32
  62
  63 #define ELM327_CAN_CONFIG_SEND_SFF           0x8000
  64 #define ELM327_CAN_CONFIG_VARIABLE_DLC       0x4000
  65 #define ELM327_CAN_CONFIG_RECV_BOTH_SFF_EFF  0x2000
  66 #define ELM327_CAN_CONFIG_BAUDRATE_MULT_8_7  0x1000
  67
  68 #define ELM327_DUMMY_CHAR 'y'
  69 #define ELM327_DUMMY_STRING "y"
  70 #define ELM327_READY_CHAR '>'
  71
  72 /* Bits in elm->cmds_todo */
  73 enum ELM327_TX_DO_BITS {
  74         ELM327_TX_DO_CAN_DATA = 0,
  75         ELM327_TX_DO_CANID_11BIT,
  76         ELM327_TX_DO_CANID_29BIT_LOW,
  77         ELM327_TX_DO_CANID_29BIT_HIGH,
  78         ELM327_TX_DO_CAN_CONFIG_PART2,
  79         ELM327_TX_DO_CAN_CONFIG,
  80         ELM327_TX_DO_RESPONSES,
  81         ELM327_TX_DO_SILENT_MONITOR,
  82         ELM327_TX_DO_INIT
  83 };
  84
  85 struct elmcan {
  86         /* This must be the first member when using alloc_candev() */
  87         struct can_priv can;
  88
  89         struct can_rx_offload offload;
  90
  91         /* TTY and netdev devices that we're bridging */
  92         struct tty_struct       *tty;
  93         struct net_device       *dev;
  94
  95         /* Per-channel lock */
  96         spinlock_t              lock;
  97
  98         /* Keep track of how many things are using this struct.
  99          * Once it reaches 0, we are in the process of cleaning up,
 100          * and new operations will be cancelled immediately.
 101          * Use atomic_t rather than refcount_t because we deliberately
 102          * decrement to 0, and refcount_dec() spills a WARN_ONCE in
 103          * that case.
 104          */
 105         atomic_t                refcount;
 106
 107         /* Stop the channel on hardware failure.
 108          * Once this is true, nothing will be sent to the TTY.
 109          */
 110         bool                    hw_failure;
 111
 112         /* TTY TX helpers */
 113         struct work_struct      tx_work;        /* Flushes TTY TX buffer   */
 114         unsigned char           *txbuf;
 115         unsigned char           *txhead;        /* Pointer to next TX byte */
 116         int                     txleft;         /* Bytes left to TX */
 117
 118         /* TTY RX helpers */
 119         unsigned char rxbuf[ELM327_SIZE_RXBUF];
 120         int rxfill;
 121
 122         /* State machine */
 123         enum {
 124                 ELM_NOTINIT = 0,
 125                 ELM_GETDUMMYCHAR,
 126                 ELM_GETPROMPT,
 127                 ELM_RECEIVING,
 128         } state;
 129
 130         int drop_next_line;
 131
 132         /* The CAN frame and config the ELM327 is sending/using,
 133          * or will send/use after finishing all cmds_todo
 134          */
 135         struct can_frame can_frame_to_send;
 136         unsigned short can_config;
 137         unsigned long can_bitrate;
 138         unsigned char can_bitrate_divisor;
 139         int silent_monitoring;
 140
 141         /* Things we have yet to send */
 142         char **next_init_cmd;
 143         unsigned long cmds_todo;
 144 };
 145
 146 /* A lock for all tty->disc_data handled by this ldisc.
 147  * This is to prevent a case where tty->disc_data is set to NULL,
 148  * yet someone is still trying to dereference it.
 149  * Without this, we cannot do a clean shutdown.
 150  */
 151 static DEFINE_SPINLOCK(elmcan_discdata_lock);
 152
 153 static inline void elm327_hw_failure(struct elmcan *elm);
 154
 155 /* Assumes elm->lock taken. */
 156 static void elm327_send(struct elmcan *elm, const void *buf, size_t len)
 157 {
 158         int actual;
 159
 160         if (elm->hw_failure)
 161                 return;
 162
 163         memcpy(elm->txbuf, buf, len);
 164
 165         /* Order of next two lines is *very* important.
 166          * When we are sending a little amount of data,
 167          * the transfer may be completed inside the ops->write()
 168          * routine, because it's running with interrupts enabled.
 169          * In this case we *never* got WRITE_WAKEUP event,
 170          * if we did not request it before write operation.
 171          *       14 Oct 1994  Dmitry Gorodchanin.
 172          */
 173         set_bit(TTY_DO_WRITE_WAKEUP, &elm->tty->flags);
 174         actual = elm->tty->ops->write(elm->tty, elm->txbuf, len);
 175         if (actual < 0) {
 176                 netdev_err(elm->dev,
 177                            "Failed to write to tty %s.\n",
 178                            elm->tty->name);
 179                 elm327_hw_failure(elm);
 180                 return;
 181         }
 182
 183         elm->txleft = len - actual;
 184         elm->txhead = elm->txbuf + actual;
 185 }
 186
 187 /* Take the ELM327 out of almost any state and back into command mode.
 188  * We send ELM327_DUMMY_CHAR which will either abort any running
 189  * operation, or be echoed back to us in case we're already in command
 190  * mode.
 191  *
 192  * Assumes elm->lock taken.
 193  */
 194 static void elm327_kick_into_cmd_mode(struct elmcan *elm)
 195 {
 196         if (elm->state != ELM_GETDUMMYCHAR && elm->state != ELM_GETPROMPT) {
 197                 elm327_send(elm, ELM327_DUMMY_STRING, 1);
 198
 199                 elm->state = ELM_GETDUMMYCHAR;
 200         }
 201 }
 202
 203 /* Schedule a CAN frame and necessary config changes to be sent to the TTY.
 204  *
 205  * Assumes elm->lock taken.
 206  */
 207 static void elm327_send_frame(struct elmcan *elm, struct can_frame *frame)
 208 {
 209         /* Schedule any necessary changes in ELM327's CAN configuration */
 210         if (elm->can_frame_to_send.can_id != frame->can_id) {
 211                 /* Set the new CAN ID for transmission. */
 212                 if ((frame->can_id & CAN_EFF_FLAG)
 213                     ^ (elm->can_frame_to_send.can_id & CAN_EFF_FLAG)) {
 214                         elm->can_config = (frame->can_id & CAN_EFF_FLAG
 215                                                 ? 0
 216                                                 : ELM327_CAN_CONFIG_SEND_SFF)
 217                                         | ELM327_CAN_CONFIG_VARIABLE_DLC
 218                                         | ELM327_CAN_CONFIG_RECV_BOTH_SFF_EFF
 219                                         | elm->can_bitrate_divisor;
 220
 221                         set_bit(ELM327_TX_DO_CAN_CONFIG, &elm->cmds_todo);
 222                 }
 223
 224                 if (frame->can_id & CAN_EFF_FLAG) {
 225                         clear_bit(ELM327_TX_DO_CANID_11BIT, &elm->cmds_todo);
 226                         set_bit(ELM327_TX_DO_CANID_29BIT_LOW, &elm->cmds_todo);
 227                         set_bit(ELM327_TX_DO_CANID_29BIT_HIGH, &elm->cmds_todo);
 228                 } else {
 229                         set_bit(ELM327_TX_DO_CANID_11BIT, &elm->cmds_todo);
 230                         clear_bit(ELM327_TX_DO_CANID_29BIT_LOW, &elm->cmds_todo);
 231                         clear_bit(ELM327_TX_DO_CANID_29BIT_HIGH, &elm->cmds_todo);
 232                 }
 233         }
 234
 235         /* Schedule the CAN frame itself. */
 236         elm->can_frame_to_send = *frame;
 237         set_bit(ELM327_TX_DO_CAN_DATA, &elm->cmds_todo);
 238
 239         elm327_kick_into_cmd_mode(elm);
 240 }
 241
 242 /* ELM327 initialization sequence.
 243  *
 244  * Assumes elm->lock taken.
 245  */
 246 static char *elm327_init_script[] = {
 247         "AT WS\r",        /* v1.0: Warm Start */
 248         "AT PP FF OFF\r", /* v1.0: All Programmable Parameters Off */
 249         "AT M0\r",        /* v1.0: Memory Off */
 250         "AT AL\r",        /* v1.0: Allow Long messages */
 251         "AT BI\r",        /* v1.0: Bypass Initialization */
 252         "AT CAF0\r",      /* v1.0: CAN Auto Formatting Off */
 253         "AT CFC0\r",      /* v1.0: CAN Flow Control Off */
 254         "AT CF 000\r",    /* v1.0: Reset CAN ID Filter */
 255         "AT CM 000\r",    /* v1.0: Reset CAN ID Mask */
 256         "AT E1\r",        /* v1.0: Echo On */
 257         "AT H1\r",        /* v1.0: Headers On */
 258         "AT L0\r",        /* v1.0: Linefeeds Off */
 259         "AT SH 7DF\r",    /* v1.0: Set CAN sending ID to 0x7df */
 260         "AT ST FF\r",     /* v1.0: Set maximum Timeout for response after TX */
 261         "AT AT0\r",       /* v1.2: Adaptive Timing Off */
 262         "AT D1\r",        /* v1.3: Print DLC On */
 263         "AT S1\r",        /* v1.3: Spaces On */
 264         "AT TP B\r",      /* v1.0: Try Protocol B */
 265         NULL
 266 };
 267
 268 static void elm327_init(struct elmcan *elm)
 269 {
 270         elm->state = ELM_NOTINIT;
 271         elm->can_frame_to_send.can_id = 0x7df; /* ELM327 HW default */
 272         elm->rxfill = 0;
 273         elm->drop_next_line = 0;
 274
 275         /* We can only set the bitrate as a fraction of 500000.
 276          * The bit timing constants in elmcan_bittiming_const will
 277          * limit the user to the right values.
 278          */
 279         elm->can_bitrate_divisor = 500000 / elm->can.bittiming.bitrate;
 280         elm->can_config = ELM327_CAN_CONFIG_SEND_SFF
 281                         | ELM327_CAN_CONFIG_VARIABLE_DLC
 282                         | ELM327_CAN_CONFIG_RECV_BOTH_SFF_EFF
 283                         | elm->can_bitrate_divisor;
 284
 285         /* Configure ELM327 and then start monitoring */
 286         elm->next_init_cmd = &elm327_init_script[0];
 287         set_bit(ELM327_TX_DO_INIT, &elm->cmds_todo);
 288         set_bit(ELM327_TX_DO_SILENT_MONITOR, &elm->cmds_todo);
 289         set_bit(ELM327_TX_DO_RESPONSES, &elm->cmds_todo);
 290         set_bit(ELM327_TX_DO_CAN_CONFIG, &elm->cmds_todo);
 291
 292         elm327_kick_into_cmd_mode(elm);
 293 }
 294
 295 /* Assumes elm->lock taken. */
 296 static void elm327_feed_frame_to_netdev(struct elmcan *elm,
 297                                         struct sk_buff *skb)
 298 {
 299         if (!netif_running(elm->dev))
 300                 return;
 301
 302         /* Queue for NAPI pickup.
 303          * rx-offload will update stats and LEDs for us.
 304          */
 305         if (can_rx_offload_queue_tail(&elm->offload, skb))
 306                 elm->dev->stats.rx_fifo_errors++;
 307
 308 #if LINUX_VERSION_CODE >= KERNEL_VERSION(5,15,0)
 309         /* Wake NAPI */
 310         can_rx_offload_irq_finish(&elm->offload);
 311 #endif
 312 }
 313
 314 /* Called when we're out of ideas and just want it all to end.
 315  * Assumes elm->lock taken.
 316  */
 317 static inline void elm327_hw_failure(struct elmcan *elm)
 318 {
 319         struct can_frame *frame;
 320         struct sk_buff *skb;
 321
 322         elm->hw_failure = true;
 323
 324         elm->can.can_stats.bus_off++;
 325         netif_stop_queue(elm->dev);
 326         elm->can.state = CAN_STATE_BUS_OFF;
 327         can_bus_off(elm->dev);
 328
 329         netdev_err(elm->dev, "ELM327 misbehaved. Blocking further communication.\n");
 330
 331         skb = alloc_can_err_skb(elm->dev, &frame);
 332         if (!skb)
 333                 return;
 334
 335         frame->can_id |= CAN_ERR_BUSOFF;
 336         elm327_feed_frame_to_netdev(elm, skb);
 337 }
 338
 339 /* Compare a buffer to a fixed string */
 340 static inline int _memstrcmp(const u8 *mem, const char *str)
 341 {
 342         return memcmp(mem, str, strlen(str));
 343 }
 344
 345 /* Compare buffer to string length, then compare buffer to fixed string.
 346  * This ensures two things:
 347  *  - It flags cases where the fixed string is only the start of the
 348  *    buffer, rather than exactly all of it.
 349  *  - It avoids byte comparisons in case the length doesn't match.
 350  */
 351 static inline int _len_memstrcmp(const u8 *mem, size_t mem_len, const char *str)
 352 {
 353         size_t str_len = strlen(str);
 354
 355         return (mem_len != str_len) || memcmp(mem, str, str_len);
 356 }
 357
 358 /* Assumes elm->lock taken. */
 359 static void elm327_parse_error(struct elmcan *elm, size_t len)
 360 {
 361         struct can_frame *frame;
 362         struct sk_buff *skb;
 363
 364         skb = alloc_can_err_skb(elm->dev, &frame);
 365         if (!skb)
 366                 /* It's okay to return here:
 367                  * The outer parsing loop will drop this UART buffer.
 368                  */
 369                 return;
 370
 371         /* Filter possible error messages based on length of RX'd line */
 372         if (!_len_memstrcmp(elm->rxbuf, len, "UNABLE TO CONNECT")) {
 373                 netdev_err(elm->dev,
 374                            "ELM327 reported UNABLE TO CONNECT. Please check your setup.\n");
 375         } else if (!_len_memstrcmp(elm->rxbuf, len, "BUFFER FULL")) {
 376                 /* This will only happen if the last data line was complete.
 377                  * Otherwise, elm327_parse_frame() will heuristically
 378                  * emit this kind of error frame instead.
 379                  */
 380                 frame->can_id |= CAN_ERR_CRTL;
 381                 frame->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
 382         } else if (!_len_memstrcmp(elm->rxbuf, len, "BUS ERROR")) {
 383                 frame->can_id |= CAN_ERR_BUSERROR;
 384         } else if (!_len_memstrcmp(elm->rxbuf, len, "CAN ERROR")) {
 385                 frame->can_id |= CAN_ERR_PROT;
 386         } else if (!_len_memstrcmp(elm->rxbuf, len, "<RX ERROR")) {
 387                 frame->can_id |= CAN_ERR_PROT;
 388         } else if (!_len_memstrcmp(elm->rxbuf, len, "BUS BUSY")) {
 389                 frame->can_id |= CAN_ERR_PROT;
 390                 frame->data[2] = CAN_ERR_PROT_OVERLOAD;
 391         } else if (!_len_memstrcmp(elm->rxbuf, len, "FB ERROR")) {
 392                 frame->can_id |= CAN_ERR_PROT;
 393                 frame->data[2] = CAN_ERR_PROT_TX;
 394         } else if (len == 5 && !_memstrcmp(elm->rxbuf, "ERR")) {
 395                 /* ERR is followed by two digits, hence line length 5 */
 396                 netdev_err(elm->dev, "ELM327 reported an ERR%c%c. Please power it off and on again.\n",
 397                            elm->rxbuf[3], elm->rxbuf[4]);
 398                 frame->can_id |= CAN_ERR_CRTL;
 399         } else {
 400                 /* Something else has happened.
 401                  * Maybe garbage on the UART line.
 402                  * Emit a generic error frame.
 403                  */
 404         }
 405
 406         elm327_feed_frame_to_netdev(elm, skb);
 407 }
 408
 409 /* Parse CAN frames coming as ASCII from ELM327.
 410  * They can be of various formats:
 411  *
 412  * 29-bit ID (EFF):  12 34 56 78 D PL PL PL PL PL PL PL PL
 413  * 11-bit ID (!EFF): 123 D PL PL PL PL PL PL PL PL
 414  *
 415  * where D = DLC, PL = payload byte
 416  *
 417  * Instead of a payload, RTR indicates a remote request.
 418  *
 419  * We will use the spaces and line length to guess the format.
 420  *
 421  * Assumes elm->lock taken.
 422  */
 423 static int elm327_parse_frame(struct elmcan *elm, size_t len)
 424 {
 425         struct can_frame *frame;
 426         struct sk_buff *skb;
 427         int hexlen;
 428         int datastart;
 429         int i;
 430
 431         skb = alloc_can_skb(elm->dev, &frame);
 432         if (!skb)
 433                 return -ENOMEM;
 434
 435         /* Find first non-hex and non-space character:
 436          *  - In the simplest case, there is none.
 437          *  - For RTR frames, 'R' is the first non-hex character.
 438          *  - An error message may replace the end of the data line.
 439          */
 440         for (hexlen = 0; hexlen <= len; hexlen++) {
 441                 if (hex_to_bin(elm->rxbuf[hexlen]) < 0 &&
 442                     elm->rxbuf[hexlen] != ' ') {
 443                         break;
 444                 }
 445         }
 446
 447         /* Sanity check whether the line is really a clean hexdump,
 448          * or terminated by an error message, or contains garbage.
 449          */
 450         if (hexlen < len &&
 451             !isdigit(elm->rxbuf[hexlen]) &&
 452             !isupper(elm->rxbuf[hexlen]) &&
 453             '<' != elm->rxbuf[hexlen] &&
 454             ' ' != elm->rxbuf[hexlen]) {
 455                 /* The line is likely garbled anyway, so bail.
 456                  * The main code will restart listening.
 457                  */
 458                 return -ENODATA;
 459         }
 460
 461         /* Use spaces in CAN ID to distinguish 29 or 11 bit address length.
 462          * No out-of-bounds access:
 463          * We use the fact that we can always read from elm->rxbuf.
 464          */
 465         if (elm->rxbuf[2] == ' ' && elm->rxbuf[5] == ' ' &&
 466             elm->rxbuf[8] == ' ' && elm->rxbuf[11] == ' ' &&
 467             elm->rxbuf[13] == ' ') {
 468                 frame->can_id = CAN_EFF_FLAG;
 469                 datastart = 14;
 470         } else if (elm->rxbuf[3] == ' ' && elm->rxbuf[5] == ' ') {
 471                 frame->can_id = 0;
 472                 datastart = 6;
 473         } else {
 474                 /* This is not a well-formatted data line.
 475                  * Assume it's an error message.
 476                  */
 477                 return -ENODATA;
 478         }
 479
 480         if (hexlen < datastart) {
 481                 /* The line is too short to be a valid frame hex dump.
 482                  * Something interrupted the hex dump or it is invalid.
 483                  */
 484                 return -ENODATA;
 485         }
 486
 487         /* From here on all chars up to buf[hexlen] are hex or spaces,
 488          * at well-defined offsets.
 489          */
 490
 491         /* Read CAN data length */
 492         frame->len = (hex_to_bin(elm->rxbuf[datastart - 2]) << 0);
 493
 494         /* Read CAN ID */
 495         if (frame->can_id & CAN_EFF_FLAG) {
 496                 frame->can_id |= (hex_to_bin(elm->rxbuf[0]) << 28)
 497                                | (hex_to_bin(elm->rxbuf[1]) << 24)
 498                                | (hex_to_bin(elm->rxbuf[3]) << 20)
 499                                | (hex_to_bin(elm->rxbuf[4]) << 16)
 500                                | (hex_to_bin(elm->rxbuf[6]) << 12)
 501                                | (hex_to_bin(elm->rxbuf[7]) << 8)
 502                                | (hex_to_bin(elm->rxbuf[9]) << 4)
 503                                | (hex_to_bin(elm->rxbuf[10]) << 0);
 504         } else {
 505                 frame->can_id |= (hex_to_bin(elm->rxbuf[0]) << 8)
 506                                | (hex_to_bin(elm->rxbuf[1]) << 4)
 507                                | (hex_to_bin(elm->rxbuf[2]) << 0);
 508         }
 509
 510         /* Check for RTR frame */
 511         if (elm->rxfill >= hexlen + 3 &&
 512             !_memstrcmp(&elm->rxbuf[hexlen], "RTR")) {
 513                 frame->can_id |= CAN_RTR_FLAG;
 514         }
 515
 516         /* Is the line long enough to hold the advertised payload?
 517          * Note: RTR frames have a DLC, but no actual payload.
 518          */
 519         if (!(frame->can_id & CAN_RTR_FLAG) &&
 520             (hexlen < frame->len * 3 + datastart)) {
 521                 /* Incomplete frame.
 522                  * Probably the ELM327's RS232 TX buffer was full.
 523                  * Emit an error frame and exit.
 524                  */
 525                 frame->can_id = CAN_ERR_FLAG | CAN_ERR_CRTL;
 526                 frame->len = CAN_ERR_DLC;
 527                 frame->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
 528                 elm327_feed_frame_to_netdev(elm, skb);
 529
 530                 /* Signal failure to parse.
 531                  * The line will be re-parsed as an error line, which will fail.
 532                  * However, this will correctly drop the state machine back into
 533                  * command mode.
 534                  */
 535                 return -ENODATA;
 536         }
 537
 538         /* Parse the data nibbles. */
 539         for (i = 0; i < frame->len; i++) {
 540                 frame->data[i] = (hex_to_bin(elm->rxbuf[datastart + 3*i]) << 4)
 541                                | (hex_to_bin(elm->rxbuf[datastart + 3*i + 1]));
 542         }
 543
 544         /* Feed the frame to the network layer. */
 545         elm327_feed_frame_to_netdev(elm, skb);
 546
 547         return 0;
 548 }
 549
 550 /* Assumes elm->lock taken. */
 551 static void elm327_parse_line(struct elmcan *elm, size_t len)
 552 {
 553         /* Skip empty lines */
 554         if (!len)
 555                 return;
 556
 557         /* Skip echo lines */
 558         if (elm->drop_next_line) {
 559                 elm->drop_next_line = 0;
 560                 return;
 561         } else if (!_memstrcmp(elm->rxbuf, "AT")) {
 562                 return;
 563         }
 564
 565         /* Regular parsing */
 566         if (elm->state == ELM_RECEIVING
 567             && elm327_parse_frame(elm, len)) {
 568                 /* Parse an error line. */
 569                 elm327_parse_error(elm, len);
 570
 571                 /* Start afresh. */
 572                 elm327_kick_into_cmd_mode(elm);
 573         }
 574 }
 575
 576 /* Assumes elm->lock taken. */
 577 static void elm327_handle_prompt(struct elmcan *elm)
 578 {
 579         struct can_frame *frame = &elm->can_frame_to_send;
 580         char local_txbuf[20];
 581
 582         if (!elm->cmds_todo) {
 583                 /* Enter CAN monitor mode */
 584                 elm327_send(elm, "ATMA\r", 5);
 585                 elm->state = ELM_RECEIVING;
 586
 587                 return;
 588         }
 589
 590         /* Reconfigure ELM327 step by step as indicated by elm->cmds_todo */
 591         if (test_bit(ELM327_TX_DO_INIT, &elm->cmds_todo)) {
 592                 strcpy(local_txbuf, *elm->next_init_cmd);
 593
 594                 elm->next_init_cmd++;
 595                 if (!(*elm->next_init_cmd)) {
 596                         clear_bit(ELM327_TX_DO_INIT, &elm->cmds_todo);
 597                         /* Init finished. */
 598                 }
 599
 600         } else if (test_and_clear_bit(ELM327_TX_DO_SILENT_MONITOR, &elm->cmds_todo)) {
 601                 sprintf(local_txbuf, "ATCSM%i\r",
 602                         !(!(elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY)));
 603
 604         } else if (test_and_clear_bit(ELM327_TX_DO_RESPONSES, &elm->cmds_todo)) {
 605                 sprintf(local_txbuf, "ATR%i\r",
 606                         !(elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY));
 607
 608         } else if (test_and_clear_bit(ELM327_TX_DO_CAN_CONFIG, &elm->cmds_todo)) {
 609                 sprintf(local_txbuf, "ATPC\r");
 610                 set_bit(ELM327_TX_DO_CAN_CONFIG_PART2, &elm->cmds_todo);
 611
 612         } else if (test_and_clear_bit(ELM327_TX_DO_CAN_CONFIG_PART2, &elm->cmds_todo)) {
 613                 sprintf(local_txbuf, "ATPB%04X\r",
 614                         elm->can_config);
 615
 616         } else if (test_and_clear_bit(ELM327_TX_DO_CANID_29BIT_HIGH, &elm->cmds_todo)) {
 617                 sprintf(local_txbuf, "ATCP%02X\r",
 618                         (frame->can_id & CAN_EFF_MASK) >> 24);
 619
 620         } else if (test_and_clear_bit(ELM327_TX_DO_CANID_29BIT_LOW, &elm->cmds_todo)) {
 621                 sprintf(local_txbuf, "ATSH%06X\r",
 622                         frame->can_id & CAN_EFF_MASK & ((1 << 24) - 1));
 623
 624         } else if (test_and_clear_bit(ELM327_TX_DO_CANID_11BIT, &elm->cmds_todo)) {
 625                 sprintf(local_txbuf, "ATSH%03X\r",
 626                         frame->can_id & CAN_SFF_MASK);
 627
 628         } else if (test_and_clear_bit(ELM327_TX_DO_CAN_DATA, &elm->cmds_todo)) {
 629                 if (frame->can_id & CAN_RTR_FLAG) {
 630                         /* Send an RTR frame. Their DLC is fixed.
 631                          * Some chips don't send them at all.
 632                          */
 633                         sprintf(local_txbuf, "ATRTR\r");
 634                 } else {
 635                         /* Send a regular CAN data frame */
 636                         int i;
 637
 638                         for (i = 0; i < frame->len; i++) {
 639                                 sprintf(&local_txbuf[2 * i], "%02X",
 640                                         frame->data[i]);
 641                         }
 642
 643                         sprintf(&local_txbuf[2 * i], "\r");
 644                 }
 645
 646                 elm->drop_next_line = 1;
 647                 elm->state = ELM_RECEIVING;
 648         }
 649
 650         elm327_send(elm, local_txbuf, strlen(local_txbuf));
 651 }
 652
 653 static bool elm327_is_ready_char(char c)
 654 {
 655         /* Bits 0xc0 are sometimes set (randomly), hence the mask.
 656          * Probably bad hardware.
 657          */
 658         return (c & 0x3f) == ELM327_READY_CHAR;
 659 }
 660
 661 /* Assumes elm->lock taken. */
 662 static void elm327_drop_bytes(struct elmcan *elm, size_t i)
 663 {
 664         memmove(&elm->rxbuf[0], &elm->rxbuf[i], ELM327_SIZE_RXBUF - i);
 665         elm->rxfill -= i;
 666 }
 667
 668 /* Assumes elm->lock taken. */
 669 static void elm327_parse_rxbuf(struct elmcan *elm)
 670 {
 671         size_t len;
 672         int i;
 673
 674         switch (elm->state) {
 675         case ELM_NOTINIT:
 676                 elm->rxfill = 0;
 677                 break;
 678
 679         case ELM_GETDUMMYCHAR:
 680         {
 681                 /* Wait for 'y' or '>' */
 682                 for (i = 0; i < elm->rxfill; i++) {
 683                         if (elm->rxbuf[i] == ELM327_DUMMY_CHAR) {
 684                                 elm327_send(elm, "\r", 1);
 685                                 elm->state = ELM_GETPROMPT;
 686                                 i++;
 687                                 break;
 688                         } else if (elm327_is_ready_char(elm->rxbuf[i])) {
 689                                 elm327_send(elm, ELM327_DUMMY_STRING, 1);
 690                                 i++;
 691                                 break;
 692                         }
 693                 }
 694
 695                 elm327_drop_bytes(elm, i);
 696
 697                 break;
 698         }
 699
 700         case ELM_GETPROMPT:
 701                 /* Wait for '>' */
 702                 if (elm327_is_ready_char(elm->rxbuf[elm->rxfill - 1]))
 703                         elm327_handle_prompt(elm);
 704
 705                 elm->rxfill = 0;
 706                 break;
 707
 708         case ELM_RECEIVING:
 709                 /* Find <CR> delimiting feedback lines. */
 710                 for (len = 0;
 711                      (len < elm->rxfill) && (elm->rxbuf[len] != '\r');
 712                      len++) {
 713                         /* empty loop */
 714                 }
 715
 716                 if (len == ELM327_SIZE_RXBUF) {
 717                         /* Line exceeds buffer. It's probably all garbage.
 718                          * Did we even connect at the right baud rate?
 719                          */
 720                         netdev_err(elm->dev,
 721                                    "RX buffer overflow. Faulty ELM327 or UART?\n");
 722                         elm327_hw_failure(elm);
 723                         break;
 724                 } else if (len == elm->rxfill) {
 725                         if (elm327_is_ready_char(elm->rxbuf[elm->rxfill - 1])) {
 726                                 /* The ELM327's AT ST response timeout ran out,
 727                                  * so we got a prompt.
 728                                  * Clear RX buffer and restart listening.
 729                                  */
 730                                 elm->rxfill = 0;
 731
 732                                 elm327_handle_prompt(elm);
 733                                 break;
 734                         }
 735
 736                         /* No <CR> found - we haven't received a full line yet.
 737                          * Wait for more data.
 738                          */
 739                         break;
 740                 }
 741
 742                 /* We have a full line to parse. */
 743                 elm327_parse_line(elm, len);
 744
 745                 /* Remove parsed data from RX buffer. */
 746                 elm327_drop_bytes(elm, len + 1);
 747
 748                 /* More data to parse? */
 749                 if (elm->rxfill)
 750                         elm327_parse_rxbuf(elm);
 751         }
 752 }
 753
 754 /* Dummy needed to use can_rx_offload */
 755 static struct sk_buff *elmcan_mailbox_read(struct can_rx_offload *offload,
 756                                            unsigned int n, u32 *timestamp,
 757                                            bool drop)
 758 {
 759         WARN_ON_ONCE(1); /* This function is a dummy, so don't call it! */
 760
 761         return ERR_PTR(-ENOBUFS);
 762 }
 763
 764 static int elmcan_netdev_open(struct net_device *dev)
 765 {
 766         struct elmcan *elm = netdev_priv(dev);
 767         int err;
 768
 769         spin_lock_bh(&elm->lock);
 770         if (elm->hw_failure) {
 771                 netdev_err(elm->dev, "Refusing to open interface after a hardware fault has been detected.\n");
 772                 spin_unlock_bh(&elm->lock);
 773                 return -EIO;
 774         }
 775
 776         if (!elm->tty) {
 777                 spin_unlock_bh(&elm->lock);
 778                 return -ENODEV;
 779         }
 780
 781         /* open_candev() checks for elm->can.bittiming.bitrate != 0 */
 782         err = open_candev(dev);
 783         if (err) {
 784                 spin_unlock_bh(&elm->lock);
 785                 return err;
 786         }
 787
 788         elm327_init(elm);
 789         spin_unlock_bh(&elm->lock);
 790
 791         elm->offload.mailbox_read = elmcan_mailbox_read;
 792         err = can_rx_offload_add_fifo(dev, &elm->offload, ELM327_NAPI_WEIGHT);
 793         if (err) {
 794                 close_candev(dev);
 795                 return err;
 796         }
 797
 798         can_rx_offload_enable(&elm->offload);
 799
 800         can_led_event(dev, CAN_LED_EVENT_OPEN);
 801         elm->can.state = CAN_STATE_ERROR_ACTIVE;
 802         netif_start_queue(dev);
 803
 804         return 0;
 805 }
 806
 807 static int elmcan_netdev_close(struct net_device *dev)
 808 {
 809         struct elmcan *elm = netdev_priv(dev);
 810
 811         netif_stop_queue(dev);
 812
 813         spin_lock_bh(&elm->lock);
 814         if (elm->tty) {
 815                 /* Interrupt whatever we're doing right now */
 816                 elm327_send(elm, ELM327_DUMMY_STRING, 1);
 817
 818                 /* Clear the wakeup bit, as the netdev will be down and thus
 819                  * the wakeup handler won't clear it
 820                  */
 821                 clear_bit(TTY_DO_WRITE_WAKEUP, &elm->tty->flags);
 822
 823                 spin_unlock_bh(&elm->lock);
 824
 825                 flush_work(&elm->tx_work);
 826         } else {
 827                 spin_unlock_bh(&elm->lock);
 828         }
 829
 830         can_rx_offload_disable(&elm->offload);
 831         elm->can.state = CAN_STATE_STOPPED;
 832         can_rx_offload_del(&elm->offload);
 833         close_candev(dev);
 834         can_led_event(dev, CAN_LED_EVENT_STOP);
 835
 836         return 0;
 837 }
 838
 839 /* Send a can_frame to a TTY. */
 840 static netdev_tx_t elmcan_netdev_start_xmit(struct sk_buff *skb,
 841                                             struct net_device *dev)
 842 {
 843         struct elmcan *elm = netdev_priv(dev);
 844         struct can_frame *frame = (struct can_frame *)skb->data;
 845
 846         /* BHs are already disabled, so no spin_lock_bh().
 847          * See Documentation/networking/netdevices.txt
 848          */
 849         spin_lock(&elm->lock);
 850
 851         /* We shouldn't get here after a hardware fault:
 852          * can_bus_off() calls netif_carrier_off()
 853          */
 854         WARN_ON_ONCE(elm->hw_failure);
 855
 856         if (!elm->tty ||
 857             elm->hw_failure ||
 858             elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY) {
 859                 spin_unlock(&elm->lock);
 860                 goto out;
 861         }
 862
 863         netif_stop_queue(dev);
 864
 865         elm327_send_frame(elm, frame);
 866         spin_unlock(&elm->lock);
 867
 868         dev->stats.tx_packets++;
 869         dev->stats.tx_bytes += frame->len;
 870
 871         can_led_event(dev, CAN_LED_EVENT_TX);
 872
 873 out:
 874         kfree_skb(skb);
 875         return NETDEV_TX_OK;
 876 }
 877
 878 static const struct net_device_ops elmcan_netdev_ops = {
 879         .ndo_open       = elmcan_netdev_open,
 880         .ndo_stop       = elmcan_netdev_close,
 881         .ndo_start_xmit = elmcan_netdev_start_xmit,
 882         .ndo_change_mtu = can_change_mtu,
 883 };
 884
 885 /* Get a reference to our struct, taking into account locks/refcounts.
 886  * This is to ensure ordering in case we are shutting down, and to ensure
 887  * there is a refcount at all (otherwise tty->disc_data may be freed and
 888  * before we increment the refcount).
 889  * Use this for anything that can race against elmcan_ldisc_close().
 890  */
 891 static struct elmcan *get_elm(struct tty_struct *tty)
 892 {
 893         struct elmcan *elm;
 894         bool got_ref;
 895
 896         spin_lock_bh(&elmcan_discdata_lock);
 897         elm = (struct elmcan *)tty->disc_data;
 898
 899         if (!elm) {
 900                 spin_unlock_bh(&elmcan_discdata_lock);
 901                 return NULL;
 902         }
 903
 904         got_ref = atomic_inc_not_zero(&elm->refcount);
 905         spin_unlock_bh(&elmcan_discdata_lock);
 906
 907         if (!got_ref)
 908                 return NULL;
 909
 910         return elm;
 911 }
 912
 913 static void put_elm(struct elmcan *elm)
 914 {
 915         atomic_dec(&elm->refcount);
 916 }
 917
 918 static bool elmcan_is_valid_rx_char(char c)
 919 {
 920         return (isdigit(c) ||
 921                 isupper(c) ||
 922                 c == ELM327_DUMMY_CHAR ||
 923                 c == ELM327_READY_CHAR ||
 924                 c == '<' ||
 925                 c == 'a' ||
 926                 c == 'b' ||
 927                 c == 'v' ||
 928                 c == '.' ||
 929                 c == '?' ||
 930                 c == '\r' ||
 931                 c == ' ');
 932 }
 933
 934 /* Handle incoming ELM327 ASCII data.
 935  * This will not be re-entered while running, but other ldisc
 936  * functions may be called in parallel.
 937  */
 938 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,14,0)
 939 static void elmcan_ldisc_rx(struct tty_struct *tty,
 940                             const unsigned char *cp, char *fp, int count)
 941 #else
 942 static void elmcan_ldisc_rx(struct tty_struct *tty,
 943                             const unsigned char *cp, const char *fp, int count)
 944 #endif
 945 {
 946         struct elmcan *elm = get_elm(tty);
 947
 948         if (!elm)
 949                 return;
 950
 951         spin_lock_bh(&elm->lock);
 952
 953         if (elm->hw_failure)
 954                 goto out;
 955
 956         while (count-- && elm->rxfill < ELM327_SIZE_RXBUF) {
 957                 if (fp && *fp++) {
 958                         netdev_err(elm->dev, "Error in received character stream. Check your wiring.");
 959
 960                         elm327_hw_failure(elm);
 961
 962                         goto out;
 963                 }
 964
 965                 /* Ignore NUL characters, which the PIC microcontroller may
 966                  * inadvertently insert due to a known hardware bug.
 967                  * See ELM327 documentation, which refers to a Microchip PIC
 968                  * bug description.
 969                  */
 970                 if (*cp != 0) {
 971                         /* Check for stray characters on the UART line.
 972                          * Likely caused by bad hardware.
 973                          */
 974                         if (!elmcan_is_valid_rx_char(*cp)) {
 975                                 netdev_err(elm->dev,
 976                                            "Received illegal character %02x.\n",
 977                                            *cp);
 978                                 elm327_hw_failure(elm);
 979
 980                                 goto out;
 981                         }
 982
 983                         elm->rxbuf[elm->rxfill++] = *cp;
 984                 }
 985
 986                 cp++;
 987         }
 988
 989         if (count >= 0) {
 990                 netdev_err(elm->dev, "Receive buffer overflowed. Bad chip or wiring?");
 991
 992                 elm327_hw_failure(elm);
 993
 994                 goto out;
 995         }
 996
 997         elm327_parse_rxbuf(elm);
 998
 999 out:
1000         spin_unlock_bh(&elm->lock);
1001         put_elm(elm);
1002 }
1003
1004 /* Write out remaining transmit buffer.
1005  * Scheduled when TTY is writable.
1006  */
1007 static void elmcan_ldisc_tx_worker(struct work_struct *work)
1008 {
1009         /* No need to use get_elm() here, as we'll always flush workers
1010          * before destroying the elmcan object.
1011          */
1012         struct elmcan *elm = container_of(work, struct elmcan, tx_work);
1013         ssize_t actual;
1014
1015         spin_lock_bh(&elm->lock);
1016         if (elm->hw_failure) {
1017                 spin_unlock_bh(&elm->lock);
1018                 return;
1019         }
1020
1021         if (!elm->tty || !netif_running(elm->dev)) {
1022                 spin_unlock_bh(&elm->lock);
1023                 return;
1024         }
1025
1026         if (elm->txleft <= 0)  {
1027                 /* Our TTY write buffer is empty:
1028                  * Allow netdev to hand us another packet
1029                  */
1030                 clear_bit(TTY_DO_WRITE_WAKEUP, &elm->tty->flags);
1031                 spin_unlock_bh(&elm->lock);
1032                 netif_wake_queue(elm->dev);
1033                 return;
1034         }
1035
1036         actual = elm->tty->ops->write(elm->tty, elm->txhead, elm->txleft);
1037         if (actual < 0) {
1038                 netdev_err(elm->dev,
1039                            "Failed to write to tty %s.\n",
1040                            elm->tty->name);
1041                 elm327_hw_failure(elm);
1042                 spin_unlock_bh(&elm->lock);
1043                 return;
1044         }
1045
1046         elm->txleft -= actual;
1047         elm->txhead += actual;
1048         spin_unlock_bh(&elm->lock);
1049 }
1050
1051 /* Called by the driver when there's room for more data. */
1052 static void elmcan_ldisc_tx_wakeup(struct tty_struct *tty)
1053 {
1054         struct elmcan *elm = get_elm(tty);
1055
1056         if (!elm)
1057                 return;
1058
1059         schedule_work(&elm->tx_work);
1060
1061         put_elm(elm);
1062 }
1063
1064 /* ELM327 can only handle bitrates that are integer divisors of 500 kHz,
1065  * or 7/8 of that. Divisors are 1 to 64.
1066  * Currently we don't implement support for 7/8 rates.
1067  */
1068 static const u32 elmcan_bitrate_const[64] = {
1069          7812,  7936,  8064,  8196,  8333,  8474,  8620,  8771,
1070          8928,  9090,  9259,  9433,  9615,  9803, 10000, 10204,
1071         10416, 10638, 10869, 11111, 11363, 11627, 11904, 12195,
1072         12500, 12820, 13157, 13513, 13888, 14285, 14705, 15151,
1073         15625, 16129, 16666, 17241, 17857, 18518, 19230, 20000,
1074         20833, 21739, 22727, 23809, 25000, 26315, 27777, 29411,
1075         31250, 33333, 35714, 38461, 41666, 45454, 50000, 55555,
1076         62500, 71428, 83333, 100000, 125000, 166666, 250000, 500000
1077 };
1078
1079 /* Dummy needed to use bitrate_const */
1080 static int elmcan_do_set_bittiming(struct net_device *netdev)
1081 {
1082         return 0;
1083 }
1084
1085 static int elmcan_ldisc_open(struct tty_struct *tty)
1086 {
1087         struct net_device *dev;
1088         struct elmcan *elm;
1089         int err;
1090
1091         if (!capable(CAP_NET_ADMIN))
1092                 return -EPERM;
1093
1094         if (!tty->ops->write)
1095                 return -EOPNOTSUPP;
1096
1097         dev = alloc_candev(sizeof(struct elmcan), 0);
1098         if (!dev)
1099                 return -ENFILE;
1100         elm = netdev_priv(dev);
1101
1102         elm->txbuf = kmalloc(ELM327_SIZE_TXBUF, GFP_KERNEL);
1103         if (!elm->txbuf) {
1104                 err = -ENOMEM;
1105                 goto out_err;
1106         }
1107
1108         /* Configure TTY interface */
1109         tty->receive_room = 65536; /* We don't flow control */
1110         elm->txleft = 0; /* Clear TTY TX buffer */
1111         spin_lock_init(&elm->lock);
1112         atomic_set(&elm->refcount, 1);
1113         INIT_WORK(&elm->tx_work, elmcan_ldisc_tx_worker);
1114
1115         /* Configure CAN metadata */
1116         elm->can.state = CAN_STATE_STOPPED;
1117         elm->can.bitrate_const = elmcan_bitrate_const;
1118         elm->can.bitrate_const_cnt = ARRAY_SIZE(elmcan_bitrate_const);
1119         elm->can.do_set_bittiming = elmcan_do_set_bittiming;
1120         elm->can.ctrlmode_supported = CAN_CTRLMODE_LISTENONLY;
1121
1122         /* Configure netdev interface */
1123         elm->dev = dev;
1124         dev->netdev_ops = &elmcan_netdev_ops;
1125
1126         /* Mark ldisc channel as alive */
1127         elm->tty = tty;
1128         tty->disc_data = elm;
1129
1130         devm_can_led_init(elm->dev);
1131
1132         /* Let 'er rip */
1133         err = register_candev(elm->dev);
1134         if (err)
1135                 goto out_err;
1136
1137         netdev_info(elm->dev, "elmcan on %s.\n", tty->name);
1138
1139         return 0;
1140
1141 out_err:
1142         kfree(elm->txbuf);
1143         free_candev(elm->dev);
1144         return err;
1145 }
1146
1147 /* Close down an elmcan channel.
1148  * This means flushing out any pending queues, and then returning.
1149  * This call is serialized against other ldisc functions:
1150  * Once this is called, no other ldisc function of ours is entered.
1151  *
1152  * We also use this function for a hangup event.
1153  */
1154 static void elmcan_ldisc_close(struct tty_struct *tty)
1155 {
1156         struct elmcan *elm = get_elm(tty);
1157
1158         if (!elm)
1159                 return;
1160
1161         /* unregister_netdev() calls .ndo_stop() so we don't have to. */
1162         unregister_candev(elm->dev);
1163
1164         /* Decrease the refcount twice, once for our own get_elm(),
1165          * and once to remove the count of 1 that we set in _open().
1166          * Once it reaches 0, we can safely destroy it.
1167          */
1168         put_elm(elm);
1169         put_elm(elm);
1170
1171         while (atomic_read(&elm->refcount) > 0)
1172                 msleep_interruptible(10);
1173
1174         /* At this point, all ldisc calls to us have become no-ops. */
1175
1176         flush_work(&elm->tx_work);
1177
1178         /* Mark channel as dead */
1179         spin_lock_bh(&elm->lock);
1180         tty->disc_data = NULL;
1181         elm->tty = NULL;
1182         spin_unlock_bh(&elm->lock);
1183
1184         netdev_info(elm->dev, "elmcan off %s.\n", tty->name);
1185
1186         kfree(elm->txbuf);
1187         free_candev(elm->dev);
1188 }
1189
1190 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,16,0)
1191 static int elmcan_ldisc_hangup(struct tty_struct *tty)
1192 #else
1193 static void elmcan_ldisc_hangup(struct tty_struct *tty)
1194 #endif
1195 {
1196         elmcan_ldisc_close(tty);
1197 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,16,0)
1198         return 0;
1199 #endif
1200 }
1201
1202 static int elmcan_ldisc_ioctl(struct tty_struct *tty,
1203 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,17,0)
1204                               struct file *file,
1205 #endif
1206                               unsigned int cmd, unsigned long arg)
1207 {
1208         struct elmcan *elm = get_elm(tty);
1209         unsigned int tmp;
1210
1211         if (!elm)
1212                 return -EINVAL;
1213
1214         switch (cmd) {
1215         case SIOCGIFNAME:
1216                 tmp = strnlen(elm->dev->name, IFNAMSIZ - 1) + 1;
1217                 if (copy_to_user((void __user *)arg, elm->dev->name, tmp)) {
1218                         put_elm(elm);
1219                         return -EFAULT;
1220                 }
1221
1222                 put_elm(elm);
1223                 return 0;
1224
1225         case SIOCSIFHWADDR:
1226                 put_elm(elm);
1227                 return -EINVAL;
1228
1229         default:
1230                 put_elm(elm);
1231 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,16,0)
1232                 return tty_mode_ioctl(tty, file, cmd, arg);
1233 #else
1234                 return tty_mode_ioctl(tty, cmd, arg);
1235 #endif
1236         }
1237 }
1238
1239 static struct tty_ldisc_ops elmcan_ldisc = {
1240         .owner          = THIS_MODULE,
1241         .name           = "elmcan",
1242         .num            = N_DEVELOPMENT,
1243         .receive_buf    = elmcan_ldisc_rx,
1244         .write_wakeup   = elmcan_ldisc_tx_wakeup,
1245         .open           = elmcan_ldisc_open,
1246         .close          = elmcan_ldisc_close,
1247         .hangup         = elmcan_ldisc_hangup,
1248         .ioctl          = elmcan_ldisc_ioctl,
1249 };
1250
1251 static int __init elmcan_init(void)
1252 {
1253         int status;
1254
1255 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,14,0)
1256         status = tty_register_ldisc(N_DEVELOPMENT, &elmcan_ldisc);
1257 #else
1258         status = tty_register_ldisc(&elmcan_ldisc);
1259 #endif
1260         if (status)
1261                 pr_err("Can't register line discipline\n");
1262
1263         return status;
1264 }
1265
1266 static void __exit elmcan_exit(void)
1267 {
1268         /* This will only be called when all channels have been closed by
1269          * userspace - tty_ldisc.c takes care of the module's refcount.
1270          */
1271 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,14,0)
1272         int status;
1273
1274         status = tty_unregister_ldisc(N_DEVELOPMENT);
1275         if (status)
1276                 pr_err("Can't unregister line discipline (error: %d)\n",
1277                        status);
1278 #else
1279         tty_unregister_ldisc(&elmcan_ldisc);
1280 #endif
1281 }
1282
1283 module_init(elmcan_init);
1284 module_exit(elmcan_exit);