1 files changed, 130 insertions, 0 deletions
diff --git a/firmware/drivers/cli.c b/firmware/drivers/cli.c
new file mode 100644
index 0000000..33c350e
--- /dev/null
+++ b/firmware/drivers/cli.c
@@ -0,0 +1,130 @@
+/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
+/***************************************************************************
+ *            cli.c
+ *
+ *  Thu Jun  6 20:20:20 CEST 2013
+ *  Copyright 2013 Bent Bisballe Nyeng
+ *  deva@aasimon.org
+ ****************************************************************************/
+
+/*
+ *  This file is part of Pedal2Metal.
+ *
+ *  Pedal2Metal is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  Pedal2Metal is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with Pedal2Metal; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
+ */
+#include "cli.h"
+
+#include <stdarg.h>
+#include <string.h>
+
+#include "uart.h"
+
+void cli_init()
+{
+  UARTInit(0, 115200);	/* baud rate setting */
+}
+
+static int cli_strlen(char *p0)
+{
+  char *p = p0;
+  while(*p) p++;
+  return p0 - p;
+}
+
+static inline void cli_write_int(int i)
+{
+	if(i < 0) {
+		UARTSend(0, "-", 1);
+		i *= -1;
+	}
+
+	char buf[] = "\0\0";
+	int pos = 1000000000;
+	int tvaersum = 0;
+	while(pos) {
+		int res = i / pos;
+		i -= res * pos;
+
+		tvaersum += res;
+		if(tvaersum || pos == 1) {
+			buf[0] = res + '0';
+			UARTSend(0, buf, 1);
+		}
+		pos /= 10;
+	}
+}
+
+static inline char *cli_to_hex(char i)
+{
+	static const char hex[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' };
+	static char buf[] = { 'f', 'f', 0 };
+	buf[0] = hex[i / 16];
+	buf[1] = hex[i % 16];
+	return buf;
+}
+
+static int cli_write_format(const char *fmt, va_list ap)
+{
+	char buf[] = "\0\0";
+
+	const char *p = fmt;
+	while(*p) {
+		if(*p != '%') {
+			buf[0] = *p;
+
+      UARTSend(0, buf, 1);
+		} 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 == 'd') cli_write_int(va_arg(ap, int));
+			if(*p == 'c') {
+				buf[0] = va_arg(ap, int);
+        UARTSend(0, 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);
+					c++; sz--;
+				}
+			}
+			if(*p == 's') {
+				char *str = va_arg(ap, char *);
+        UARTSend(0, str, cli_strlen(str));
+			}
+		}
+		p++;
+	}
+
+	// If last character wasn't a newline, print one.
+	if(p > fmt && *(p - 1) != '\n') UARTSend(0, "\n\r", 2);
+
+	return 0;
+}
+
+int cli_write(const char *fmt, ...)
+{
+  va_list ap;
+	va_start(ap, fmt);
+	cli_write_format(fmt, ap);
+	va_end(ap);
+
+	return 0;
+}