module/elmcan.c

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