X-Git-Url: https://git.enpas.org/?a=blobdiff_plain;f=module%2Felmcan.c;h=e8f163f81f1c38cc4056344c1203d82a92cce8e7;hb=c7d484d0a91da93ff32ff169cf125e3544373acd;hp=1c1465f1a570bed53d1c9ad6515315438db6c73a;hpb=5698ee2f40c0459c83136496bb0418f3e0f9271b;p=elmcan.git

diff --git a/module/elmcan.c b/module/elmcan.c
index 1c1465f..e8f163f 100644
--- a/module/elmcan.c
+++ b/module/elmcan.c
@@ -1,27 +1,20 @@
-/*
- * elmcan.c - ELM327 based CAN interface driver
- *            (tty line discipline)
- *
- * This file is derived from linux/drivers/net/can/slcan.c
- *
- * elmcan.c Author : Max Staudt <elmcan@enpas.org>
- * slcan.c Author  : Oliver Hartkopp <socketcan@hartkopp.net>
- * slip.c Authors  : Laurence Culhane <loz@holmes.demon.co.uk>
- *                   Fred N. van Kempen <waltje@uwalt.nl.mugnet.org>
- *
- * SPDX-License-Identifier: GPL-2.0
+// SPDX-License-Identifier: GPL-2.0
+/* ELM327 based CAN interface driver (tty line discipline)
  *
+ * This driver started as a derivative of linux/drivers/net/can/slcan.c
+ * and my thanks go to the original authors for their inspiration, even
+ * after almost none of their code is left.
  */
 
 #define pr_fmt(fmt) "[elmcan] " fmt
 
-
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 
 #include <linux/atomic.h>
 #include <linux/bitops.h>
+#include <linux/ctype.h>
 #include <linux/delay.h>
 #include <linux/errno.h>
 #include <linux/if_ether.h>
@@ -32,51 +25,69 @@
 #include <linux/spinlock.h>
 #include <linux/string.h>
 #include <linux/tty.h>
+#include <linux/tty_ldisc.h>
+#include <linux/version.h>
 #include <linux/workqueue.h>
 
+#include <uapi/linux/tty.h>
+
 #include <linux/can.h>
 #include <linux/can/dev.h>
 #include <linux/can/error.h>
 #include <linux/can/led.h>
+#include <linux/can/rx-offload.h>
 
-
-MODULE_ALIAS_LDISC(N_ELMCAN);
+MODULE_ALIAS_LDISC(N_DEVELOPMENT);
 MODULE_DESCRIPTION("ELM327 based CAN interface");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Max Staudt <max-linux@enpas.org>");
 
-/* Line discipline ID number */
-#ifndef N_ELMCAN
-#define N_ELMCAN 29
+/* Line discipline ID number.
+ * N_DEVELOPMENT will likely be defined from Linux 5.18 onwards:
+ * https://git.kernel.org/pub/scm/linux/kernel/git/gregkh/tty.git/commit/?h=tty-next&id=c2faf737abfb10f88f2d2612d573e9edc3c42c37
+ */
+#ifndef N_DEVELOPMENT
+#define N_DEVELOPMENT 29
 #endif
 
+/* Compatibility for Linux < 5.11 */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5,11,0)
+#define len can_dlc
+#endif
+
+#define ELM327_NAPI_WEIGHT 4
+
+#define ELM327_SIZE_RXBUF 256
+#define ELM327_SIZE_TXBUF 32
+
 #define ELM327_CAN_CONFIG_SEND_SFF           0x8000
 #define ELM327_CAN_CONFIG_VARIABLE_DLC       0x4000
 #define ELM327_CAN_CONFIG_RECV_BOTH_SFF_EFF  0x2000
 #define ELM327_CAN_CONFIG_BAUDRATE_MULT_8_7  0x1000
 
-#define ELM327_MAGIC_CHAR 'y'
-#define ELM327_MAGIC_STRING "y"
+#define ELM327_DUMMY_CHAR 'y'
+#define ELM327_DUMMY_STRING "y"
 #define ELM327_READY_CHAR '>'
 
-
 /* Bits in elm->cmds_todo */
