Adding debugging system for the os layer.

12 files changed, 155 insertions, 1 deletions

diff --git a/os/Makefile b/os/Makefile
index 42a553c..453c8d1 100644
--- a/os/Makefile
+++ b/os/Makefile
@@ -7,7 +7,7 @@ include config.mk
 include $(ROOTDIR)/FreeRTOS/config.mk
 include $(ROOTDIR)/arch/config.mk
 
-TARGET_SRCS = src/init.c src/sbrk.c src/sprintf.c src/malloc.c src/free.c src/fclose.c src/read.c src/lseek.c src/write.c src/close.c
+TARGET_SRCS = src/init.c src/sbrk.c src/sprintf.c src/malloc.c src/free.c src/fclose.c src/read.c src/lseek.c src/write.c src/close.c src/osdebug.c
 
 include $(ROOTDIR)/target-rules.mk
 
diff --git a/os/include/osdebug.h b/os/include/osdebug.h
new file mode 100644
index 0000000..8147cdb
--- /dev/null
+++ b/os/include/osdebug.h
@@ -0,0 +1,14 @@
+#ifndef __OSDEBUG_H__
+#define __OSDEBUG_H__
+
+#include <BoardConsole.h>
+
+void osDbgPrintf(const char * fmt, ...);
+
+#ifdef FULLDEBUG
+#define DBGOUT osDbgPrintf
+#else
+#define DBGOUT(...)
+#endif
+
+#endif
diff --git a/os/src/close.c b/os/src/close.c
index ae0716f..aa4002b 100644
--- a/os/src/close.c
+++ b/os/src/close.c
@@ -1,5 +1,7 @@
 #include <reent.h>
+#include <osdebug.h>
 
 int _close_r(struct _reent * reent, int fd) {
+    DBGOUT("_close_r(%p, %d)\r\n", reent, fd);
     return 0;
 }
diff --git a/os/src/fclose.c b/os/src/fclose.c
index fe81f62..4721fde 100644
--- a/os/src/fclose.c
+++ b/os/src/fclose.c
@@ -1,6 +1,8 @@
 #include <stdio.h>
 #include <reent.h>
+#include <osdebug.h>
 
 int fclose(FILE * fp) {
+    DBGOUT("fclose(%p)\r\n", fp);
     return _fclose_r(_impure_ptr, fp);
 }
diff --git a/os/src/free.c b/os/src/free.c
index d80aff2..0091d56 100644
--- a/os/src/free.c
+++ b/os/src/free.c
@@ -1,6 +1,8 @@
 #include <reent.h>
 #include <malloc.h>
+#include <osdebug.h>
 
 void free(void * ptr) {
+    DBGOUT("free(%p)\r\n", ptr);
     _free_r(_impure_ptr, ptr);
 }
diff --git a/os/src/lseek.c b/os/src/lseek.c
index f132693..fdd0b94 100644
--- a/os/src/lseek.c
+++ b/os/src/lseek.c
@@ -1,5 +1,7 @@
 #include <reent.h>
+#include <osdebug.h>
 
 _off_t _lseek_r(struct _reent * reent, int fd, _off_t seek, int wheel) {
+    DBGOUT("_lseek_r(%p, %d, %d, %d)\r\n", reent, fd, (int) seek, wheel);
     return 0;
 }
diff --git a/os/src/malloc.c b/os/src/malloc.c
index 539d976..0906cc0 100644
--- a/os/src/malloc.c
+++ b/os/src/malloc.c
@@ -1,6 +1,8 @@
 #include <reent.h>
 #include <malloc.h>
+#include <osdebug.h>
 
 void * malloc(size_t size) {
+    DBGOUT("malloc(%u)\r\n", size);
     return _malloc_r(_impure_ptr, size);
 }
