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