-enum ELM_TODO {
-	ELM_TODO_CAN_DATA = 0,
-	ELM_TODO_CANID_11BIT,
-	ELM_TODO_CANID_29BIT_LOW,
-	ELM_TODO_CANID_29BIT_HIGH,
-	ELM_TODO_CAN_CONFIG,
-	ELM_TODO_RESPONSES,
-	ELM_TODO_SILENT_MONITOR,
-	ELM_TODO_INIT
+enum ELM327_TX_DO_BITS {
+	ELM327_TX_DO_CAN_DATA = 0,
+	ELM327_TX_DO_CANID_11BIT,
+	ELM327_TX_DO_CANID_29BIT_LOW,
+	ELM327_TX_DO_CANID_29BIT_HIGH,
+	ELM327_TX_DO_CAN_CONFIG_PART2,
+	ELM327_TX_DO_CAN_CONFIG,
+	ELM327_TX_DO_RESPONSES,
+	ELM327_TX_DO_SILENT_MONITOR,
+	ELM327_TX_DO_INIT
 };
 
-
 struct elmcan {
 	/* This must be the first member when using alloc_candev() */
 	struct can_priv can;
 
+	struct can_rx_offload offload;
+
 	/* TTY and netdev devices that we're bridging */
 	struct tty_struct	*tty;
 	struct net_device	*dev;
@@ -100,18 +111,18 @@ struct elmcan {
 
 	/* TTY TX helpers */
 	struct work_struct	tx_work;	/* Flushes TTY TX buffer   */
-	unsigned char		txbuf[32];
+	unsigned char		*txbuf;
 	unsigned char		*txhead;	/* Pointer to next TX byte */
 	int			txleft;		/* Bytes left to TX */
 
 	/* TTY RX helpers */
-	unsigned char rxbuf[256];
+	unsigned char rxbuf[ELM327_SIZE_RXBUF];
 	int rxfill;
 
 	/* State machine */
 	enum {
 		ELM_NOTINIT = 0,
-		ELM_GETMAGICCHAR,
+		ELM_GETDUMMYCHAR,
 		ELM_GETPROMPT,
 		ELM_RECEIVING,
 	} state;
@@ -119,8 +130,9 @@ struct elmcan {
 	int drop_next_line;
 
 	/* The CAN frame and config the ELM327 is sending/using,
-	 * or will send/use after finishing all cmds_todo */
-	struct can_frame can_frame;
+	 * or will send/use after finishing all cmds_todo
+	 */
+	struct can_frame can_frame_to_send;
 	unsigned short can_config;
 	unsigned long can_bitrate;
 	unsigned char can_bitrate_divisor;
@@ -131,7 +143,6 @@ struct elmcan {
 	unsigned long cmds_todo;
 };
 
-
 /* A lock for all tty->disc_data handled by this ldisc.
  * This is to prevent a case where tty->disc_data is set to NULL,
  * yet someone is still trying to dereference it.
@@ -139,24 +150,15 @@ struct elmcan {
  */
 static DEFINE_SPINLOCK(elmcan_discdata_lock);
 
-
 static inline void elm327_hw_failure(struct elmcan *elm);
 
-
-
- /************************************************************************
-  *		ELM327: Transmission				*
-  *								*
-  * (all functions assume elm->lock taken)			*
-  ************************************************************************/
-
+/* Assumes elm->lock taken. */
 static void elm327_send(struct elmcan *elm, const void *buf, size_t len)
 {
 	int actual;
 
-	if (elm->hw_failure) {
+	if (elm->hw_failure)
 		return;
-	}
 
 	memcpy(elm->txbuf, buf, len);
 
@@ -171,7 +173,9 @@ static void elm327_send(struct elmcan *elm, const void *buf, size_t len)
 	set_bit(TTY_DO_WRITE_WAKEUP, &elm->tty->flags);
 	actual = elm->tty->ops->write(elm->tty, elm->txbuf, len);
 	if (actual < 0) {
-		netdev_err(elm->dev, "Failed to write to tty %s.\n", elm->tty->name);
+		netdev_err(elm->dev,
+			   "Failed to write to tty %s.\n",
+			   elm->tty->name);
 		elm327_hw_failure(elm);
 		return;
 	}
@@ -180,69 +184,65 @@ static void elm327_send(struct elmcan *elm, const void *buf, size_t len)
 	elm->txhead = elm->txbuf + actual;
 }
 
-
-/*
- * Take the ELM327 out of almost any state and back into command mode
+/* Take the ELM327 out of almost any state and back into command mode.
+ * We send ELM327_DUMMY_CHAR which will either abort any running
+ * operation, or be echoed back to us in case we're already in command
+ * mode.
  *
  * Assumes elm->lock taken.
  */
 static void elm327_kick_into_cmd_mode(struct elmcan *elm)
 {
-	if (elm->state != ELM_GETMAGICCHAR && elm->state != ELM_GETPROMPT) {
-		elm327_send(elm, ELM327_MAGIC_STRING, 1);
+	if (elm->state != ELM_GETDUMMYCHAR && elm->state != ELM_GETPROMPT) {
+		elm327_send(elm, ELM327_DUMMY_STRING, 1);
 
-		elm->state = ELM_GETMAGICCHAR;
-		elm->rxfill = 0;
+		elm->state = ELM_GETDUMMYCHAR;
 	}
 }
 
-
-/*
- * Schedule a CAN frame, and any necessary config changes,
- * to be sent down the TTY.
+/* Schedule a CAN frame and necessary config changes to be sent to the TTY.
  *
  * Assumes elm->lock taken.
  */
 static void elm327_send_frame(struct elmcan *elm, struct can_frame *frame)
 {
 	/* Schedule any necessary changes in ELM327's CAN configuration */
-	if (elm->can_frame.can_id != frame->can_id) {
+	if (elm->can_frame_to_send.can_id != frame->can_id) {
 		/* Set the new CAN ID for transmission. */
-		if ((frame->can_id & CAN_EFF_FLAG) ^ (elm->can_frame.can_id & CAN_EFF_FLAG)) {
-			elm->can_config = (frame->can_id & CAN_EFF_FLAG ? 0 : ELM327_CAN_CONFIG_SEND_SFF)
+		if ((frame->can_id & CAN_EFF_FLAG)
+		    ^ (elm->can_frame_to_send.can_id & CAN_EFF_FLAG)) {
+			elm->can_config = (frame->can_id & CAN_EFF_FLAG
+						? 0
+						: ELM327_CAN_CONFIG_SEND_SFF)
 					| ELM327_CAN_CONFIG_VARIABLE_DLC
 					| ELM327_CAN_CONFIG_RECV_BOTH_SFF_EFF
 					| elm->can_bitrate_divisor;
 
-			set_bit(ELM_TODO_CAN_CONFIG, &elm->cmds_todo);
+			set_bit(ELM327_TX_DO_CAN_CONFIG, &elm->cmds_todo);
 		}
 
 		if (frame->can_id & CAN_EFF_FLAG) {
-			clear_bit(ELM_TODO_CANID_11BIT, &elm->cmds_todo);
-			set_bit(ELM_TODO_CANID_29BIT_LOW, &elm->cmds_todo);
-			set_bit(ELM_TODO_CANID_29BIT_HIGH, &elm->cmds_todo);
+			clear_bit(ELM327_TX_DO_CANID_11BIT, &elm->cmds_todo);
+			set_bit(ELM327_TX_DO_CANID_29BIT_LOW, &elm->cmds_todo);
+			set_bit(ELM327_TX_DO_CANID_29BIT_HIGH, &elm->cmds_todo);
 		} else {
-			set_bit(ELM_TODO_CANID_11BIT, &elm->cmds_todo);
-			clear_bit(ELM_TODO_CANID_29BIT_LOW, &elm->cmds_todo);
-			clear_bit(ELM_TODO_CANID_29BIT_HIGH, &elm->cmds_todo);
+			set_bit(ELM327_TX_DO_CANID_11BIT, &elm->cmds_todo);
+			clear_bit(ELM327_TX_DO_CANID_29BIT_LOW, &elm->cmds_todo);
+			clear_bit(ELM327_TX_DO_CANID_29BIT_HIGH, &elm->cmds_todo);
 		}
 	}
 
 	/* Schedule the CAN frame itself. */
-	elm->can_frame = *frame;
-	set_bit(ELM_TODO_CAN_DATA, &elm->cmds_todo);
+	elm->can_frame_to_send = *frame;
+	set_bit(ELM327_TX_DO_CAN_DATA, &elm->cmds_todo);
 
 	elm327_kick_into_cmd_mode(elm);
 }
 
-
-
- /************************************************************************
-  *		ELM327: Initialization sequence			*
-  *								*
-  * (assumes elm->lock taken)					*
-  ************************************************************************/
-
+/* ELM327 initialization sequence.
+ *
+ * Assumes elm->lock taken.
+ */
 static char *elm327_init_script[] = {
 	"AT WS\r",        /* v1.0: Warm Start */
 	"AT PP FF OFF\r", /* v1.0: All Programmable Parameters Off */
@@ -265,11 +265,10 @@ static char *elm327_init_script[] = {
 	NULL
 };
 
-
 static void elm327_init(struct elmcan *elm)
 {
 	elm->state = ELM_NOTINIT;
-	elm->can_frame.can_id = 0x7df;
+	elm->can_frame_to_send.can_id = 0x7df; /* ELM327 HW default */
 	elm->rxfill = 0;
 	elm->drop_next_line = 0;
 
@@ -285,149 +284,152 @@ static void elm327_init(struct elmcan *elm)
 
 	/* Configure ELM327 and then start monitoring */
 	elm->next_init_cmd = &elm327_init_script[0];
-	set_bit(ELM_TODO_INIT, &elm->cmds_todo);
-	set_bit(ELM_TODO_SILENT_MONITOR, &elm->cmds_todo);
-	set_bit(ELM_TODO_RESPONSES, &elm->cmds_todo);
-	set_bit(ELM_TODO_CAN_CONFIG, &elm->cmds_todo);
+	set_bit(ELM327_TX_DO_INIT, &elm->cmds_todo);
+	set_bit(ELM327_TX_DO_SILENT_MONITOR, &elm->cmds_todo);
+	set_bit(ELM327_TX_DO_RESPONSES, &elm->cmds_todo);
+	set_bit(ELM327_TX_DO_CAN_CONFIG, &elm->cmds_todo);
 
 	elm327_kick_into_cmd_mode(elm);
 }
 
+/* Assumes elm->lock taken. */
+static void elm327_feed_frame_to_netdev(struct elmcan *elm,
+					struct sk_buff *skb)
+{
+	if (!netif_running(elm->dev))
+		return;
 
+	/* Queue for NAPI pickup.
+	 * rx-offload will update stats and LEDs for us.
+	 */
+	if (can_rx_offload_queue_tail(&elm->offload, skb))
+		elm->dev->stats.rx_fifo_errors++;
 
- /************************************************************************
-  *		ELM327: Reception -> netdev glue		*
-  *								*
-  * (assumes elm->lock taken)					*
-  ************************************************************************/
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,15,0)
+	/* Wake NAPI */
+	can_rx_offload_irq_finish(&elm->offload);
+#endif
+}
 
-static void elm327_feed_frame_to_netdev(struct elmcan *elm, const struct can_frame *frame)
+/* Called when we're out of ideas and just want it all to end.
+ * Assumes elm->lock taken.
+ */
+static inline void elm327_hw_failure(struct elmcan *elm)
 {
-	struct can_frame *cf;
+	struct can_frame *frame;
 	struct sk_buff *skb;
 
-	if (!netif_running(elm->dev)) {
-		return;
-	}
-
-	skb = alloc_can_skb(elm->dev, &cf);
-
+	skb = alloc_can_err_skb(elm->dev, &frame);
 	if (!skb)
 		return;
 
-	memcpy(cf, frame, sizeof(struct can_frame));
+	frame->data[5] = 'R';
+	frame->data[6] = 'I';
+	frame->data[7] = 'P';
 
-	elm->dev->stats.rx_packets++;
-	elm->dev->stats.rx_bytes += frame->can_dlc;
-	netif_rx_ni(skb);
+	elm327_feed_frame_to_netdev(elm, skb);
 
-	can_led_event(elm->dev, CAN_LED_EVENT_RX);
-}
-
-
-
- /************************************************************************
-  *		ELM327: "Panic" handler				*
-  *								*
-  * (assumes elm->lock taken)					*
-  ************************************************************************/
-
-/* Called when we're out of ideas and just want it all to end. */
-static inline void elm327_hw_failure(struct elmcan *elm)
-{
-	struct can_frame frame;
-
-	memset(&frame, 0, sizeof(frame));
-	frame.can_id = CAN_ERR_FLAG;
-	frame.can_dlc = CAN_ERR_DLC;
-	frame.data[5] = 'R';
-	frame.data[6] = 'I';
-	frame.data[7] = 'P';
-	elm327_feed_frame_to_netdev(elm, &frame);
-
-	netdev_err(elm->dev, "ELM327 misbehaved. "
-			"Blocking further communication.\n");
+	netdev_err(elm->dev, "ELM327 misbehaved. Blocking further communication.\n");
 
 	elm->hw_failure = true;
 	can_bus_off(elm->dev);
 }
 
+/* Compare a buffer to a fixed string */
+static inline int _memstrcmp(const u8 *mem, const char *str)
+{
+	return memcmp(mem, str, strlen(str));
+}
 
+/* Compare buffer to string length, then compare buffer to fixed string.
+ * This ensures two things:
+ *  - It flags cases where the fixed string is only the start of the
+ *    buffer, rather than exactly all of it.
+ *  - It avoids byte comparisons in case the length doesn't match.
+ */
+static inline int _len_memstrcmp(const u8 *mem, size_t mem_len, const char *str)
+{
+	size_t str_len = strlen(str);
 
- /************************************************************************
-  *		ELM327: Reception parser			*
-  *								*
-  * (assumes elm->lock taken)					*
-  ************************************************************************/
+	return (mem_len != str_len) || memcmp(mem, str, str_len);
+}
 
-static void elm327_parse_error(struct elmcan *elm, int len)
+/* Assumes elm->lock taken. */
+static void elm327_parse_error(struct elmcan *elm, size_t len)
 {
-	struct can_frame frame;
+	struct can_frame *frame;
+	struct sk_buff *skb;
 
-	memset(&frame, 0, sizeof(frame));
-	frame.can_id = CAN_ERR_FLAG;
-	frame.can_dlc = CAN_ERR_DLC;
+	skb = alloc_can_err_skb(elm->dev, &frame);
+	if (!skb)
+		/* It's okay to return here:
+		 * The outer parsing loop will drop this UART buffer.
+		 */
+		return;
 
-	switch(len) {
-		case 17:
-			if (!memcmp(elm->rxbuf, "UNABLE TO CONNECT", 17)) {
-				netdev_err(elm->dev, "The ELM327 reported UNABLE TO CONNECT. Please check your setup.\n");
-			}
-			break;
-		case 11:
-			if (!memcmp(elm->rxbuf, "BUFFER FULL", 11)) {
-				/* This case will only happen if the last data
-				 * line was complete.
-				 * Otherwise, elm327_parse_frame() will emit the
-				 * error frame instead.
-				 */
-				frame.can_id |= CAN_ERR_CRTL;
-				frame.data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
-			}
-			break;
-		case 9:
-			if (!memcmp(elm->rxbuf, "BUS ERROR", 9)) {
-				frame.can_id |= CAN_ERR_BUSERROR;
-			}
-			if (!memcmp(elm->rxbuf, "CAN ERROR", 9)
-				|| !memcmp(elm->rxbuf, "<RX ERROR", 9)) {
-				frame.can_id |= CAN_ERR_PROT;
-			}
-			break;
-		case 8:
-			if (!memcmp(elm->rxbuf, "BUS BUSY", 8)) {
-				frame.can_id |= CAN_ERR_PROT;
-				frame.data[2] = CAN_ERR_PROT_OVERLOAD;
-			}
-			if (!memcmp(elm->rxbuf, "FB ERROR", 8)) {
-				frame.can_id |= CAN_ERR_PROT;
-				frame.data[2] = CAN_ERR_PROT_TX;
-			}
-			break;
-		case 5:
-			if (!memcmp(elm->rxbuf, "ERR", 3)) {
-				netdev_err(elm->dev, "The ELM327 reported an ERR%c%c. Please power it off and on again.\n",
-					elm->rxbuf[3], elm->rxbuf[4]);
-				frame.can_id |= CAN_ERR_CRTL;
-			}
-			break;
-		default:
-			/* Don't emit an error frame if we're unsure */
-			return;
+	/* Filter possible error messages based on length of RX'd line */
+	if (!_len_memstrcmp(elm->rxbuf, len, "UNABLE TO CONNECT")) {
+		netdev_err(elm->dev,
+			   "ELM327 reported UNABLE TO CONNECT. Please check your setup.\n");
+	} else if (!_len_memstrcmp(elm->rxbuf, len, "BUFFER FULL")) {
+		/* This will only happen if the last data line was complete.
+		 * Otherwise, elm327_parse_frame() will heuristically
+		 * emit this kind of error frame instead.
+		 */
+		frame->can_id |= CAN_ERR_CRTL;
+		frame->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
+	} else if (!_len_memstrcmp(elm->rxbuf, len, "BUS ERROR")) {
+		frame->can_id |= CAN_ERR_BUSERROR;
+	} else if (!_len_memstrcmp(elm->rxbuf, len, "CAN ERROR")) {
+		frame->can_id |= CAN_ERR_PROT;
+	} else if (!_len_memstrcmp(elm->rxbuf, len, "<RX ERROR")) {
+		frame->can_id |= CAN_ERR_PROT;
+	} else if (!_len_memstrcmp(elm->rxbuf, len, "BUS BUSY")) {
+		frame->can_id |= CAN_ERR_PROT;
+		frame->data[2] = CAN_ERR_PROT_OVERLOAD;
+	} else if (!_len_memstrcmp(elm->rxbuf, len, "FB ERROR")) {
+		frame->can_id |= CAN_ERR_PROT;
+		frame->data[2] = CAN_ERR_PROT_TX;
+	} else if (len == 5 && !_memstrcmp(elm->rxbuf, "ERR")) {
+		/* ERR is followed by two digits, hence line length 5 */
+		netdev_err(elm->dev, "ELM327 reported an ERR%c%c. Please power it off and on again.\n",
+			   elm->rxbuf[3], elm->rxbuf[4]);
+		frame->can_id |= CAN_ERR_CRTL;
+	} else {
+		/* Something else has happened.
+		 * Maybe garbage on the UART line.
+		 * Emit a generic error frame.
+		 */
 	}
 
-	elm327_feed_frame_to_netdev(elm, &frame);
+	elm327_feed_frame_to_netdev(elm, skb);
 }
 
-
-static int elm327_parse_frame(struct elmcan *elm, int len)
+/* Parse CAN frames coming as ASCII from ELM327.
+ * They can be of various formats:
+ *
+ * 29-bit ID (EFF):  12 34 56 78 D PL PL PL PL PL PL PL PL
+ * 11-bit ID (!EFF): 123 D PL PL PL PL PL PL PL PL
+ *
+ * where D = DLC, PL = payload byte
+ *
+ * Instead of a payload, RTR indicates a remote request.
+ *
+ * We will use the spaces and line length to guess the format.
+ *
+ * Assumes elm->lock taken.
+ */
+static int elm327_parse_frame(struct elmcan *elm, size_t len)
 {
-	struct can_frame frame;
+	struct can_frame *frame;
+	struct sk_buff *skb;
 	int hexlen;
 	int datastart;
 	int i;
 
-	memset(&frame, 0, sizeof(frame));
+	skb = alloc_can_skb(elm->dev, &frame);
+	if (!skb)
+		return -ENOMEM;
 
 	/* Find first non-hex and non-space character:
 	 *  - In the simplest case, there is none.
@@ -435,36 +437,50 @@ static int elm327_parse_frame(struct elmcan *elm, int len)
 	 *  - An error message may replace the end of the data line.
 	 */
 	for (hexlen = 0; hexlen <= len; hexlen++) {
-		if (hex_to_bin(elm->rxbuf[hexlen]) < 0
-		    && elm->rxbuf[hexlen] != ' ') {
+		if (hex_to_bin(elm->rxbuf[hexlen]) < 0 &&
+		    elm->rxbuf[hexlen] != ' ') {
 			break;
 		}
 	}
 
+	/* Sanity check whether the line is really a clean hexdump,
+	 * or terminated by an error message, or contains garbage.
+	 */
+	if (hexlen < len &&
+	    !isdigit(elm->rxbuf[hexlen]) &&
+	    !isupper(elm->rxbuf[hexlen]) &&
+	    '<' != elm->rxbuf[hexlen] &&
+	    ' ' != elm->rxbuf[hexlen]) {
+		/* The line is likely garbled anyway, so bail.
+		 * The main code will restart listening.
+		 */
+		return -ENODATA;
+	}
+
 	/* Use spaces in CAN ID to distinguish 29 or 11 bit address length.
 	 * No out-of-bounds access:
 	 * We use the fact that we can always read from elm->rxbuf.
 	 */
-	if (elm->rxbuf[2] == ' ' && elm->rxbuf[5] == ' '
-		&& elm->rxbuf[8] == ' ' && elm->rxbuf[11] == ' '
-		&& elm->rxbuf[13] == ' ') {
-		frame.can_id = CAN_EFF_FLAG;
+	if (elm->rxbuf[2] == ' ' && elm->rxbuf[5] == ' ' &&
+	    elm->rxbuf[8] == ' ' && elm->rxbuf[11] == ' ' &&
+	    elm->rxbuf[13] == ' ') {
+		frame->can_id = CAN_EFF_FLAG;
 		datastart = 14;
 	} else if (elm->rxbuf[3] == ' ' && elm->rxbuf[5] == ' ') {
-		frame.can_id = 0;
+		frame->can_id = 0;
 		datastart = 6;
 	} else {
 		/* This is not a well-formatted data line.
 		 * Assume it's an error message.
 		 */
-		return 1;
+		return -ENODATA;
 	}
 
 	if (hexlen < datastart) {
 		/* The line is too short to be a valid frame hex dump.
 		 * Something interrupted the hex dump or it is invalid.
 		 */
-		return 1;
+		return -ENODATA;
 	}
 
 	/* From here on all chars up to buf[hexlen] are hex or spaces,
@@ -472,186 +488,210 @@ static int elm327_parse_frame(struct elmcan *elm, int len)
 	 */
 
 	/* Read CAN data length */
-	frame.can_dlc = (hex_to_bin(elm->rxbuf[datastart - 2]) << 0);
+	frame->len = (hex_to_bin(elm->rxbuf[datastart - 2]) << 0);
 
 	/* Read CAN ID */
-	if (frame.can_id & CAN_EFF_FLAG) {
-		frame.can_id |= (hex_to_bin(elm->rxbuf[0]) << 28)
-			      | (hex_to_bin(elm->rxbuf[1]) << 24)
-			      | (hex_to_bin(elm->rxbuf[3]) << 20)
-			      | (hex_to_bin(elm->rxbuf[4]) << 16)
-			      | (hex_to_bin(elm->rxbuf[6]) << 12)
-			      | (hex_to_bin(elm->rxbuf[7]) << 8)
-			      | (hex_to_bin(elm->rxbuf[9]) << 4)
-			      | (hex_to_bin(elm->rxbuf[10]) << 0);
+	if (frame->can_id & CAN_EFF_FLAG) {
+		frame->can_id |= (hex_to_bin(elm->rxbuf[0]) << 28)
+			       | (hex_to_bin(elm->rxbuf[1]) << 24)
+			       | (hex_to_bin(elm->rxbuf[3]) << 20)
+			       | (hex_to_bin(elm->rxbuf[4]) << 16)
+			       | (hex_to_bin(elm->rxbuf[6]) << 12)
+			       | (hex_to_bin(elm->rxbuf[7]) << 8)
+			       | (hex_to_bin(elm->rxbuf[9]) << 4)
+			       | (hex_to_bin(elm->rxbuf[10]) << 0);
 	} else {
-		frame.can_id |= (hex_to_bin(elm->rxbuf[0]) << 8)
-			      | (hex_to_bin(elm->rxbuf[1]) << 4)
-			      | (hex_to_bin(elm->rxbuf[2]) << 0);
+		frame->can_id |= (hex_to_bin(elm->rxbuf[0]) << 8)
+			       | (hex_to_bin(elm->rxbuf[1]) << 4)
+			       | (hex_to_bin(elm->rxbuf[2]) << 0);
 	}
 
 	/* Check for RTR frame */
-	if (elm->rxfill >= hexlen + 3
-	    && elm->rxbuf[hexlen + 0] == 'R'
-	    && elm->rxbuf[hexlen + 1] == 'T'
-	    && elm->rxbuf[hexlen + 2] == 'R') {
-		frame.can_id |= CAN_RTR_FLAG;
+	if (elm->rxfill >= hexlen + 3 &&
+	    !_memstrcmp(&elm->rxbuf[hexlen], "RTR")) {
+		frame->can_id |= CAN_RTR_FLAG;
 	}
 
-	/* Is the line long enough to hold the advertised payload? */
-	if (!(frame.can_id & CAN_RTR_FLAG) && (hexlen < frame.can_dlc * 3 + datastart)) {
-		/* Incomplete frame. */
-
-		/* Probably the ELM327's RS232 TX buffer was full.
+	/* Is the line long enough to hold the advertised payload?
+	 * Note: RTR frames have a DLC, but no actual payload.
+	 */
+	if (!(frame->can_id & CAN_RTR_FLAG) &&
+	    (hexlen < frame->len * 3 + datastart)) {
+		/* Incomplete frame.
+		 * Probably the ELM327's RS232 TX buffer was full.
 		 * Emit an error frame and exit.
 		 */
-		frame.can_id = CAN_ERR_FLAG | CAN_ERR_CRTL;
-		frame.can_dlc = CAN_ERR_DLC;
-		frame.data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
-		elm327_feed_frame_to_netdev(elm, &frame);
+		frame->can_id = CAN_ERR_FLAG | CAN_ERR_CRTL;
+		frame->len = CAN_ERR_DLC;
+		frame->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
+		elm327_feed_frame_to_netdev(elm, skb);
 
 		/* Signal failure to parse.
 		 * The line will be re-parsed as an error line, which will fail.
 		 * However, this will correctly drop the state machine back into
 		 * command mode.
 		 */
-		return 2;
+		return -ENODATA;
 	}
 
 	/* Parse the data nibbles. */
-	for (i = 0; i < frame.can_dlc; i++) {
-		frame.data[i] = (hex_to_bin(elm->rxbuf[datastart+3*i]) << 4)
-                                | (hex_to_bin(elm->rxbuf[datastart+3*i+1]) << 0);
+	for (i = 0; i < frame->len; i++) {
+		frame->data[i] = (hex_to_bin(elm->rxbuf[datastart + 3*i]) << 4)
+			       | (hex_to_bin(elm->rxbuf[datastart + 3*i + 1]));
 	}
 
 	/* Feed the frame to the network layer. */
-	elm327_feed_frame_to_netdev(elm, &frame);
+	elm327_feed_frame_to_netdev(elm, skb);
 
 	return 0;
 }
 
-
-static void elm327_parse_line(struct elmcan *elm, int len)
+/* Assumes elm->lock taken. */
+static void elm327_parse_line(struct elmcan *elm, size_t len)
 {
 	/* Skip empty lines */
-	if (!len) {
+	if (!len)
 		return;
-	}
 
 	/* Skip echo lines */
 	if (elm->drop_next_line) {
 		elm->drop_next_line = 0;
 		return;
-	} else if (elm->rxbuf[0] == 'A' && elm->rxbuf[1] == 'T') {
+	} else if (!_memstrcmp(elm->rxbuf, "AT")) {
 		return;
 	}
 
 	/* Regular parsing */
-	switch(elm->state) {
-		case ELM_RECEIVING:
-			if (elm327_parse_frame(elm, len)) {
-				/* Parse an error line. */
-				elm327_parse_error(elm, len);
+	switch (elm->state) {
+	case ELM_RECEIVING:
+		if (elm327_parse_frame(elm, len)) {
+			/* Parse an error line. */
+			elm327_parse_error(elm, len);
 
-				/* After the error line, we expect a prompt. */
-				elm->state = ELM_GETPROMPT;
-			}
-			break;
-		default:
-			break;
+			/* Start afresh. */
+			elm327_kick_into_cmd_mode(elm);
+		}
+		break;
+	default:
+		break;
 	}
 }
 
-
+/* Assumes elm->lock taken. */
 static void elm327_handle_prompt(struct elmcan *elm)
 {
-	if (elm->cmds_todo) {
-		struct can_frame *frame = &elm->can_frame;
-		char txbuf[20];
-
-		if (test_bit(ELM_TODO_INIT, &elm->cmds_todo)) {
-			elm327_send(elm, *elm->next_init_cmd, strlen(*elm->next_init_cmd));
-			elm->next_init_cmd++;
-			if (!(*elm->next_init_cmd)) {
-				clear_bit(ELM_TODO_INIT, &elm->cmds_todo);
-				netdev_info(elm->dev, "Initialization finished.\n");
-			}
+	struct can_frame *frame = &elm->can_frame_to_send;
+	char local_txbuf[20];
+
+	if (!elm->cmds_todo) {
+		/* Enter CAN monitor mode */
+		elm327_send(elm, "ATMA\r", 5);
+		elm->state = ELM_RECEIVING;
+
+		return;
+	}
+
+	/* Reconfigure ELM327 step by step as indicated by elm->cmds_todo */
+	if (test_bit(ELM327_TX_DO_INIT, &elm->cmds_todo)) {
+		strcpy(local_txbuf, *elm->next_init_cmd);
+
+		elm->next_init_cmd++;
+		if (!(*elm->next_init_cmd)) {
+			clear_bit(ELM327_TX_DO_INIT, &elm->cmds_todo);
+			/* Init finished. */
+		}
 
-			/* Some chips are unreliable and need extra time after
-			 * init commands, as seen with a clone.
-			 * So let's do a dummy get-cmd-prompt dance.
+	} else if (test_and_clear_bit(ELM327_TX_DO_SILENT_MONITOR, &elm->cmds_todo)) {
+		sprintf(local_txbuf, "ATCSM%i\r",
+			!(!(elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY)));
+
+	} else if (test_and_clear_bit(ELM327_TX_DO_RESPONSES, &elm->cmds_todo)) {
+		sprintf(local_txbuf, "ATR%i\r",
+			!(elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY));
+
+	} else if (test_and_clear_bit(ELM327_TX_DO_CAN_CONFIG, &elm->cmds_todo)) {
+		sprintf(local_txbuf, "ATPC\r");
+		set_bit(ELM327_TX_DO_CAN_CONFIG_PART2, &elm->cmds_todo);
+
+	} else if (test_and_clear_bit(ELM327_TX_DO_CAN_CONFIG_PART2, &elm->cmds_todo)) {
+		sprintf(local_txbuf, "ATPB%04X\r",
+			elm->can_config);
+
+	} else if (test_and_clear_bit(ELM327_TX_DO_CANID_29BIT_HIGH, &elm->cmds_todo)) {
+		sprintf(local_txbuf, "ATCP%02X\r",
+			(frame->can_id & CAN_EFF_MASK) >> 24);
+
+	} else if (test_and_clear_bit(ELM327_TX_DO_CANID_29BIT_LOW, &elm->cmds_todo)) {
+		sprintf(local_txbuf, "ATSH%06X\r",
+			frame->can_id & CAN_EFF_MASK & ((1 << 24) - 1));
+
+	} else if (test_and_clear_bit(ELM327_TX_DO_CANID_11BIT, &elm->cmds_todo)) {
+		sprintf(local_txbuf, "ATSH%03X\r",
+			frame->can_id & CAN_SFF_MASK);
+
+	} else if (test_and_clear_bit(ELM327_TX_DO_CAN_DATA, &elm->cmds_todo)) {
+		if (frame->can_id & CAN_RTR_FLAG) {
+			/* Send an RTR frame. Their DLC is fixed.
+			 * Some chips don't send them at all.
 			 */
-			elm->state = ELM_NOTINIT;
-			elm327_kick_into_cmd_mode(elm);
+			sprintf(local_txbuf, "ATRTR\r");
+		} else {
+			/* Send a regular CAN data frame */
+			int i;
 
-			return;
-		} else if (test_and_clear_bit(ELM_TODO_SILENT_MONITOR, &elm->cmds_todo)) {
-			snprintf(txbuf, sizeof(txbuf), "ATCSM%i\r", !(!(elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY)));
-		} else if (test_and_clear_bit(ELM_TODO_RESPONSES, &elm->cmds_todo)) {
-			snprintf(txbuf, sizeof(txbuf), "ATR%i\r", !(elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY));
-		} else if (test_and_clear_bit(ELM_TODO_CAN_CONFIG, &elm->cmds_todo)) {
-			snprintf(txbuf, sizeof(txbuf), "ATPB%04X\r", elm->can_config);
-		} else if (test_and_clear_bit(ELM_TODO_CANID_29BIT_HIGH, &elm->cmds_todo)) {
-			snprintf(txbuf, sizeof(txbuf), "ATCP%02X\r", (frame->can_id & CAN_EFF_MASK) >> 24);
-		} else if (test_and_clear_bit(ELM_TODO_CANID_29BIT_LOW, &elm->cmds_todo)) {
-			snprintf(txbuf, sizeof(txbuf), "ATSH%06X\r", frame->can_id & CAN_EFF_MASK & ((1 << 24) - 1));
-		} else if (test_and_clear_bit(ELM_TODO_CANID_11BIT, &elm->cmds_todo)) {
-			snprintf(txbuf, sizeof(txbuf), "ATSH%03X\r", frame->can_id & CAN_SFF_MASK);
-		} else if (test_and_clear_bit(ELM_TODO_CAN_DATA, &elm->cmds_todo)) {
-			if (frame->can_id & CAN_RTR_FLAG) {
-				snprintf(txbuf, sizeof(txbuf), "ATRTR\r");
-			} else {
-				int i;
-
-				for (i = 0; i < frame->can_dlc; i++) {
-					sprintf(&txbuf[2*i], "%02X", frame->data[i]);
-				}
-
-				sprintf(&txbuf[2*i], "\r");
+			for (i = 0; i < frame->len; i++) {
+				sprintf(&local_txbuf[2 * i], "%02X",
+					frame->data[i]);
 			}
 
-			elm->drop_next_line = 1;
-			elm->state = ELM_RECEIVING;
+			sprintf(&local_txbuf[2 * i], "\r");
 		}
 
-		elm327_send(elm, txbuf, strlen(txbuf));
-	} else {
-		/* Enter CAN monitor mode */
-		elm327_send(elm, "ATMA\r", 5);
+		elm->drop_next_line = 1;
 		elm->state = ELM_RECEIVING;
 	}
+
+	elm327_send(elm, local_txbuf, strlen(local_txbuf));
 }
 
+static bool elm327_is_ready_char(char c)
+{
+	/* Bits 0xc0 are sometimes set (randomly), hence the mask.
+	 * Probably bad hardware.
+	 */
+	return (c & 0x3f) == ELM327_READY_CHAR;
+}
 
-static void elm327_drop_bytes(struct elmcan *elm, int i)
+/* Assumes elm->lock taken. */
+static void elm327_drop_bytes(struct elmcan *elm, size_t i)
 {
-	memmove(&elm->rxbuf[0], &elm->rxbuf[i], sizeof(elm->rxbuf) - i);
+	memmove(&elm->rxbuf[0], &elm->rxbuf[i], ELM327_SIZE_RXBUF - i);
 	elm->rxfill -= i;
 }
 
-
+/* Assumes elm->lock taken. */
 static void elm327_parse_rxbuf(struct elmcan *elm)
 {
-	int len;
+	size_t len;
 
 	switch (elm->state) {
 	case ELM_NOTINIT:
 		elm->rxfill = 0;
-		return;
+		break;
 
-	case ELM_GETMAGICCHAR:
+	case ELM_GETDUMMYCHAR:
 	{
 		/* Wait for 'y' or '>' */
 		int i;
 
 		for (i = 0; i < elm->rxfill; i++) {
-			if (elm->rxbuf[i] == ELM327_MAGIC_CHAR) {
+			if (elm->rxbuf[i] == ELM327_DUMMY_CHAR) {
 				elm327_send(elm, "\r", 1);
 				elm->state = ELM_GETPROMPT;
 				i++;
 				break;
-			} else if (elm->rxbuf[i] == ELM327_READY_CHAR) {
-				elm327_send(elm, ELM327_MAGIC_STRING, 1);
+			} else if (elm327_is_ready_char(elm->rxbuf[i])) {
+				elm327_send(elm, ELM327_DUMMY_STRING, 1);
 				i++;
 				break;
 			}
@@ -659,17 +699,16 @@ static void elm327_parse_rxbuf(struct elmcan *elm)
 
 		elm327_drop_bytes(elm, i);
 
-		return;
+		break;
 	}
 
 	case ELM_GETPROMPT:
 		/* Wait for '>' */
-		if (elm->rxbuf[elm->rxfill - 1] == ELM327_READY_CHAR) {
+		if (elm327_is_ready_char(elm->rxbuf[elm->rxfill - 1]))
 			elm327_handle_prompt(elm);
-		}
 
 		elm->rxfill = 0;
-		return;
+		break;
 
 	case ELM_RECEIVING:
 		/* Find <CR> delimiting feedback lines. */
@@ -679,16 +718,16 @@ static void elm327_parse_rxbuf(struct elmcan *elm)
 			/* empty loop */
 		}
 
-		if (len == sizeof(elm->rxbuf)) {
+		if (len == ELM327_SIZE_RXBUF) {
 			/* Line exceeds buffer. It's probably all garbage.
 			 * Did we even connect at the right baud rate?
 			 */
-			netdev_err(elm->dev, "RX buffer overflow. Faulty ELM327 connected?\n");
+			netdev_err(elm->dev,
+				   "RX buffer overflow. Faulty ELM327 or UART?\n");
 			elm327_hw_failure(elm);
-			return;
+			break;
 		} else if (len == elm->rxfill) {
-			if (elm->state == ELM_RECEIVING
-				&& elm->rxbuf[elm->rxfill - 1] == ELM327_READY_CHAR) {
+			if (elm327_is_ready_char(elm->rxbuf[elm->rxfill - 1])) {
 				/* The ELM327's AT ST response timeout ran out,
 				 * so we got a prompt.
 				 * Clear RX buffer and restart listening.
@@ -696,39 +735,37 @@ static void elm327_parse_rxbuf(struct elmcan *elm)
 				elm->rxfill = 0;
 
 				elm327_handle_prompt(elm);
-				return;
-			} else {
-				/* We haven't received a full line yet.
-				 * Wait for more data.
-				 */
-				return;
+				break;
 			}
+
+			/* No <CR> found - we haven't received a full line yet.
+			 * Wait for more data.
+			 */
+			break;
 		}
 
 		/* We have a full line to parse. */
 		elm327_parse_line(elm, len);
 
 		/* Remove parsed data from RX buffer. */
-		elm327_drop_bytes(elm, len+1);
+		elm327_drop_bytes(elm, len + 1);
 
 		/* More data to parse? */
-		if (elm->rxfill) {
+		if (elm->rxfill)
 			elm327_parse_rxbuf(elm);
-		}
 	}
 }
 
+/* Dummy needed to use can_rx_offload */
+static struct sk_buff *elmcan_mailbox_read(struct can_rx_offload *offload,
+					   unsigned int n, u32 *timestamp,
+					   bool drop)
+{
+	WARN_ON_ONCE(1); /* This function is a dummy, so don't call it! */
 
+	return ERR_PTR(-ENOBUFS);
+}
 
-
-
- /************************************************************************
-  *		netdev						*
-  *								*
-  * (takes elm->lock)						*
-  ************************************************************************/
-
-/* Netdevice DOWN -> UP routine */
 static int elmcan_netdev_open(struct net_device *dev)
 {
 	struct elmcan *elm = netdev_priv(dev);
@@ -736,13 +773,12 @@ static int elmcan_netdev_open(struct net_device *dev)
 
 	spin_lock_bh(&elm->lock);
 	if (elm->hw_failure) {
-		netdev_err(elm->dev, "Refusing to open interface after "
-				"a hardware fault has been detected.\n");
+		netdev_err(elm->dev, "Refusing to open interface after a hardware fault has been detected.\n");
 		spin_unlock_bh(&elm->lock);
 		return -EIO;
 	}
 
-	if (elm->tty == NULL) {
+	if (!elm->tty) {
 		spin_unlock_bh(&elm->lock);
 		return -ENODEV;
 	}
@@ -754,10 +790,18 @@ static int elmcan_netdev_open(struct net_device *dev)
 		return err;
 	}
 
-	/* Initialize the ELM327 */
 	elm327_init(elm);
 	spin_unlock_bh(&elm->lock);
 
+	elm->offload.mailbox_read = elmcan_mailbox_read;
+	err = can_rx_offload_add_fifo(dev, &elm->offload, ELM327_NAPI_WEIGHT);
+	if (err) {
+		close_candev(dev);
+		return err;
+	}
+
+	can_rx_offload_enable(&elm->offload);
+
 	can_led_event(dev, CAN_LED_EVENT_OPEN);
 	elm->can.state = CAN_STATE_ERROR_ACTIVE;
 	netif_start_queue(dev);
@@ -765,17 +809,16 @@ static int elmcan_netdev_open(struct net_device *dev)
 	return 0;
 }
 
-/* Netdevice UP -> DOWN routine */
 static int elmcan_netdev_close(struct net_device *dev)
 {
 	struct elmcan *elm = netdev_priv(dev);
 
+	netif_stop_queue(dev);
+
 	spin_lock_bh(&elm->lock);
 	if (elm->tty) {
-		/* TTY discipline is running. */
-
 		/* Interrupt whatever we're doing right now */
-		elm327_send(elm, ELM327_MAGIC_STRING, 1);
+		elm327_send(elm, ELM327_DUMMY_STRING, 1);
 
 		/* Clear the wakeup bit, as the netdev will be down and thus
 		 * the wakeup handler won't clear it
@@ -789,22 +832,21 @@ static int elmcan_netdev_close(struct net_device *dev)
 		spin_unlock_bh(&elm->lock);
 	}
 
+	can_rx_offload_disable(&elm->offload);
 	elm->can.state = CAN_STATE_STOPPED;
-	netif_stop_queue(dev);
+	can_rx_offload_del(&elm->offload);
 	close_candev(dev);
 	can_led_event(dev, CAN_LED_EVENT_STOP);
 
 	return 0;
 }
 
-/* Send a can_frame to a TTY queue. */
-static netdev_tx_t elmcan_netdev_start_xmit(struct sk_buff *skb, struct net_device *dev)
+/* Send a can_frame to a TTY. */
+static netdev_tx_t elmcan_netdev_start_xmit(struct sk_buff *skb,
+					    struct net_device *dev)
 {
 	struct elmcan *elm = netdev_priv(dev);
-	struct can_frame *frame = (struct can_frame *) skb->data;
-
-	if (skb->len != sizeof(struct can_frame))
-		goto out;
+	struct can_frame *frame = (struct can_frame *)skb->data;
 
 	if (!netif_running(dev))  {
 		netdev_warn(elm->dev, "xmit: iface is down.\n");
@@ -819,10 +861,11 @@ static netdev_tx_t elmcan_netdev_start_xmit(struct sk_buff *skb, struct net_devi
 	/* We shouldn't get here after a hardware fault:
 	 * can_bus_off() calls netif_carrier_off()
 	 */
-	BUG_ON(elm->hw_failure);
+	WARN_ON_ONCE(elm->hw_failure);
 
-	if (elm->tty == NULL
-		|| elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY) {
+	if (!elm->tty ||
+	    elm->hw_failure ||
+	    elm->can.ctrlmode & CAN_CTRLMODE_LISTENONLY) {
 		spin_unlock(&elm->lock);
 		goto out;
 	}
@@ -833,7 +876,7 @@ static netdev_tx_t elmcan_netdev_start_xmit(struct sk_buff *skb, struct net_devi
 	spin_unlock(&elm->lock);
 
 	dev->stats.tx_packets++;
-	dev->stats.tx_bytes += frame->can_dlc;
+	dev->stats.tx_bytes += frame->len;
 
 	can_led_event(dev, CAN_LED_EVENT_TX);
 
@@ -842,43 +885,26 @@ out:
 	return NETDEV_TX_OK;
 }
 
-static int elmcan_netdev_change_mtu(struct net_device *dev, int new_mtu)
-{
-	return -EINVAL;
-}
-
 static const struct net_device_ops elmcan_netdev_ops = {
 	.ndo_open	= elmcan_netdev_open,
 	.ndo_stop	= elmcan_netdev_close,
 	.ndo_start_xmit	= elmcan_netdev_start_xmit,
-	.ndo_change_mtu	= elmcan_netdev_change_mtu,
+	.ndo_change_mtu	= can_change_mtu,
 };
 
-
-
-
-
- /************************************************************************
-  *		Line discipline					*
-  *								*
-  * (takes elm->lock)						*
-  ************************************************************************/
-
-/*
- * Get a reference to our struct, taking into account locks/refcounts.
+/* Get a reference to our struct, taking into account locks/refcounts.
  * This is to ensure ordering in case we are shutting down, and to ensure
- * there is a refcount at all (because tty->disc_data may be NULL).
+ * there is a refcount at all (otherwise tty->disc_data may be freed and
+ * before we increment the refcount).
+ * Use this for anything that can race against elmcan_ldisc_close().
  */
-static struct elmcan* get_elm(struct tty_struct *tty)
+static struct elmcan *get_elm(struct tty_struct *tty)
 {
 	struct elmcan *elm;
 	bool got_ref;
 
-	/* Lock all elmcan TTYs, so tty->disc_data can't become NULL
-	 * the moment before we increase the reference counter.
-	 */
 	spin_lock_bh(&elmcan_discdata_lock);
-	elm = (struct elmcan *) tty->disc_data;
+	elm = (struct elmcan *)tty->disc_data;
 
 	if (!elm) {
 		spin_unlock_bh(&elmcan_discdata_lock);
@@ -888,9 +914,8 @@ static struct elmcan* get_elm(struct tty_struct *tty)
 	got_ref = atomic_inc_not_zero(&elm->refcount);
 	spin_unlock_bh(&elmcan_discdata_lock);
 
-	if (!got_ref) {
+	if (!got_ref)
 		return NULL;
-	}
 
 	return elm;
 }
@@ -900,68 +925,99 @@ static void put_elm(struct elmcan *elm)
 	atomic_dec(&elm->refcount);
 }
 
+static bool elmcan_is_valid_rx_char(char c)
+{
+	return (isdigit(c) ||
+		isupper(c) ||
+		c == ELM327_DUMMY_CHAR ||
+		c == ELM327_READY_CHAR ||
+		c == '<' ||
+		c == 'a' ||
+		c == 'b' ||
+		c == 'v' ||
+		c == '.' ||
+		c == '?' ||
+		c == '\r' ||
+		c == ' ');
+}
 
-
-/*
- * Handle the 'receiver data ready' interrupt.
- * This function is called by the 'tty_io' module in the kernel when
- * a block of ELM327 CAN data has been received, which can now be parsed
- * and sent on to some IP layer for further processing. This will not
- * be re-entered while running but other ldisc functions may be called
- * in parallel
+/* Handle incoming ELM327 ASCII data.
+ * This will not be re-entered while running, but other ldisc
+ * functions may be called in parallel.
  */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5,14,0)
+static void elmcan_ldisc_rx(struct tty_struct *tty,
+			    const unsigned char *cp, char *fp, int count)
+#else
 static void elmcan_ldisc_rx(struct tty_struct *tty,
-			const unsigned char *cp, char *fp, int count)
+			    const unsigned char *cp, const char *fp, int count)
+#endif
 {
 	struct elmcan *elm = get_elm(tty);
 
 	if (!elm)
 		return;
 
-	/* Read the characters out of the buffer */
-	while (count-- && elm->rxfill < sizeof(elm->rxbuf)) {
+	spin_lock_bh(&elm->lock);
+
+	if (elm->hw_failure)
+		goto out;
+
+	while (count-- && elm->rxfill < ELM327_SIZE_RXBUF) {
 		if (fp && *fp++) {
 			netdev_err(elm->dev, "Error in received character stream. Check your wiring.");
 
-			spin_lock_bh(&elm->lock);
 			elm327_hw_failure(elm);
-			spin_unlock_bh(&elm->lock);
 
-			put_elm(elm);
-			return;
+			goto out;
 		}
+
+		/* Ignore NUL characters, which the PIC microcontroller may
+		 * inadvertently insert due to a known hardware bug.
+		 * See ELM327 documentation, which refers to a Microchip PIC
+		 * bug description.
+		 */
 		if (*cp != 0) {
+			/* Check for stray characters on the UART line.
+			 * Likely caused by bad hardware.
+			 */
+			if (!elmcan_is_valid_rx_char(*cp)) {
+				netdev_err(elm->dev,
+					   "Received illegal character %02x.\n",
+					   *cp);
+				elm327_hw_failure(elm);
+
+				goto out;
+			}
+
 			elm->rxbuf[elm->rxfill++] = *cp;
 		}
+
 		cp++;
 	}
 
 	if (count >= 0) {
 		netdev_err(elm->dev, "Receive buffer overflowed. Bad chip or wiring?");
 
-		spin_lock_bh(&elm->lock);
 		elm327_hw_failure(elm);
-		spin_unlock_bh(&elm->lock);
 
-		put_elm(elm);
-		return;
+		goto out;
 	}
 
-	spin_lock_bh(&elm->lock);
 	elm327_parse_rxbuf(elm);
-	spin_unlock_bh(&elm->lock);
 
+out:
+	spin_unlock_bh(&elm->lock);
 	put_elm(elm);
 }
 
-/*
- * Write out remaining transmit buffer.
+/* Write out remaining transmit buffer.
  * Scheduled when TTY is writable.
  */
 static void elmcan_ldisc_tx_worker(struct work_struct *work)
 {
 	/* No need to use get_elm() here, as we'll always flush workers
-	 * befory destroying the elmcan object.
+	 * before destroying the elmcan object.
 	 */
 	struct elmcan *elm = container_of(work, struct elmcan, tx_work);
 	ssize_t actual;
@@ -979,7 +1035,7 @@ static void elmcan_ldisc_tx_worker(struct work_struct *work)
 
 	if (elm->txleft <= 0)  {
 		/* Our TTY write buffer is empty:
-		 * We can start transmission of another packet
+		 * Allow netdev to hand us another packet
 		 */
 		clear_bit(TTY_DO_WRITE_WAKEUP, &elm->tty->flags);
 		spin_unlock_bh(&elm->lock);
@@ -989,7 +1045,9 @@ static void elmcan_ldisc_tx_worker(struct work_struct *work)
 
 	actual = elm->tty->ops->write(elm->tty, elm->txhead, elm->txleft);
 	if (actual < 0) {
-		netdev_err(elm->dev, "Failed to write to tty %s.\n", elm->tty->name);
+		netdev_err(elm->dev,
+			   "Failed to write to tty %s.\n",
+			   elm->tty->name);
 		elm327_hw_failure(elm);
 		spin_unlock_bh(&elm->lock);
 		return;
@@ -1000,11 +1058,7 @@ static void elmcan_ldisc_tx_worker(struct work_struct *work)
 	spin_unlock_bh(&elm->lock);
 }
 
-
-/*
- * Called by the driver when there's room for more data.
- * Schedule the transmit.
- */
+/* Called by the driver when there's room for more data. */
 static void elmcan_ldisc_tx_wakeup(struct tty_struct *tty)
 {
 	struct elmcan *elm = get_elm(tty);
@@ -1017,28 +1071,27 @@ static void elmcan_ldisc_tx_wakeup(struct tty_struct *tty)
 	put_elm(elm);
 }
 
-
-
-/* Some fake bit timings to allow bitrate setting */
-static const struct can_bittiming_const elmcan_bittiming_const = {
-	.name = "elmcan",
-	.tseg1_min = 1,
-	.tseg1_max = 1,
-	.tseg2_min = 0,
-	.tseg2_max = 0,
-	.sjw_max = 1,
-	.brp_min = 1,
-	.brp_max = 500,
-	.brp_inc = 1,
+/* ELM327 can only handle bitrates that are integer divisors of 500 kHz,
+ * or 7/8 of that. Divisors are 1 to 64.
+ * Currently we don't implement support for 7/8 rates.
+ */
+static const u32 elmcan_bitrate_const[64] = {
+	 7812,  7936,  8064,  8196,  8333,  8474,  8620,  8771,
+	 8928,  9090,  9259,  9433,  9615,  9803, 10000, 10204,
+	10416, 10638, 10869, 11111, 11363, 11627, 11904, 12195,
+	12500, 12820, 13157, 13513, 13888, 14285, 14705, 15151,
+	15625, 16129, 16666, 17241, 17857, 18518, 19230, 20000,
+	20833, 21739, 22727, 23809, 25000, 26315, 27777, 29411,
+	31250, 33333, 35714, 38461, 41666, 45454, 50000, 55555,
+	62500, 71428, 83333, 100000, 125000, 166666, 250000, 500000
 };
 
-/*
- * Open the high-level part of the elmcan channel.
- * This function is called by the TTY module when the
- * elmcan line discipline is called for.
- *
- * Called in process context serialized from other ldisc calls.
- */
+/* Dummy needed to use bitrate_const */
+static int elmcan_do_set_bittiming(struct net_device *netdev)
+{
+	return 0;
+}
+
 static int elmcan_ldisc_open(struct tty_struct *tty)
 {
 	struct net_device *dev;
@@ -1051,13 +1104,17 @@ static int elmcan_ldisc_open(struct tty_struct *tty)
 	if (!tty->ops->write)
 		return -EOPNOTSUPP;
 
-
-	/* OK.  Find a free elmcan channel to use. */
 	dev = alloc_candev(sizeof(struct elmcan), 0);
 	if (!dev)
 		return -ENFILE;
 	elm = netdev_priv(dev);
 
+	elm->txbuf = kmalloc(ELM327_SIZE_TXBUF, GFP_KERNEL);
+	if (!elm->txbuf) {
+		err = -ENOMEM;
+		goto out_err;
+	}
+
 	/* Configure TTY interface */
 	tty->receive_room = 65536; /* We don't flow control */
 	elm->txleft = 0; /* Clear TTY TX buffer */
@@ -1067,11 +1124,12 @@ static int elmcan_ldisc_open(struct tty_struct *tty)
 
 	/* Configure CAN metadata */
 	elm->can.state = CAN_STATE_STOPPED;
-	elm->can.clock.freq = 1000000;
-	elm->can.bittiming_const = &elmcan_bittiming_const;
+	elm->can.bitrate_const = elmcan_bitrate_const;
+	elm->can.bitrate_const_cnt = ARRAY_SIZE(elmcan_bitrate_const);
+	elm->can.do_set_bittiming = elmcan_do_set_bittiming;
 	elm->can.ctrlmode_supported = CAN_CTRLMODE_LISTENONLY;
 
-	/* Configure netlink interface */
+	/* Configure netdev interface */
 	elm->dev = dev;
 	dev->netdev_ops = &elmcan_netdev_ops;
 
@@ -1083,18 +1141,20 @@ static int elmcan_ldisc_open(struct tty_struct *tty)
 
 	/* Let 'er rip */
 	err = register_candev(elm->dev);
-	if (err) {
-		free_candev(elm->dev);
-		return err;
-	}
+	if (err)
+		goto out_err;
 
 	netdev_info(elm->dev, "elmcan on %s.\n", tty->name);
 
 	return 0;
+
+out_err:
+	kfree(elm->txbuf);
+	free_candev(elm->dev);
+	return err;
 }
 
-/*
- * Close down an elmcan channel.
+/* Close down an elmcan channel.
  * This means flushing out any pending queues, and then returning.
  * This call is serialized against other ldisc functions:
  * Once this is called, no other ldisc function of ours is entered.
@@ -1103,15 +1163,12 @@ static int elmcan_ldisc_open(struct tty_struct *tty)
  */
 static void elmcan_ldisc_close(struct tty_struct *tty)
 {
-	/* Use get_elm() to synchronize against other users */
 	struct elmcan *elm = get_elm(tty);
 
 	if (!elm)
 		return;
 
-	/* Tear down network side.
-	 * unregister_netdev() calls .ndo_stop() so we don't have to.
-	 */
+	/* unregister_netdev() calls .ndo_stop() so we don't have to. */
 	unregister_candev(elm->dev);
 
 	/* Decrease the refcount twice, once for our own get_elm(),
@@ -1121,13 +1178,12 @@ static void elmcan_ldisc_close(struct tty_struct *tty)
 	put_elm(elm);
 	put_elm(elm);
 
-	/* Spin until refcount reaches 0 */
-	while(atomic_read(&elm->refcount) > 0)
-		msleep(1);
+	while (atomic_read(&elm->refcount) > 0)
+		msleep_interruptible(10);
 
-	/* At this point, all ldisc calls to us will be no-ops.
-	 * Since the refcount is 0, they are bailing immediately.
-	 */
+	/* At this point, all ldisc calls to us have become no-ops. */
+
+	flush_work(&elm->tx_work);
 
 	/* Mark channel as dead */
 	spin_lock_bh(&elm->lock);
@@ -1135,24 +1191,29 @@ static void elmcan_ldisc_close(struct tty_struct *tty)
 	elm->tty = NULL;
 	spin_unlock_bh(&elm->lock);
 
-	/* Flush TTY side */
-	flush_work(&elm->tx_work);
-
 	netdev_info(elm->dev, "elmcan off %s.\n", tty->name);
 
-	/* Free our memory */
+	kfree(elm->txbuf);
 	free_candev(elm->dev);
 }
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5,16,0)
 static int elmcan_ldisc_hangup(struct tty_struct *tty)
+#else
+static void elmcan_ldisc_hangup(struct tty_struct *tty)
+#endif
 {
 	elmcan_ldisc_close(tty);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5,16,0)
 	return 0;
+#endif
 }
 
-/* Perform I/O control on an active elmcan channel. */
-static int elmcan_ldisc_ioctl(struct tty_struct *tty, struct file *file,
-			unsigned int cmd, unsigned long arg)
+static int elmcan_ldisc_ioctl(struct tty_struct *tty,
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5,17,0)
+			      struct file *file,
+#endif
+			      unsigned int cmd, unsigned long arg)
 {
 	struct elmcan *elm = get_elm(tty);
 	unsigned int tmp;
@@ -1162,7 +1223,7 @@ static int elmcan_ldisc_ioctl(struct tty_struct *tty, struct file *file,
 
 	switch (cmd) {
 	case SIOCGIFNAME:
-		tmp = strlen(elm->dev->name) + 1;
+		tmp = strnlen(elm->dev->name, IFNAMSIZ - 1) + 1;
 		if (copy_to_user((void __user *)arg, elm->dev->name, tmp)) {
 			put_elm(elm);
 			return -EFAULT;
@@ -1177,14 +1238,18 @@ static int elmcan_ldisc_ioctl(struct tty_struct *tty, struct file *file,
 
 	default:
 		put_elm(elm);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5,16,0)
 		return tty_mode_ioctl(tty, file, cmd, arg);
+#else
+		return tty_mode_ioctl(tty, cmd, arg);
+#endif
 	}
 }
 
 static struct tty_ldisc_ops elmcan_ldisc = {
 	.owner		= THIS_MODULE,
-	.magic		= TTY_LDISC_MAGIC,
 	.name		= "elmcan",
+	.num		= N_DEVELOPMENT,
 	.receive_buf	= elmcan_ldisc_rx,
 	.write_wakeup	= elmcan_ldisc_tx_wakeup,
 	.open		= elmcan_ldisc_open,
@@ -1193,26 +1258,18 @@ static struct tty_ldisc_ops elmcan_ldisc = {
 	.ioctl		= elmcan_ldisc_ioctl,
 };
 
-
-
-
-
- /************************************************************************
-  *		Module init/exit				*
-  ************************************************************************/
-
 static int __init elmcan_init(void)
 {
 	int status;
 
-	pr_info("ELM327 based best-effort CAN interface driver\n");
-	pr_info("This device is severely limited as a CAN interface, see documentation.\n");
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5,14,0)
+	status = tty_register_ldisc(N_DEVELOPMENT, &elmcan_ldisc);
+#else
+	status = tty_register_ldisc(&elmcan_ldisc);
+#endif
+	if (status)
+		pr_err("Can't register line discipline\n");
 
-	/* Fill in our line protocol discipline, and register it */
-	status = tty_register_ldisc(N_ELMCAN, &elmcan_ldisc);
-	if (status) {
-		pr_err("can't register line discipline\n");
-	}
 	return status;
 }
 
@@ -1221,12 +1278,16 @@ static void __exit elmcan_exit(void)
 	/* This will only be called when all channels have been closed by
 	 * userspace - tty_ldisc.c takes care of the module's refcount.
 	 */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5,14,0)
 	int status;
 
-	status = tty_unregister_ldisc(N_ELMCAN);
-	if (status) {
-		pr_err("Can't unregister line discipline (error: %d)\n", status);
-	}
+	status = tty_unregister_ldisc(N_DEVELOPMENT);
+	if (status)
+		pr_err("Can't unregister line discipline (error: %d)\n",
+		       status);
+#else
+	tty_unregister_ldisc(&elmcan_ldisc);
+#endif
 }
 
 module_init(elmcan_init);