diff --git a/os/src/osdebug.c b/os/src/osdebug.c
new file mode 100644
index 0000000..15aec5b
--- /dev/null
+++ b/os/src/osdebug.c
@@ -0,0 +1,120 @@
+#include <stdarg.h>
+#include <string.h>
+#include <stdint.h>
+#include <BoardConsole.h>
+#include <osdebug.h>
+
+static const char hex_conv[] = "0123456789ABCDEF";
+static void dbgput(const void * _str, int n) {
+    const char * str = (const char *) str;
+    while(n--)
+        BoardConsolePutc(*(str++));
+}
+
+void osDbgPrintf(const char * str, ...) {
+    if (!str)
+        return;
+
+    va_list ap;
+    const char * percent;
+    const char * ptr = str;
+    int entry_size;
+    char tmp_n_conv[33], * tmp_conv_ptr;
+
+    int seen_something;
+
+    int arg_c;
+    char * arg_s;
+    unsigned long arg_u = 0;
+    long arg_i;
+    uintptr_t arg_p;
+    int str_size, i;
+
+    va_start(ap, str);
+
+    while ((percent = strchr(ptr, '%'))) {
+        dbgput(ptr, percent - ptr);
+        ptr = percent + 1;
+        entry_size = 0;
+        while ((*ptr >= '0') && (*ptr <= '9')) {
+            entry_size *= 10;
+            entry_size += *ptr - '0';
+            ptr++;
+        }
+        switch (*ptr) {
+        case '%':
+            dbgput("%", 1);
+            break;
+        case 'c':
+            arg_c = va_arg(ap, int);
+            dbgput(&arg_c, 1);
+            break;
+        case 's':
+            arg_s = va_arg(ap, char *);
+            if (arg_s) {
+                for (str_size = strlen(arg_s); str_size < entry_size; str_size++)
+                    dbgput(" ", 1);
+                dbgput(arg_s, strlen(arg_s));
+            } else {
+                for (str_size = 0; str_size < entry_size; str_size++)
+                    dbgput(" ", 1);
+            }
+            break;
+        case 'i':
+        case 'd':
+            arg_i = va_arg(ap, long);
+            if (arg_i < 0) {
+                dbgput("-", 1);
+                arg_u = -arg_i;
+            } else {
+                arg_u = arg_i;
+            }
+        case 'u':
+            if (*ptr == 'u')
+                arg_u = va_arg(ap, unsigned long);
+            tmp_conv_ptr = tmp_n_conv + 32;
+            *tmp_conv_ptr = 0;
+            do {
+                *--tmp_conv_ptr = hex_conv[arg_u % 10];
+                arg_u /= 10;
+            } while (arg_u);
+            dbgput(tmp_conv_ptr, strlen(tmp_conv_ptr));
+            break;
+        case 'p':
+            arg_p = va_arg(ap, uintptr_t);
+            if (arg_p) {
+                dbgput("0x", 2);
+                for (i = sizeof(arg_p) * 2 - 1; i >= 0; i--) {
+                    dbgput(&hex_conv[(arg_p >> (i << 2)) & 15], 1);
+                }
+            } else {
+                dbgput("(nil)", 4);
+            }
+            break;
+        case 'x':
+            arg_u = va_arg(ap, unsigned long);
+            seen_something = 0;
+            for (i = sizeof(arg_p) * 2 - 1; i >= 0; i--) {
+                if (!seen_something && ((arg_u >> (i << 2)) == 0))
+                    continue;
+                dbgput(&hex_conv[(arg_u >> (i << 2)) & 15], 1);
+                seen_something = 1;
+            }
+            if (!seen_something)
+                dbgput("0", 1);
+            break;
+        case 0:  // malformed format string with a trailing %.
+            dbgput("%", 1);
+            return;
+        default:
+            dbgput("<unsupported format: %", 22);
+            dbgput(ptr, 1);
+            dbgput(">", 1);
+            break;
+        }
+        ptr++;
+    }
+
+    if (*ptr)
+      dbgput(ptr, strlen(ptr));
+}
diff --git a/os/src/read.c b/os/src/read.c
index 08351a7..4850c9c 100644
--- a/os/src/read.c
+++ b/os/src/read.c
@@ -1,5 +1,7 @@
 #include <reent.h>
+#include <osdebug.h>
 
 _ssize_t _read_r(struct _reent * reent, int fd, void * ptr, size_t size) {
+    DBGOUT("_read_r(%p, %d, %p, %u)\r\n", reent, fd, ptr, size);
     return 0;
 }
diff --git a/os/src/sbrk.c b/os/src/sbrk.c
index 4208b2e..d013f4f 100644
--- a/os/src/sbrk.c
+++ b/os/src/sbrk.c
@@ -6,6 +6,8 @@
 #include "task.h"
 #include "mpu_wrappers.h"
 
+#include <osdebug.h>
+
 // Mostly stolen from mbed-freertos
 
 extern uintptr_t __heap_start, __heap_end;
@@ -17,6 +19,8 @@ PRIVILEGED_FUNCTION void *_sbrk_r(struct _reent *ptr, ptrdiff_t incr)
 {
     void *prev_heap_end, *next_heap_end, *ret;
     void *stack_min = (void *)(__stack_start ? __stack_start : __heap_end);
+    
+    DBGOUT("_sbrk_r(%p, %u)\r\n", ptr, incr);
 
     taskENTER_CRITICAL();
     {
diff --git a/os/src/sprintf.c b/os/src/sprintf.c
index 30d27b3..3dc1b2c 100644
--- a/os/src/sprintf.c
+++ b/os/src/sprintf.c
@@ -1,10 +1,12 @@
 #include <reent.h>
 #include <stdio.h>
 #include <stdarg.h>
+#include <osdebug.h>
 
 int sprintf(char * str, const char * fmt, ...) {
     int r;
     va_list ap;
+    DBGOUT("sprintf(%p, %s, ...)\r\n", str, fmt);
     va_start(ap, fmt);
     r = _vsprintf_r(_impure_ptr, str, fmt, ap);
     va_end(ap);
diff --git a/os/src/write.c b/os/src/write.c
index a54195f..50fadc6 100644
--- a/os/src/write.c
+++ b/os/src/write.c
@@ -1,5 +1,7 @@
 #include <reent.h>
+#include <osdebug.h>
 
 _ssize_t _write_r(struct _reent * reent, int fd, const void * buf, size_t size) {
+    DBGOUT("_write_r(%p, %d, %p, %u)\r\n", reent, fd, buf, size);
     return 0;
 }