From 724fdb18e7cb0952fdefde7d66f19e4add9b4b76 Mon Sep 17 00:00:00 2001
From: Romain Goyet <romain.goyet@numworks.com>
Date: Tue, 14 Feb 2017 10:57:30 +0100
Subject: [PATCH] [ion] Implement Log::print using SWO on the device

Change-Id: I17e024535cc0f0daf74953c7221e2b98959e6c47
---
 gdb_script.gdb                 |  2 ++
 ion/src/device/Makefile        |  1 +
 ion/src/device/log.cpp         | 15 +++++++++++++++
 ion/src/device/regs/itm.h      | 32 ++++++++++++++++++++++++++++++++
 ion/src/device/regs/register.h |  1 +
 ion/src/device/regs/regs.h     |  1 +
 6 files changed, 52 insertions(+)
 create mode 100644 ion/src/device/log.cpp
 create mode 100644 ion/src/device/regs/itm.h

diff --git a/gdb_script.gdb b/gdb_script.gdb
index 4d41c4a9b..b5640762a 100644
--- a/gdb_script.gdb
+++ b/gdb_script.gdb
@@ -8,6 +8,8 @@ set pagination off
 load
 
 # Tell OpenOCD to reset and halt
+monitor itm ports on
+monitor tpiu config internal swo.log.bin uart off 16000000
 monitor reset halt
 
 break init
diff --git a/ion/src/device/Makefile b/ion/src/device/Makefile
index d6b5df1b8..c2ed1e605 100644
--- a/ion/src/device/Makefile
+++ b/ion/src/device/Makefile
@@ -7,6 +7,7 @@ objs += $(addprefix ion/src/device/, \
   display.o\
   keyboard.o\
   led.o\
+  log.o\
   power.o\
   sd_card.o\
 )
diff --git a/ion/src/device/log.cpp b/ion/src/device/log.cpp
new file mode 100644
index 000000000..2b6a25a99
--- /dev/null
+++ b/ion/src/device/log.cpp
@@ -0,0 +1,15 @@
+#include <ion/log.h>
+#include "regs/itm.h"
+
+// We're printing using SWO.
+// This is achieved by writing to the ITM register, which is sent through the
+// Cortex Debug bus
+
+void Ion::Log::print(const char * message) {
+  char character = 0;
+  while ((character = *message++) != 0) {
+    if (ITM.TER()->get(0)) {
+      ITM.STIM(0)->set(character);
+    }
+  }
+}
diff --git a/ion/src/device/regs/itm.h b/ion/src/device/regs/itm.h
new file mode 100644
index 000000000..196737ca3
--- /dev/null
+++ b/ion/src/device/regs/itm.h
@@ -0,0 +1,32 @@
+#ifndef REGS_ITM_H
+#define REGS_ITM_H
+
+#include "register.h"
+
+// See ARM Cortex M4 TRM
+
+class ITM {
+public:
+  class STIM : public Register8 {
+  };
+
+  // http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0314h/Chdbicac.html
+  class TER : Register32 {
+  public:
+    bool get(int index) volatile { return (bool)getBitRange(index, index); }
+  };
+
+  constexpr ITM() {};
+  volatile STIM * STIM(int i) const {
+    return (class STIM *)(Base() + 4*i);
+  };
+  REGS_REGISTER_AT(TER, 0xE00);
+private:
+  constexpr uint32_t Base() const {
+    return 0xE0000000;
+  }
+};
+
+constexpr ITM ITM;
+
+#endif
diff --git a/ion/src/device/regs/register.h b/ion/src/device/regs/register.h
index 45134acb4..87056ed7a 100644
--- a/ion/src/device/regs/register.h
+++ b/ion/src/device/regs/register.h
@@ -37,6 +37,7 @@ private:
   T m_value;
 };
 
+typedef Register<uint8_t> Register8;
 typedef Register<uint16_t> Register16;
 typedef Register<uint32_t> Register32;
 typedef Register<uint64_t> Register64;
diff --git a/ion/src/device/regs/regs.h b/ion/src/device/regs/regs.h
index 712fd56be..4b96a63a3 100644
--- a/ion/src/device/regs/regs.h
+++ b/ion/src/device/regs/regs.h
@@ -8,6 +8,7 @@
 #include "flash.h"
 #include "fsmc.h"
 #include "gpio.h"
+#include "itm.h"
 #include "pwr.h"
 #include "rcc.h"
 #include "sdio.h"