[PATCH u-boot-marvell 05/10] tools: kwboot: Fix sending and processing debug message pattern (-d option)

["Pali Rohár" <pali@kernel.org>]

-d option is currently broken. In most cases BootROM does not detect this
message pattern. For sending debug message pattern it is needed to do same
steps as for boot message pattern.

Implement sending debug message pattern via same separate thread like it is
for boot message pattern.

Checking if BootROM entered into UART debug mode is different than
detecting UART boot mode. When in boot mode, BootROM sends xmodem NAK
bytes. When in debug mode, BootROM activates console echo and reply back
every written byte (extept \r\n which is interpreted as executing command
and \b which is interpreting as removing the last sent byte).

So in kwboot, check that BootROM send back at least 4 debug message
patterns as a echo reply for debug message patterns which kwboot is sending
in the loop.

Then there is another observation, if host writes too many bytes (as
command) then BootROM command line buffer may overflow after trying to
execute such long command. To workaround this overflow, it is enough to
remove bytes from the input line buffer by sending 3 \b bytes for every
sent character. So do it.

With this change, it is possbile to enter into the UART debug mode with
kwboot -d option.

Signed-off-by: Pali Rohár <pali@kernel.org>
---
 tools/kwboot.c | 139 +++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 123 insertions(+), 16 deletions(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c
index 9fd90b9bec71..3ab49e74bb67 100644
--- a/tools/kwboot.c
+++ b/tools/kwboot.c
@@ -881,30 +881,139 @@ kwboot_bootmsg(int tty)
 static int
 kwboot_debugmsg(int tty)
 {
-	int rc;
+	unsigned char buf[8192];
+	pthread_t write_thread;
+	int rc, err, i, pos;
+	size_t off;
 
-	kwboot_printv("Sending debug message. Please reboot the target...");
+	/* flush input and output queue */
+	tcflush(tty, TCIOFLUSH);
 
-	do {
-		char buf[16];
+	rc = kwboot_msg_start_thread(&write_thread, &tty, kwboot_msg_debug);
+	if (rc) {
+		perror("Failed to start write thread");
+		return rc;
+	}
 
-		rc = tcflush(tty, TCIOFLUSH);
-		if (rc)
-			break;
+	kwboot_printv("Sending debug message. Please reboot the target...");
+	kwboot_spinner();
 
-		rc = kwboot_tty_send(tty, kwboot_msg_debug, sizeof(kwboot_msg_debug), 0);
-		if (rc)
+	err = 0;
+	off = 0;
+	while (1) {
+		/* Read immediately all bytes in queue without waiting */
+		rc = read(tty, buf + off, sizeof(buf) - off);
+		if ((rc < 0 && errno == EINTR) || rc == 0) {
+			continue;
+		} else if (rc < 0) {
+			err = errno;
 			break;
-
-		rc = kwboot_tty_recv(tty, buf, 16, msg_rsp_timeo);
+		}
+		off += rc - 1;
 
 		kwboot_spinner();
 
-	} while (rc);
+		/*
+		 * Check if we received at least 4 debug message patterns
+		 * (console echo from BootROM) in cyclic buffer
+		 */
+
+		for (pos = 0; pos < sizeof(kwboot_msg_debug); pos++)
+			if (buf[off] == kwboot_msg_debug[(pos + off) % sizeof(kwboot_msg_debug)])
+				break;
+
+		for (i = off; i >= 0; i--)
+			if (buf[i] != kwboot_msg_debug[(pos + i) % sizeof(kwboot_msg_debug)])
+				break;
+
+		off -= i;
+
+		if (off >= 4 * sizeof(kwboot_msg_debug))
+			break;
+
+		/* If not move valid suffix from end of the buffer to the beginning of buffer */
+		memmove(buf, buf + i + 1, off);
+	}
 
 	kwboot_printv("\n");
 
-	return rc;
+	rc = kwboot_msg_stop_thread(write_thread);
+	if (rc) {
+		perror("Failed to stop write thread");
+		return rc;
+	}
+
+	if (err) {
+		errno = err;
+		perror("Failed to read response for debug message pattern");
+		return -1;
+	}
+
+	/* flush output queue with remaining debug message patterns */
+	rc = tcflush(tty, TCOFLUSH);
+	if (rc) {
+		perror("Failed to flush output queue");
+		return rc;
+	}
+
+	kwboot_printv("Clearing input buffer...\n");
+
+	/*
+	 * Wait until BootROM transmit all remaining echo characters.
+	 * Experimentally it was measured that for Armada 385 BootROM
+	 * it is required to wait at least 0.415s. So wait 0.5s.
+	 */
+	usleep(500 * 1000);
+
+	/*
+	 * In off variable is stored number of characters received after the
+	 * successful detection of echo reply. So these characters are console
+	 * echo for other following debug message patterns. BootROM may have in
+	 * its output queue other echo characters which were being transmitting
+	 * before above sleep call. So read remaining number of echo characters
+	 * sent by the BootROM now.
+	 */
+	while ((rc = kwboot_tty_recv(tty, &buf[0], 1, 0)) == 0)
+		off++;
+	if (errno != ETIMEDOUT) {
+		perror("Failed to read response");
+		return rc;
+	}
+
+	/*
+	 * Clear every echo character set by the BootROM by backspace byte.
+	 * This is required prior writing any command to the BootROM debug
+	 * because BootROM command line buffer has limited size. If length
+	 * of the command is larger than buffer size then it looks like
+	 * that Armada 385 BootROM crashes after sending ENTER. So erase it.
+	 * Experimentally it was measured that for Armada 385 BootROM it is
+	 * required to send at least 3 backspace bytes for one echo character.
+	 * This is unknown why. But lets do it.
+	 */
+	off *= 3;
+	memset(buf, '\x08', sizeof(buf));
+	while (off > sizeof(buf)) {
+		rc = kwboot_tty_send(tty, buf, sizeof(buf), 1);
+		if (rc) {
+			perror("Failed to send clear sequence");
+			return rc;
+		}
+		off -= sizeof(buf);
+	}
+	rc = kwboot_tty_send(tty, buf, off, 0);
+	if (rc) {
+		perror("Failed to send clear sequence");
+		return rc;
+	}
+
+	usleep(msg_rsp_timeo * 1000);
+	rc = tcflush(tty, TCIFLUSH);
+	if (rc) {
+		perror("Failed to flush input queue");
+		return rc;
+	}
+
+	return 0;
 }
 
 static size_t
@@ -1951,10 +2060,8 @@ main(int argc, char **argv)
 
 	if (debugmsg) {
 		rc = kwboot_debugmsg(tty);
-		if (rc) {
-			perror("debugmsg");
+		if (rc)
 			goto out;
-		}
 	} else if (bootmsg) {
 		rc = kwboot_bootmsg(tty);
 		if (rc)
-- 
2.20.1