From 239af986dfe06be98af2c26d1b59317da5f5e589 Mon Sep 17 00:00:00 2001
From: Johny Mattsson <jmattsson@dius.com.au>
Date: Tue, 31 May 2016 18:46:59 +1000
Subject: [PATCH] Debug functions for tracking RTOS task usage.

Since configUSE_TRACE_FACILITY is not enabled :(
---
 app/include/rtos_dbg.h | 12 ++++++++++++
 app/user/rtos_dbg.c    | 35 +++++++++++++++++++++++++++++++++++
 2 files changed, 47 insertions(+)
 create mode 100644 app/include/rtos_dbg.h
 create mode 100644 app/user/rtos_dbg.c

diff --git a/app/include/rtos_dbg.h b/app/include/rtos_dbg.h
new file mode 100644
index 00000000..71130349
--- /dev/null
+++ b/app/include/rtos_dbg.h
@@ -0,0 +1,12 @@
+#ifndef _RTOS_DBG_H_
+#define _RTOS_DBG_H_
+
+#include <stdint.h>
+
+#define rtos_dbg_here() do{rtos_dbg_task_print(__FILE__,__LINE__);} while(0)
+#define rtos_dbg_stack_here() do{rtos_dbg_stack_print(__FILE__,__LINE__);} while(0)
+
+void rtos_dbg_task_print (const char *file, uint32_t line);
+void rtos_dbg_stack_print (const char *file, uint32_t line);
+
+#endif
diff --git a/app/user/rtos_dbg.c b/app/user/rtos_dbg.c
new file mode 100644
index 00000000..4ca8f666
--- /dev/null
+++ b/app/user/rtos_dbg.c
@@ -0,0 +1,35 @@
+#include "rtos_dbg.h"
+#include <freertos/FreeRTOSConfig.h>
+#include <stdio.h>
+
+extern struct {
+  uint32_t *pxTopOfStack;
+  uint32_t x[5];
+  uint32_t y[5];
+  unsigned uxPriority;
+  uint32_t *pxStack;
+  signed char pcTaskName[configMAX_TASK_NAME_LEN];
+} *pxCurrentTCB;
+
+void rtos_dbg_task_print (const char *file, uint32_t line)
+{
+  printf(">>dbg: %s:%d in RTOS task \"%s\": prio %d\n",
+    file,
+    line,
+    pxCurrentTCB->pcTaskName,
+    pxCurrentTCB->uxPriority
+  );
+}
+
+
+void rtos_dbg_stack_print (const char *file, uint32_t line)
+{
+  uint32_t fill = 0xa5a5a5a5u;
+  uint32_t nwords = 0;
+  uint32_t *p = (uint32_t*)pxCurrentTCB->pxStack;
+  for (;p < pxCurrentTCB->pxTopOfStack && *p == fill; ++p)
+    ++nwords; 
+
+  printf(">>dbg: %s:%d in RTOS task \"%s\": %u stack untouched\n",
+    file, line, pxCurrentTCB->pcTaskName, nwords * 4);
+}