1 files changed, 71 insertions, 15 deletions

diff --git a/firmware/drivers/cli.c b/firmware/drivers/cli.c
index 33c350e..db8695f 100644
--- a/firmware/drivers/cli.c
+++ b/firmware/drivers/cli.c
@@ -33,20 +33,32 @@
 
 void cli_init()
 {
-  UARTInit(0, 115200);	/* baud rate setting */
+  uart_init(0, 115200);
 }
 
+void cli_write(const char *buf, unsigned int size)
+{
+  uart_send(0, buf, size);
+}
+
+int cli_read(char *buf, unsigned int size)
+{
+  uart_receive(0, buf, size);
+  return 0;
+}
+
+/*
 static int cli_strlen(char *p0)
 {
   char *p = p0;
   while(*p) p++;
-  return p0 - p;
+  return p - p0;
 }
-
+*/
 static inline void cli_write_int(int i)
 {
 	if(i < 0) {
-		UARTSend(0, "-", 1);
+		uart_send(0, "-", 1);
 		i *= -1;
 	}
 
@@ -60,7 +72,7 @@ static inline void cli_write_int(int i)
 		tvaersum += res;
 		if(tvaersum || pos == 1) {
 			buf[0] = res + '0';
-			UARTSend(0, buf, 1);
+			cli_write(buf, 1);
 		}
 		pos /= 10;
 	}
@@ -82,44 +94,46 @@ static int cli_write_format(const char *fmt, va_list ap)
 	const char *p = fmt;
 	while(*p) {
 		if(*p != '%') {
-			buf[0] = *p;
-
-      UARTSend(0, buf, 1);
+      buf[0] = *p;
+      cli_write(buf, 1);
+      if(*p == '\n') cli_write("\r", 1); // also write carrige return.
 		} else {
 			p++;
 			if(!*p) {
 				return 1; // Error: Invalid format string
 			}
-			if(*p == '\n') UARTSend(0, "\n\r", 2);
-			if(*p == '%') UARTSend(0, "%", 1);
+			if(*p == '%') cli_write("%", 1);
 			if(*p == 'd') cli_write_int(va_arg(ap, int));
 			if(*p == 'c') {
 				buf[0] = va_arg(ap, int);
-        UARTSend(0, buf, 1);
+        cli_write(buf, 1);
 			}
 			if(*p == 'x') {
 				char *c = va_arg(ap, char *);
 				size_t sz = va_arg(ap, int);
 				while(sz) {
-          UARTSend(0, cli_to_hex(*c), 2);
+          cli_write(cli_to_hex(*c), 2);
 					c++; sz--;
 				}
 			}
 			if(*p == 's') {
 				char *str = va_arg(ap, char *);
-        UARTSend(0, str, cli_strlen(str));
+        while(*str) {
+          cli_write(str, 1);
+          str++;
+        }
 			}
 		}
 		p++;
 	}
 
 	// If last character wasn't a newline, print one.
-	if(p > fmt && *(p - 1) != '\n') UARTSend(0, "\n\r", 2);
+	//if(p > fmt && *(p - 1) != '\n') cli_write("\n\r", 2);
 
 	return 0;
 }
 
-int cli_write(const char *fmt, ...)
+int cli_printf(const char *fmt, ...)
 {
   va_list ap;
 	va_start(ap, fmt);
@@ -128,3 +142,45 @@ int cli_write(const char *fmt, ...)
 
 	return 0;
 }
+
+int cli_readline(char *cmd, unsigned int size)
+{
+  char buf[2] = { };
+  unsigned int cmdpos = 0;
+  int parsing = 1;
+  while(parsing &&
+        cmdpos < (size - 1)) // Make sure we have room for the terminating zero.
+    {
+    cli_read(buf, 1); // read one byte
+
+    switch(*buf) {
+    case '\n':
+    case '\r':
+      parsing = 0; // break out of parse loop.
+      break;
+
+    case '\b': // backspace
+      if(cmdpos) {
+        cmdpos--;
+        cli_write("\b \b", 3); // erase last written character from echo.
+        continue;
+      }
+      break;
+
+    case 27: // ignore 'ESC' sent on arrow keys.
+      // NOTE: Sequence ESC, '[', 'D' is left arrow.
+      //       Sequence ESC, '[', 'C' is right arrow.
+      continue;
+
+    default:
+      cmd[cmdpos++] = *buf;
+      break;
+    }
+
+    // Echo
+    cli_write(buf, 1); // write one byte
+  }
+  cmd[cmdpos++] = '\0';
+
+  return cmdpos;
+}