From c13c769f490a1b40ad06c80934d7c628910a6a22 Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Sat, 29 Aug 2015 13:21:33 +0200
Subject: [PATCH] Support more channels for 16 bit ports

---
 rpp-test-sw/commands/cmd_port.c | 30 +++++++++++++++++++++++-------
 1 file changed, 23 insertions(+), 7 deletions(-)

diff --git a/rpp-test-sw/commands/cmd_port.c b/rpp-test-sw/commands/cmd_port.c
index 2f7afd0..5f404d7 100644
--- a/rpp-test-sw/commands/cmd_port.c
+++ b/rpp-test-sw/commands/cmd_port.c
@@ -30,6 +30,7 @@
 #ifdef TARGET_HAS_SPI
 #include "drv/spi.h"
 #endif
+#include <drv/endian.h>
 #include "cmdproc_utils.h"
 
 #define MIN(a, b) ((a) < (b) ? (a) : (b))
@@ -107,15 +108,30 @@ int cmd_do_port_val(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 			return cmd_opchar_replong(cmd_io, param, value, 0, 16);
 		}
 		case 16: {
-			uint16_t value;
-			ret = sscanf(param[2], "%hi", &value);
-			if (ret == EOF || ret == 0)
-				break;
-			value = cpu_to_be16(value);
-			ret = port->set(port, &value, sizeof(value));
+			uint16_t value[8];
+			int i, n;
+			char *p = param[2];
+			assert(port->numchn <= ARRAY_SIZE(value));
+
+			for (i = 0; i < port->numchn; i++) {
+				ret = sscanf(param[2], "%hi%n", &value[i], &n);
+				if (ret == EOF || ret == 0)
+					break;
+				value[i] = cpu_to_be16(value[i]);
+				p += n;
+			}
+
+			ret = port->set(port, &value, port->numchn * 2);
 			if (ret == FAILURE)
 				return -CMDERR_WRPERM;
-			return cmd_opchar_replong(cmd_io, param, be16_to_cpu(value), 0, 16);
+			if (port->numchn == 1)
+				return cmd_opchar_replong(cmd_io, param, be16_to_cpu(value[0]), 0, 16);
+			else {
+				for (i = 0; i < port->numchn; i++)
+					rpp_sci_printf("%#x ", be16_to_cpu(value[i]));
+				rpp_sci_printf("\n");
+				return 0;
+			}
 		}
 		case 24: {
 			uint32_t value;
-- 
2.39.2