From 1408921a24dcd8e8ecf03e5da93e106e26bef55e Mon Sep 17 00:00:00 2001
From: David Lechner <david@lechnology.com>
Date: Sat, 7 Mar 2020 12:34:53 -0600
Subject: [PATCH] sensors/ev3_uart_sensor_ld: improve resync of data messages

It is common to get UART hardware buffer overruns and miss data after
switching to the higher baud rate for receiving data. This adds some
extra checks to try to get back into sync after this happens without
having to wait for the sensor to timeout and go through the connection
sequence again. It doesn't completely fix the problem but is an
improvement over the current situation.
---
 sensors/ev3_uart_sensor_ld.c | 22 +++++++++++++++++++++-
 1 file changed, 21 insertions(+), 1 deletion(-)

diff --git a/sensors/ev3_uart_sensor_ld.c b/sensors/ev3_uart_sensor_ld.c
index 4ba87cac..0988f489 100644
--- a/sensors/ev3_uart_sensor_ld.c
+++ b/sensors/ev3_uart_sensor_ld.c
@@ -571,6 +571,7 @@ static int ev3_uart_receive_buf2(struct tty_struct *tty,
 
 	while (pos < count) {
 		if (port->partial_msg_size) {
+			msg_type = port->msg[0] & EV3_UART_MSG_TYPE_MASK;
 			msg_size = ev3_uart_msg_size(port->msg[0]);
 		} else if (cp[pos] == 0xFF) {
 			/*
@@ -580,9 +581,29 @@ static int ev3_uart_receive_buf2(struct tty_struct *tty,
 			pos++;
 			continue;
 		} else {
+			msg_type = cp[pos] & EV3_UART_MSG_TYPE_MASK;
 			msg_size = ev3_uart_msg_size(cp[pos]);
 		}
 
+		if (port->info_done && (msg_type != EV3_UART_MSG_TYPE_DATA ||
+					msg_size < 3 ||
+					msg_size > EV3_UART_MAX_MESSAGE_SIZE)) {
+			/**
+			 * If we are receiving data, sometimes there can
+			 * be a hardware buffer overflow and we get out
+			 * of sync. So keep trying to get back in sync.
+			 */
+			if (port->partial_msg_size) {
+				port->partial_msg_size--;
+				for (i = 0; i < port->partial_msg_size; i++) {
+					port->msg[i] = port->msg[i + 1];
+				}
+			} else {
+				pos++;
+			}
+			continue;
+		}
+
 		if (msg_size > EV3_UART_MAX_MESSAGE_SIZE) {
 			debug_pr("header: 0x%02x\n", cp[pos]);
 			port->last_err = "Bad message size";
@@ -615,7 +636,6 @@ static int ev3_uart_receive_buf2(struct tty_struct *tty,
 			pos += msg_size;
 		}
 
-		msg_type = message[0] & EV3_UART_MSG_TYPE_MASK;
 		cmd = message[0] & EV3_UART_MSG_CMD_MASK;
 		mode = cmd;
 		cmd2 = message[1];