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