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