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 a buffer to a fixed string */
 320 static inline int _memstrcmp(const u8 *mem, const char *str)
 321 {
 322         return memcmp(mem, str, strlen(str));
 323 }
 324
 325 /* Compare buffer to string length, then compare buffer to fixed string.
 326  * This ensures two things:
 327  *  - It flags cases where the fixed string is only the start of the
 328  *    buffer, rather than exactly all of it.
 329  *  - It avoids byte comparisons in case the length doesn't match.
 330  */
 331 static inline int _len_memstrcmp(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 (!_len_memstrcmp(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 (!_len_memstrcmp(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 (!_len_memstrcmp(elm->rxbuf, len, "BUS ERROR")) {
 364                 frame->can_id |= CAN_ERR_BUSERROR;
 365         } else if (!_len_memstrcmp(elm->rxbuf, len, "CAN ERROR")) {
 366                 frame->can_id |= CAN_ERR_PROT;
 367         } else if (!_len_memstrcmp(elm->rxbuf, len, "<RX ERROR")) {
 368                 frame->can_id |= CAN_ERR_PROT;
 369         } else if (!_len_memstrcmp(elm->rxbuf, len, "BUS BUSY")) {
 370                 frame->can_id |= CAN_ERR_PROT;
 371                 frame->data[2] = CAN_ERR_PROT_OVERLOAD;
 372         } else if (!_len_memstrcmp(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 && !_memstrcmp(elm->rxbuf, "ERR")) {
 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             !_memstrcmp(&elm->rxbuf[hexlen], "RTR")) {
 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 (!_memstrcmp(elm->rxbuf, "AT")) {
 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         char local_txbuf[20];
 561
 562         lockdep_assert_held(elm->lock);
 563
 564         if (!elm->cmds_todo) {
 565                 /* Enter CAN monitor mode */
 566                 elm327_send(elm, "ATMA\r", 5);
 567                 elm->state = ELM327_STATE_RECEIVING;
 568
 569                 return;
 570         }
 571
 572         /* Reconfigure ELM327 step by step as indicated by elm->cmds_todo */
 573         if (test_bit(ELM327_TX_DO_INIT, &elm->cmds_todo)) {
 574                 strcpy(local_txbuf, *elm->next_init_cmd);
 575
 576                 elm->next_init_cmd++;
 577                 if (!(*elm->next_init_cmd)) {
 578                         clear_bit(ELM327_TX_DO_INIT, &elm->cmds_todo);
 579                         /* Init finished. */
 580                 }
 581
 582         } else if (test_and_clear_bit(ELM327_TX_DO_SILENT_MONITOR, &elm->cmds_todo)) {
 583                 sprintf(local_txbuf, "ATCSM%i\r",
 584                         !(!(elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY)));
 585
 586         } else if (test_and_clear_bit(ELM327_TX_DO_RESPONSES, &elm->cmds_todo)) {
 587                 sprintf(local_txbuf, "ATR%i\r",
 588                         !(elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY));
 589
 590         } else if (test_and_clear_bit(ELM327_TX_DO_CAN_CONFIG, &elm->cmds_todo)) {
 591                 sprintf(local_txbuf, "ATPC\r");
 592                 set_bit(ELM327_TX_DO_CAN_CONFIG_PART2, &elm->cmds_todo);
 593
 594         } else if (test_and_clear_bit(ELM327_TX_DO_CAN_CONFIG_PART2, &elm->cmds_todo)) {
 595                 sprintf(local_txbuf, "ATPB%04X\r",
 596                         elm->can_config);
 597
 598         } else if (test_and_clear_bit(ELM327_TX_DO_CANID_29BIT_HIGH, &elm->cmds_todo)) {
 599                 sprintf(local_txbuf, "ATCP%02X\r",
 600                         (frame->can_id & CAN_EFF_MASK) >> 24);
 601
 602         } else if (test_and_clear_bit(ELM327_TX_DO_CANID_29BIT_LOW, &elm->cmds_todo)) {
 603                 sprintf(local_txbuf, "ATSH%06X\r",
 604                         frame->can_id & CAN_EFF_MASK & ((1 << 24) - 1));
 605
 606         } else if (test_and_clear_bit(ELM327_TX_DO_CANID_11BIT, &elm->cmds_todo)) {
 607                 sprintf(local_txbuf, "ATSH%03X\r",
 608                         frame->can_id & CAN_SFF_MASK);
 609
 610         } else if (test_and_clear_bit(ELM327_TX_DO_CAN_DATA, &elm->cmds_todo)) {
 611                 if (frame->can_id & CAN_RTR_FLAG) {
 612                         /* Send an RTR frame. Their DLC is fixed.
 613                          * Some chips don't send them at all.
 614                          */
 615                         sprintf(local_txbuf, "ATRTR\r");
 616                 } else {
 617                         /* Send a regular CAN data frame */
 618                         int i;
 619
 620                         for (i = 0; i < frame->len; i++) {
 621                                 sprintf(&local_txbuf[2 * i], "%02X",
 622                                         frame->data[i]);
 623                         }
 624
 625                         sprintf(&local_txbuf[2 * i], "\r");
 626                 }
 627
 628                 elm->drop_next_line = 1;
 629                 elm->state = ELM327_STATE_RECEIVING;
 630         }
 631
 632         elm327_send(elm, local_txbuf, strlen(local_txbuf));
 633 }
 634
 635 static bool elm327_is_ready_char(char c)
 636 {
 637         /* Bits 0xc0 are sometimes set (randomly), hence the mask.
 638          * Probably bad hardware.
 639          */
 640         return (c & 0x3f) == ELM327_READY_CHAR;
 641 }
 642
 643 static void elm327_drop_bytes(struct elmcan *elm, size_t i)
 644 {
 645         lockdep_assert_held(elm->lock);
 646
 647         memmove(&elm->rxbuf[0], &elm->rxbuf[i], ELM327_SIZE_RXBUF - i);
 648         elm->rxfill -= i;
 649 }
 650
 651 static void elm327_parse_rxbuf(struct elmcan *elm)
 652 {
 653         size_t len;
 654         int i;
 655
 656         lockdep_assert_held(elm->lock);
 657
 658         switch (elm->state) {
 659         case ELM327_STATE_NOTINIT:
 660                 elm->rxfill = 0;
 661                 break;
 662
 663         case ELM327_STATE_GETDUMMYCHAR:
 664         {
 665                 /* Wait for 'y' or '>' */
 666                 for (i = 0; i < elm->rxfill; i++) {
 667                         if (elm->rxbuf[i] == ELM327_DUMMY_CHAR) {
 668                                 elm327_send(elm, "\r", 1);
 669                                 elm->state = ELM327_STATE_GETPROMPT;
 670                                 i++;
 671                                 break;
 672                         } else if (elm327_is_ready_char(elm->rxbuf[i])) {
 673                                 elm327_send(elm, ELM327_DUMMY_STRING, 1);
 674                                 i++;
 675                                 break;
 676                         }
 677                 }
 678
 679                 elm327_drop_bytes(elm, i);
 680
 681                 break;
 682         }
 683
 684         case ELM327_STATE_GETPROMPT:
 685                 /* Wait for '>' */
 686                 if (elm327_is_ready_char(elm->rxbuf[elm->rxfill - 1]))
 687                         elm327_handle_prompt(elm);
 688
 689                 elm->rxfill = 0;
 690                 break;
 691
 692         case ELM327_STATE_RECEIVING:
 693                 /* Find <CR> delimiting feedback lines. */
 694                 for (len = 0;
 695                      (len < elm->rxfill) && (elm->rxbuf[len] != '\r');
 696                      len++) {
 697                         /* empty loop */
 698                 }
 699
 700                 if (len == ELM327_SIZE_RXBUF) {
 701                         /* Line exceeds buffer. It's probably all garbage.
 702                          * Did we even connect at the right baud rate?
 703                          */
 704                         netdev_err(elm->dev,
 705                                    "RX buffer overflow. Faulty ELM327 or UART?\n");
 706                         elm327_hw_failure(elm);
 707                         break;
 708                 } else if (len == elm->rxfill) {
 709                         if (elm327_is_ready_char(elm->rxbuf[elm->rxfill - 1])) {
 710                                 /* The ELM327's AT ST response timeout ran out,
 711                                  * so we got a prompt.
 712                                  * Clear RX buffer and restart listening.
 713                                  */
 714                                 elm->rxfill = 0;
 715
 716                                 elm327_handle_prompt(elm);
 717                                 break;
 718                         }
 719
 720                         /* No <CR> found - we haven't received a full line yet.
 721                          * Wait for more data.
 722                          */
 723                         break;
 724                 }
 725
 726                 /* We have a full line to parse. */
 727                 elm327_parse_line(elm, len);
 728
 729                 /* Remove parsed data from RX buffer. */
 730                 elm327_drop_bytes(elm, len + 1);
 731
 732                 /* More data to parse? */
 733                 if (elm->rxfill)
 734                         elm327_parse_rxbuf(elm);
 735         }
 736 }
 737
 738 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,10,0)
 739 /* Dummy needed to use can_rx_offload */
 740 static struct sk_buff *elmcan_mailbox_read(struct can_rx_offload *offload,
 741                                            unsigned int n, u32 *timestamp,
 742                                            bool drop)
 743 {
 744         WARN_ON_ONCE(1); /* This function is a dummy, so don't call it! */
 745
 746         return ERR_PTR(-ENOBUFS);
 747 }
 748 #endif
 749
 750 static int elmcan_netdev_open(struct net_device *dev)
 751 {
 752         struct elmcan *elm = netdev_priv(dev);
 753         int err;
 754
 755         spin_lock_bh(&elm->lock);
 756         if (elm->hw_failure) {
 757                 netdev_err(elm->dev, "Refusing to open interface after a hardware fault has been detected.\n");
 758                 spin_unlock_bh(&elm->lock);
 759                 return -EIO;
 760         }
 761
 762         if (!elm->tty) {
 763                 spin_unlock_bh(&elm->lock);
 764                 return -ENODEV;
 765         }
 766
 767         /* open_candev() checks for elm->can.bittiming.bitrate != 0 */
 768         err = open_candev(dev);
 769         if (err) {
 770                 spin_unlock_bh(&elm->lock);
 771                 return err;
 772         }
 773
 774         elm327_init(elm);
 775         spin_unlock_bh(&elm->lock);
 776
 777 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,10,0)
 778         elm->offload.mailbox_read = elmcan_mailbox_read;
 779         err = can_rx_offload_add_fifo(dev, &elm->offload, ELM327_NAPI_WEIGHT);
 780 #else
 781         err = can_rx_offload_add_manual(dev, &elm->offload, ELM327_NAPI_WEIGHT);
 782 #endif
 783         if (err) {
 784                 close_candev(dev);
 785                 return err;
 786         }
 787
 788         can_rx_offload_enable(&elm->offload);
 789
 790         can_led_event(dev, CAN_LED_EVENT_OPEN);
 791         elm->can.state = CAN_STATE_ERROR_ACTIVE;
 792         netif_start_queue(dev);
 793
 794         return 0;
 795 }
 796
 797 static int elmcan_netdev_close(struct net_device *dev)
 798 {
 799         struct elmcan *elm = netdev_priv(dev);
 800
 801         netif_stop_queue(dev);
 802
 803         spin_lock_bh(&elm->lock);
 804         if (elm->tty) {
 805                 /* Interrupt whatever we're doing right now */
 806                 elm327_send(elm, ELM327_DUMMY_STRING, 1);
 807
 808                 /* Clear the wakeup bit, as the netdev will be down and thus
 809                  * the wakeup handler won't clear it
 810                  */
 811                 clear_bit(TTY_DO_WRITE_WAKEUP, &elm->tty->flags);
 812
 813                 spin_unlock_bh(&elm->lock);
 814
 815                 flush_work(&elm->tx_work);
 816         } else {
 817                 spin_unlock_bh(&elm->lock);
 818         }
 819
 820         can_rx_offload_disable(&elm->offload);
 821         elm->can.state = CAN_STATE_STOPPED;
 822         can_rx_offload_del(&elm->offload);
 823         close_candev(dev);
 824         can_led_event(dev, CAN_LED_EVENT_STOP);
 825
 826         return 0;
 827 }
 828
 829 /* Send a can_frame to a TTY. */
 830 static netdev_tx_t elmcan_netdev_start_xmit(struct sk_buff *skb,
 831                                             struct net_device *dev)
 832 {
 833         struct elmcan *elm = netdev_priv(dev);
 834         struct can_frame *frame = (struct can_frame *)skb->data;
 835
 836         if (can_dropped_invalid_skb(dev, skb))
 837                 return NETDEV_TX_OK;
 838
 839         /* BHs are already disabled, so no spin_lock_bh().
 840          * See Documentation/networking/netdevices.txt
 841          */
 842         spin_lock(&elm->lock);
 843
 844         /* We shouldn't get here after a hardware fault:
 845          * can_bus_off() calls netif_carrier_off()
 846          */
 847         WARN_ON_ONCE(elm->hw_failure);
 848
 849         if (!elm->tty ||
 850             elm->hw_failure ||
 851             elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY) {
 852                 spin_unlock(&elm->lock);
 853                 goto out;
 854         }
 855
 856         netif_stop_queue(dev);
 857
 858         elm327_send_frame(elm, frame);
 859         spin_unlock(&elm->lock);
 860
 861         dev->stats.tx_packets++;
 862         dev->stats.tx_bytes += frame->len;
 863
 864         can_led_event(dev, CAN_LED_EVENT_TX);
 865
 866 out:
 867         kfree_skb(skb);
 868         return NETDEV_TX_OK;
 869 }
 870
 871 static const struct net_device_ops elmcan_netdev_ops = {
 872         .ndo_open       = elmcan_netdev_open,
 873         .ndo_stop       = elmcan_netdev_close,
 874         .ndo_start_xmit = elmcan_netdev_start_xmit,
 875         .ndo_change_mtu = can_change_mtu,
 876 };
 877
 878 static bool elmcan_is_valid_rx_char(char c)
 879 {
 880         return (isdigit(c) ||
 881                 isupper(c) ||
 882                 c == ELM327_DUMMY_CHAR ||
 883                 c == ELM327_READY_CHAR ||
 884                 c == '<' ||
 885                 c == 'a' ||
 886                 c == 'b' ||
 887                 c == 'v' ||
 888                 c == '.' ||
 889                 c == '?' ||
 890                 c == '\r' ||
 891                 c == ' ');
 892 }
 893
 894 /* Handle incoming ELM327 ASCII data.
 895  * This will not be re-entered while running, but other ldisc
 896  * functions may be called in parallel.
 897  */
 898 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,14,0)
 899 static void elmcan_ldisc_rx(struct tty_struct *tty,
 900                             const unsigned char *cp, char *fp, int count)
 901 #else
 902 static void elmcan_ldisc_rx(struct tty_struct *tty,
 903                             const unsigned char *cp, const char *fp, int count)
 904 #endif
 905 {
 906         struct elmcan *elm = (struct elmcan *)tty->disc_data;
 907
 908         spin_lock_bh(&elm->lock);
 909
 910         if (elm->hw_failure)
 911                 goto out;
 912
 913         while (count-- && elm->rxfill < ELM327_SIZE_RXBUF) {
 914                 if (fp && *fp++) {
 915                         netdev_err(elm->dev, "Error in received character stream. Check your wiring.");
 916
 917                         elm327_hw_failure(elm);
 918
 919                         goto out;
 920                 }
 921
 922                 /* Ignore NUL characters, which the PIC microcontroller may
 923                  * inadvertently insert due to a known hardware bug.
 924                  * See ELM327 documentation, which refers to a Microchip PIC
 925                  * bug description.
 926                  */
 927                 if (*cp != 0) {
 928                         /* Check for stray characters on the UART line.
 929                          * Likely caused by bad hardware.
 930                          */
 931                         if (!elmcan_is_valid_rx_char(*cp)) {
 932                                 netdev_err(elm->dev,
 933                                            "Received illegal character %02x.\n",
 934                                            *cp);
 935                                 elm327_hw_failure(elm);
 936
 937                                 goto out;
 938                         }
 939
 940                         elm->rxbuf[elm->rxfill++] = *cp;
 941                 }
 942
 943                 cp++;
 944         }
 945
 946         if (count >= 0) {
 947                 netdev_err(elm->dev, "Receive buffer overflowed. Bad chip or wiring?");
 948
 949                 elm327_hw_failure(elm);
 950
 951                 goto out;
 952         }
 953
 954         elm327_parse_rxbuf(elm);
 955
 956 out:
 957         spin_unlock_bh(&elm->lock);
 958 }
 959
 960 /* Write out remaining transmit buffer.
 961  * Scheduled when TTY is writable.
 962  */
 963 static void elmcan_ldisc_tx_worker(struct work_struct *work)
 964 {
 965         struct elmcan *elm = container_of(work, struct elmcan, tx_work);
 966         ssize_t written;
 967
 968         if (elm->hw_failure)
 969                 return;
 970
 971         spin_lock_bh(&elm->lock);
 972
 973         if (elm->txleft) {
 974                 written = elm->tty->ops->write(elm->tty, elm->txhead, elm->txleft);
 975                 if (written < 0) {
 976                         netdev_err(elm->dev,
 977                                    "Failed to write to tty %s.\n",
 978                                    elm->tty->name);
 979                         elm327_hw_failure(elm);
 980                         spin_unlock_bh(&elm->lock);
 981                         return;
 982                 } else {
 983                         elm->txleft -= written;
 984                         elm->txhead += written;
 985                 }
 986         }
 987
 988         if (!elm->txleft)  {
 989                 clear_bit(TTY_DO_WRITE_WAKEUP, &elm->tty->flags);
 990                 spin_unlock_bh(&elm->lock);
 991                 netif_wake_queue(elm->dev);
 992         } else {
 993                 spin_unlock_bh(&elm->lock);
 994         }
 995 }
 996
 997 /* Called by the driver when there's room for more data. */
 998 static void elmcan_ldisc_tx_wakeup(struct tty_struct *tty)
 999 {
1000         struct elmcan *elm = (struct elmcan *)tty->disc_data;
1001
1002         schedule_work(&elm->tx_work);
1003 }
1004
1005 /* ELM327 can only handle bitrates that are integer divisors of 500 kHz,
1006  * or 7/8 of that. Divisors are 1 to 64.
1007  * Currently we don't implement support for 7/8 rates.
1008  */
1009 static const u32 elmcan_bitrate_const[64] = {
1010          7812,  7936,  8064,  8196,  8333,  8474,  8620,  8771,
1011          8928,  9090,  9259,  9433,  9615,  9803, 10000, 10204,
1012         10416, 10638, 10869, 11111, 11363, 11627, 11904, 12195,
1013         12500, 12820, 13157, 13513, 13888, 14285, 14705, 15151,
1014         15625, 16129, 16666, 17241, 17857, 18518, 19230, 20000,
1015         20833, 21739, 22727, 23809, 25000, 26315, 27777, 29411,
1016         31250, 33333, 35714, 38461, 41666, 45454, 50000, 55555,
1017         62500, 71428, 83333, 100000, 125000, 166666, 250000, 500000
1018 };
1019
1020 /* Dummy needed to use bitrate_const */
1021 static int elmcan_do_set_bittiming(struct net_device *netdev)
1022 {
1023         return 0;
1024 }
1025
1026 static int elmcan_ldisc_open(struct tty_struct *tty)
1027 {
1028         struct net_device *dev;
1029         struct elmcan *elm;
1030         int err;
1031
1032         if (!capable(CAP_NET_ADMIN))
1033                 return -EPERM;
1034
1035         if (!tty->ops->write)
1036                 return -EOPNOTSUPP;
1037
1038         dev = alloc_candev(sizeof(struct elmcan), 0);
1039         if (!dev)
1040                 return -ENFILE;
1041         elm = netdev_priv(dev);
1042
1043         elm->txbuf = kmalloc(ELM327_SIZE_TXBUF, GFP_KERNEL);
1044         if (!elm->txbuf) {
1045                 err = -ENOMEM;
1046                 goto out_err;
1047         }
1048
1049         /* Configure TTY interface */
1050         tty->receive_room = 65536; /* We don't flow control */
1051         elm->txleft = 0; /* Clear TTY TX buffer */
1052         spin_lock_init(&elm->lock);
1053         INIT_WORK(&elm->tx_work, elmcan_ldisc_tx_worker);
1054
1055         /* Configure CAN metadata */
1056         elm->can.bitrate_const = elmcan_bitrate_const;
1057         elm->can.bitrate_const_cnt = ARRAY_SIZE(elmcan_bitrate_const);
1058         elm->can.do_set_bittiming = elmcan_do_set_bittiming;
1059         elm->can.ctrlmode_supported = CAN_CTRLMODE_LISTENONLY;
1060
1061         /* Configure netdev interface */
1062         elm->dev = dev;
1063         dev->netdev_ops = &elmcan_netdev_ops;
1064
1065         /* Mark ldisc channel as alive */
1066         elm->tty = tty;
1067         tty->disc_data = elm;
1068
1069         devm_can_led_init(elm->dev);
1070
1071         /* Let 'er rip */
1072         err = register_candev(elm->dev);
1073         if (err)
1074                 goto out_err;
1075
1076         netdev_info(elm->dev, "elmcan on %s.\n", tty->name);
1077
1078         return 0;
1079
1080 out_err:
1081         kfree(elm->txbuf);
1082         free_candev(elm->dev);
1083         return err;
1084 }
1085
1086 /* Close down an elmcan channel.
1087  * This means flushing out any pending queues, and then returning.
1088  * This call is serialized against other ldisc functions:
1089  * Once this is called, no other ldisc function of ours is entered.
1090  *
1091  * We also use this function for a hangup event.
1092  */
1093 static void elmcan_ldisc_close(struct tty_struct *tty)
1094 {
1095         struct elmcan *elm = (struct elmcan *)tty->disc_data;
1096
1097         /* unregister_netdev() calls .ndo_stop() so we don't have to. */
1098         unregister_candev(elm->dev);
1099
1100         /* Ensure that our worker won't be rescheduled */
1101         clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
1102         flush_work(&elm->tx_work);
1103
1104         /* Mark channel as dead */
1105         spin_lock_bh(&elm->lock);
1106         tty->disc_data = NULL;
1107         elm->tty = NULL;
1108         spin_unlock_bh(&elm->lock);
1109
1110         netdev_info(elm->dev, "elmcan off %s.\n", tty->name);
1111
1112         kfree(elm->txbuf);
1113         free_candev(elm->dev);
1114 }
1115
1116 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,16,0)
1117 static int elmcan_ldisc_hangup(struct tty_struct *tty)
1118 #else
1119 static void elmcan_ldisc_hangup(struct tty_struct *tty)
1120 #endif
1121 {
1122         elmcan_ldisc_close(tty);
1123 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,16,0)
1124         return 0;
1125 #endif
1126 }
1127
1128 static int elmcan_ldisc_ioctl(struct tty_struct *tty,
1129 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,17,0)
1130                               struct file *file,
1131 #endif
1132                               unsigned int cmd, unsigned long arg)
1133 {
1134         struct elmcan *elm = (struct elmcan *)tty->disc_data;
1135         unsigned int tmp;
1136
1137         switch (cmd) {
1138         case SIOCGIFNAME:
1139                 tmp = strnlen(elm->dev->name, IFNAMSIZ - 1) + 1;
1140                 if (copy_to_user((void __user *)arg, elm->dev->name, tmp))
1141                         return -EFAULT;
1142                 return 0;
1143
1144         case SIOCSIFHWADDR:
1145                 return -EINVAL;
1146
1147         default:
1148 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,16,0)
1149                 return tty_mode_ioctl(tty, file, cmd, arg);
1150 #else
1151                 return tty_mode_ioctl(tty, cmd, arg);
1152 #endif
1153         }
1154 }
1155
1156 static struct tty_ldisc_ops elmcan_ldisc = {
1157         .owner          = THIS_MODULE,
1158         .name           = "elmcan",
1159         .num            = N_DEVELOPMENT,
1160         .receive_buf    = elmcan_ldisc_rx,
1161         .write_wakeup   = elmcan_ldisc_tx_wakeup,
1162         .open           = elmcan_ldisc_open,
1163         .close          = elmcan_ldisc_close,
1164         .hangup         = elmcan_ldisc_hangup,
1165         .ioctl          = elmcan_ldisc_ioctl,
1166 };
1167
1168 static int __init elmcan_init(void)
1169 {
1170         int status;
1171
1172 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,14,0)
1173         status = tty_register_ldisc(N_DEVELOPMENT, &elmcan_ldisc);
1174 #else
1175         status = tty_register_ldisc(&elmcan_ldisc);
1176 #endif
1177         if (status)
1178                 pr_err("Can't register line discipline\n");
1179
1180         return status;
1181 }
1182
1183 static void __exit elmcan_exit(void)
1184 {
1185         /* This will only be called when all channels have been closed by
1186          * userspace - tty_ldisc.c takes care of the module's refcount.
1187          */
1188 #if LINUX_VERSION_CODE < KERNEL_VERSION(5,14,0)
1189         int status;
1190
1191         status = tty_unregister_ldisc(N_DEVELOPMENT);
1192         if (status)
1193                 pr_err("Can't unregister line discipline (error: %d)\n",
1194                        status);
1195 #else
1196         tty_unregister_ldisc(&elmcan_ldisc);
1197 #endif
1198 }
1199
1200 module_init(elmcan_init);
1201 module_exit(elmcan_exit);
1202
1203 MODULE_ALIAS_LDISC(N_DEVELOPMENT);
1204 MODULE_DESCRIPTION("ELM327 based CAN interface");
1205 MODULE_LICENSE("GPL");
1206 MODULE_AUTHOR("Max Staudt <max-linux@enpas.org>");