From 02ad780abc8d2dc7bdad73db372f02a31d6ade5c Mon Sep 17 00:00:00 2001
From: Bjorn Andersson <bjorn.andersson@linaro.org>
Date: Wed, 24 Jan 2018 15:04:39 -0800
Subject: [PATCH] peripheral: Split data receive path in hdlc and raw

Split the two code paths related to handling of HDLC encoded vs raw
packets coming from the remote processor, this in order to facilitate
further rework of the HDLC decoder.

Signed-off-by: Bjorn Andersson <bjorn.andersson@linaro.org>
---
 peripheral.c | 61 ++++++++++++++++++++++++++++++++++------------------
 1 file changed, 40 insertions(+), 21 deletions(-)

diff --git a/peripheral.c b/peripheral.c
index 7547f1e..61d1a9e 100644
--- a/peripheral.c
+++ b/peripheral.c
@@ -83,9 +83,8 @@ static int diag_cmd_recv(int fd, void *data)
 	return 0;
 }
 
-static int diag_data_recv(int fd, void *data)
+static int diag_data_recv_hdlc(int fd, struct peripheral *peripheral)
 {
-	struct peripheral *peripheral = data;
 	uint8_t buf[4096];
 	size_t msglen;
 	size_t len;
@@ -95,34 +94,54 @@ static int diag_data_recv(int fd, void *data)
 
 	for (;;) {
 		n = read(fd, buf, sizeof(buf));
-		if (n < 0) {
-			if (errno != EAGAIN) {
-				warn("failed to read from data channel");
-				peripheral_close(peripheral);
-			}
-
-			break;
-		}
+		if (n < 0)
+			return -errno;
 
 		ptr = buf;
 		len = n;
 		for (;;) {
-			if (peripheral->features & DIAG_FEATURE_APPS_HDLC_ENCODE) {
-				msg = ptr;
-				msglen = len;
-			} else {
-				msg = hdlc_decode_one(&ptr, &len, &msglen);
-				if (!msg)
-					break;
-			}
+			msg = hdlc_decode_one(&ptr, &len, &msglen);
+			if (!msg)
+				break;
 
 			diag_forward_response(msg, msglen);
-
-			if (peripheral->features & DIAG_FEATURE_APPS_HDLC_ENCODE)
-				break;
 		}
 	}
 
+	/* Not reached */
+}
+
+static int diag_data_recv_raw(int fd, struct peripheral *peripheral)
+{
+	uint8_t buf[4096];
+	ssize_t n;
+
+	for (;;) {
+		n = read(fd, buf, sizeof(buf));
+		if (n < 0)
+			return -errno;
+
+		diag_forward_response(buf, n);
+	}
+
+	/* Not reached */
+}
+
+static int diag_data_recv(int fd, void *data)
+{
+	struct peripheral *peripheral = data;
+	ssize_t n;
+
+	if (peripheral->features & DIAG_FEATURE_APPS_HDLC_ENCODE)
+		n = diag_data_recv_raw(fd, peripheral);
+	else
+		n = diag_data_recv_hdlc(fd, peripheral);
+
+	if (n < 0 && n != -EAGAIN) {
+		warn("failed to read from data channel");
+		peripheral_close(peripheral);
+	}
+
 	return 0;
 }